Commit 97e04e29 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Laplace implementation, rangeAndBearing & infrared begin of implementation,...

Laplace implementation, rangeAndBearing & infrared begin of implementation, testbech lapalce to correct
parent 2eae3db4
No preview for this file type
<?xml version="1.0" encoding="UTF-8"?>
<module type="JAVA_MODULE" version="4">
<component name="NewModuleRootManager" inherit-compiler-output="true">
<exclude-output />
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>
\ No newline at end of file
<component name="ProjectCodeStyleConfiguration">
<state>
<option name="PREFERRED_PROJECT_CODE_STYLE" value="Default" />
</state>
</component>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="JavaScriptSettings">
<option name="languageLevel" value="ES6" />
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Project.iml" filepath="$PROJECT_DIR$/.idea/Project.iml" />
</modules>
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ChangeListManager">
<list default="true" id="d9bcd0a7-f75a-4f20-942a-8a93013b7509" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/.idea/Project.iml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/codeStyles/codeStyleConfig.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/misc.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/modules.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/vcs.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/infrared.c" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/infrared.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/laplace.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/measurements.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.c" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.h" afterDir="false" />
<change beforePath="$PROJECT_DIR$/README.md" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/.collision.wbproj" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/.obstacle.wbproj" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/collision.wbt" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/textures/ague.jpg" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/textures/stone.jpg" beforeDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
<option name="LAST_RESOLUTION" value="IGNORE" />
</component>
<component name="CodeStyleSettingsInfer">
<option name="done" value="true" />
</component>
<component name="Git.Settings">
<option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
</component>
<component name="ProjectCodeStyleSettingsMigration">
<option name="version" value="1" />
</component>
<component name="ProjectId" id="1UQv71DgzkdiOxmtUnr0nTA47QM" />
<component name="PropertiesComponent">
<property name="SHARE_PROJECT_CONFIGURATION_FILES" value="true" />
<property name="WebServerToolWindowFactoryState" value="false" />
<property name="aspect.path.notification.shown" value="true" />
<property name="last_opened_file_path" value="$PROJECT_DIR$/controllers/commonSrc" />
</component>
<component name="RecentsManager">
<key name="CopyFile.RECENT_KEYS">
<recent name="$PROJECT_DIR$/controllers/commonSrc" />
</key>
<key name="MoveFile.RECENT_KEYS">
<recent name="$PROJECT_DIR$/controllers/ctrl1" />
<recent name="$PROJECT_DIR$/controllers/commonSrc" />
<recent name="$PROJECT_DIR$/controllers" />
</key>
</component>
<component name="RunDashboard">
<option name="ruleStates">
<list>
<RuleState>
<option name="name" value="ConfigurationTypeDashboardGroupingRule" />
</RuleState>
<RuleState>
<option name="name" value="StatusDashboardGroupingRule" />
</RuleState>
</list>
</option>
</component>
<component name="SvnConfiguration">
<configuration />
</component>
<component name="TaskManager">
<task active="true" id="Default" summary="Default task">
<changelist id="d9bcd0a7-f75a-4f20-942a-8a93013b7509" name="Default Changelist" comment="" />
<created>1575301462899</created>
<option name="number" value="Default" />
<option name="presentableId" value="Default" />
<updated>1575301462899</updated>
<workItem from="1575301466330" duration="7502000" />
</task>
<servers />
</component>
<component name="TypeScriptGeneratedFilesManager">
<option name="version" value="1" />
</component>
<component name="Vcs.Log.Tabs.Properties">
<option name="TAB_STATES">
<map>
<entry key="MAIN">
<value>
<State />
</value>
</entry>
</map>
</option>
</component>
</project>
\ No newline at end of file
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
### Generic Makefile.include for Webots controllers, physics plugins, robot
### window libraries, remote control libraries and other libraries
### to be used with GNU make
###
### Platforms: Windows, macOS, Linux
### Languages: C, C++
###
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
### Edmund Ronald, Sergei Poskriakov
###
###-----------------------------------------------------------------------------
###
### This file is meant to be included from the Makefile files located in the
### Webots projects subdirectories. It is possible to set a number of variables
### to customize the build process, i.e., add source files, compilation flags,
### include paths, libraries, etc. These variables should be set in your local
### Makefile just before including this Makefile.include. This Makefile.include
### should never be modified.
###
### Here is a description of the variables you may set in your local Makefile:
###
### ---- C Sources ----
### if your program uses several C source files:
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
###
### ---- C++ Sources ----
### if your program uses several C++ source files:
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
###
### ---- Compilation options ----
### if special compilation flags are necessary:
### CFLAGS = -Wno-unused-result
###
### ---- Linked libraries ----
### if your program needs additional libraries:
### INCLUDE = -I"/my_library_path/include"
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
###
### ---- Linking options ----
### if special linking flags are needed:
### LFLAGS = -s
###
### ---- Webots included libraries ----
### if you want to use the Webots C API in your C++ controller program:
### USE_C_API = true
### if you want to link with the Qt framework embedded in Webots:
### QT = core gui widgets network
###
### ---- Debug mode ----
### if you want to display the gcc command line for compilation and link, as
### well as the rm command details used for cleaning:
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include
build/release/ctrl1.o: ctrl1.c \
/Applications/Webots.app/include/controller/c/webots/robot.h \
/Applications/Webots.app/include/controller/c/webots/types.h \
/Applications/Webots.app/include/controller/c/webots/nodes.h \
/Applications/Webots.app/include/controller/c/webots/motor.h \
/Applications/Webots.app/include/controller/c/webots/differential_wheels.h \
/Applications/Webots.app/include/controller/c/webots/distance_sensor.h \
/Applications/Webots.app/include/controller/c/webots/emitter.h \
/Applications/Webots.app/include/controller/c/webots/receiver.h
/********************************************************************************/
/* File: ctrl1.c */
/* Version: 1.0 */
/* Date: 1-Dec-19 */
/* Description: Controller for epucks */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "../commonSrc/laplace.h"
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR false
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_NAME "ROBOT"
// ********
#define NB_SENSORS 8 // Number of distance sensors
/* Harware elements declaration */
WbDeviceTag left_motor; //handler for left wheel of the robot
WbDeviceTag right_motor; //handler for the right wheel of the robot
WbDeviceTag ds[NB_SENSORS]; // Handle for the infrared distance sensors
WbDeviceTag receiver; // Handle for the receiver node
WbDeviceTag emitter; // Handle for the emitter node
static void reset(){
}
int main(){
char* robot_name=(char*) wb_robot_get_name();
int msl = 0, msr = 0; // Wheel speeds
float msl_w, msr_w;
int bmsl, bmsr, sum_sensors; // Braitenberg parameters
int i;
int distances[NB_SENSORS]; // Array for the distance sensor readings
int max_sens = 0; // Store highest sensor value
reset(); // Resetting the robot
for(;;){
bmsl = 0; bmsr = 0;
sum_sensors = 0;
max_sens = 0;
Measurement* list = getRelativePositions(ROBOT_NAME);
speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]);
speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]);
// Compute wheels speed from reynold's speed
compute_wheel_speeds(&msl, &msr);
// Adapt speed instinct to distance sensor values
if (sum_sensors > NB_SENSORS*MIN_SENS) {
msl -= msl*max_sens/(2*MAX_SENS);
msr -= msr*max_sens/(2*MAX_SENS);
}
// Add Braitenberg
msl += bmsl;
msr += bmsr;
/*Webots 2018b*/
// Set speed
msl_w = msl*MAX_SPEED_WEB/1000;
msr_w = msr*MAX_SPEED_WEB/1000;
wb_motor_set_velocity(left_motor, msl_w);
wb_motor_set_velocity(right_motor, msr_w);
//wb_differential_wheels_set_speed(msl,msr);
/*Webots 2018b*/
// Continue one step
wb_robot_step(TIME_STEP);
}
}
void initialDirection(){
}
void prepareData(){
for (k=0;k<FLOCK_SIZE;k++) {
if (k != robot_id) { // Loop on flockmates only
// If neighbor k is too close (Euclidean distance)
if (pow(loc[robot_id][0]-loc[k][0],2)+pow(loc[robot_id][1]-loc[k][1],2) < NEIGHBOR_BOUND)
{
}
}
}
}
\ No newline at end of file
/***********************
* Inclusions
*/
#include <webots/distance_sensor.h>
#include "infrared.h"
#include <stdlib.h>
#include <stdio.h>
#define NB_SENSORS 8
WbDeviceTag ds[NB_SENSORS];
void init(){
int i;
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
for(i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
}
void mergeListWithProxValues(Measurement* measurementList){
int distances[NB_SENSORS];
for(i=0;i<NB_SENSORS;i++)
{
distances[i] = wb_distance_sensor_get_value(ds[i]);
}
}
\ No newline at end of file
#ifndef INFRARED
#define INFRARED
#include "measurements.h"
#endif
\ No newline at end of file
/***********************
* Inclusions
*/
#include "laplace.h"
#include <stdlib.h>
#include <stdio.h>
/***********************
* Local constants definition
*/
#define PRINT_MATRIX 0
#define IS_TEST 0
#define ALPHA_FREE 0.5
#define ALPHA_OBST 0.7
/***********************
* Local prototype
*/
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void transpose(float* input, float* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void printMatrix(char* name, float* matrix, int m, int n)
/***********************
* Methods implementation
*/
void transpose(float* input, float* output, int m, int n)
{
for(int i=0; i<m; i++)
for(int j=0; j<n; j++)
{
output[j*m+i] = input[i*n+j];
}
}
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)
{
int i, j, k;
for (i = 0; i < m; i++)
{
for (j = 0; j < p; j++)
{
res[i*p+j] = 0;
for (k = 0; k < n; k++)
res[i*p+j] += mat1[i*n+k]*mat2[k*p+j];
}
}
}
void printMatrix(char* name, float* matrix, int m, int n){
if(!PRINT_MATRIX) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(int i= 0; i<m; i++){
for(int j= 0; j<n; j++){
printf("%f\t", matrix[i*n+j]);
}
printf("\n");
}
}
void laplacian(Measurement* objectsInSurround, int numberOfObjects, float* velTot){
// Compute neighborhood by range and bearing (contains neighbors robots and obstacles)
// X contains relative positions of each neighbors (size = (num_neighbors * 2)
float incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
for(int i = 0; i < (numberOfObjects+1) * numberOfObjects; i++)
incidenceMatrix[i] = 0.;
float weightsMatrix[numberOfObjects*numberOfObjects];
for(int i = 0; i < numberOfObjects * numberOfObjects; i++)
weightsMatrix[i] = 0.;
// Compute incidence matrix of size (num_neighbors+1 x num_neighbors)
for (int j=0; j<numberOfObjects; j++)
{
incidenceMatrix[j] = -1;
incidenceMatrix[(j+1)*numberOfObjects+j] = 1;
}
printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
{
Measurement* nextObject = objectsInSurround;
int j = 0;
while(nextObject){
if (nextObject->robotID != -1)
weightsMatrix[j*numberOfObjects+j] = 1;
else{
weightsMatrix[j*numberOfObjects+j] = -2; // more importance to avoid obstacle
obstacleInSurround = 1;
}
nextObject = nextObject->next;
j++;
}
}
for (int j=0; j<numberOfObjects; j++)
{
if (objectLocation[j].robotID != -1)
weightsMatrix[j*numberOfObjects+j] = 1;
else{
weightsMatrix[j*numberOfObjects+j] = -2; // more importance to avoid obstacle
obstacleInSurround = 1;
}
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
float IW[(numberOfObjects+1) * numberOfObjects];
multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
float L[(numberOfObjects+1) * (numberOfObjects+1)];
multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float vel_graph[2] = {0, 0};
// Compute velocity of current robot with v = -L(x-B)
{
Measurement* nextObject = objectsInSurround;
int i = 0;
while(nextObject){
vel_graph[X] += -L[0] * nextObject->relativePosition[X];
vel_graph[Y] += -L[0] * nextObject->relativePosition[Y];
nextObject = nextObject->next;
i++;
}
}
float vel_migr[2] = {5, 0};
velTot[X] = alpha * vel_graph[X] + (1-alpha) * vel_migr[X]; // tot velocity is a weighted sum of graph and migration velocities
velTot[Y] = alpha * vel_graph[Y] + (1-alpha) * vel_migr[Y];
}
/***********************
* Test mathods - Usable using IS_TEST constant
*/
#ifdef IS_TEST
int main(){
float velTot[2];
Measurement* robotsMeasurements = malloc(sizeof(Measurement));
robotsMeasurements[0] = malloc(sizeof(Measurement));