Commit f20ca6b9 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

PSO integration and debug, needs to be run for a long time and possibly pick...

PSO integration and debug, needs to be run for a long time and possibly pick up and try other relevant parameters (Such as migration speed), an evalutation of the proximitiy of friends in  the fit function local to epuck would be a good thing to implement
parent 4dd5f2d5
......@@ -3,7 +3,8 @@
#include "measurements.h"
void infrared_getSensorsValues(double* intensity);
void infrared_init();
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio);
Measurement* infrared_updateList(Measurement* measurementList, const double* weights, double* frontSpeedRatio);
#endif
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, double* newSpeed, double* weights, double* my_position);
void laplace_compute(Measurement* objectsInSurround, double* newSpeed, const double laplace_divider);
void laplace_init();
#endif
#ifndef __ROBMAIN_H__
#define __ROBMAIN_H__
#include "measurements.h"
void rob_main_reset();
void rob_main_loop(Measurement** list,
double* my_position,
double* my_speed,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
const double laplace_divider,
double* velocityRatio);
#endif
\ No newline at end of file
......@@ -32,7 +32,6 @@ static WbDeviceTag ds[NB_SENSORS];
//double ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
double ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
double ps_weights[8] = {20, 10, 5, 5, 5, 5, 15, 30};
void infrared_init(){
......@@ -50,10 +49,8 @@ void infrared_init(){
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio){
double intensity[NB_SENSORS];
int i;
void infrared_getSensorsValues(double* intensity){
int i;
#ifdef IS_REAL_ROBOT
double sensorMean[NB_SENSORS];
......@@ -74,6 +71,7 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
}
#else
double maxIntensity = 0;
int argMaxIntensity;
for(i=0;i<NB_SENSORS;i++)
......@@ -85,7 +83,14 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
}
}
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, const double* weights, double* frontSpeedRatio){
double intensity[NB_SENSORS];
infrared_getSensorsValues(intensity);
int i;
int fakeObjectAdded =0;
for(i=0;i<NB_SENSORS;i++)
{
......@@ -98,7 +103,7 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
if(i==0 || i==1 || i==2 || i==5 || i==6 || i== 7) *frontSpeedRatio = 0.8;
if((i == 2)) {
if(i == 2) {
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
......@@ -113,34 +118,34 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->relativePosition[THETA] = 3.64; // relative y pos
temp->life = 3;
temp->weight = 10;
temp->weight = weights[8];
measurementList = temp;
fakeObjectAdded= 1;
}
if((i == 5)) {
if(i == 5) {
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->next = measurementList;
temp->robotID = -1;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
temp->weight = 15;
temp->weight = weights[9];
measurementList = temp;
fakeObjectAdded= 1;
measurementList = temp;
fakeObjectAdded= 1;
}
}
Measurement* temp = malloc(sizeof(Measurement));
......@@ -154,13 +159,14 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i];
temp->weight = weights[i];
temp->life = 1;
// //temp->weight = ps_weights[i]*2;
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 35;
else
temp->weight = 20;
temp->weight = 20;*/
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
......@@ -40,7 +40,7 @@ void laplace_init(){
}
}
void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob, double* weights, double* currentPositionRefRob){
void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob, const double laplace_divider){
updatedSpeedRefRob[X] = 0;
updatedSpeedRefRob[Y] = 0;
......
/********************************************************************************/
/* File: rob_main.c */
/* Date: Dec-19 */
/* Description: Motors lib - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
#include "inc/laplace.h"
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#include "inc/rob_main.h"
#ifdef IS_REAL_ROBOT
#else
#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>
#endif
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
// ******** SETTINGS PARAMETERS
#define ROBOT_TO_MONITOR -1
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.05
#define SPEED_CORR 0.05
#define MAX_ACC 0.03
// ********
#define NB_SENSORS 8 // Number of distance sensors
void rob_main_reset(){
#ifdef IS_REAL_ROBOT
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
#else
wb_robot_init();
if(utilities_getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",utilities_getRobotId());
#endif
laplace_init();
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
motors_init();
}
void rob_main_migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
if(frontSpeedRatio == 1.){
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
}
my_speed[X] = my_speed[X] + MIGR_SPEED ;
// add constant migration to the other side
}
void rob_main_loop(Measurement** list,
double* my_position,
double* my_speed,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
const double laplace_divider,
double* velocityRatio){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
int lastTheta = my_position[2];
deadReckoning_updateSelfPosition(my_position, applied_speed);
int deltaTheta = my_position[2] - lastTheta;
Measurement* element = *list;
Measurement* previousElement = NULL;
while(element != NULL){
element->life--;
if(element->life == 0){
if(!previousElement) *list = element->next;
else previousElement->next = element->next;
free(element);
}
else{
element->relativePosition[THETA] -= deltaTheta;
previousElement = element;
}
if(!previousElement) element = *list;
else element = previousElement->next;
}
*list = rangeAndBearing_getRelativeRobotPositions(*list);
*list = infrared_updateList(*list, infrared_objects_w, velocityRatio);
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(*list, my_speed, laplace_divider);
double laplacianWeight = (0.4-exp(-sqrtf(my_speed[X]*my_speed[X]+my_speed[Y]*my_speed[Y])*20)*0.4);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
if(utilities_debug()) printf("Velocity ratio = %f / Laplacian Weight: %f\n", *velocityRatio, laplacianWeight);
rob_main_migratory(my_speed_tmp, my_position, laplacianWeight, *velocityRatio);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
//my_speed[X] = velocityRatio * my_speed[X] + (1-velocityRatio) * old_speed[X];
if(my_speed[X] - old_speed[X] > MAX_ACC) my_speed[X] = old_speed[X] + MAX_ACC;
if(my_speed[X] - old_speed[X] < -MAX_ACC) my_speed[X] = old_speed[X] - MAX_ACC;
if(my_speed[Y] - old_speed[Y] > MAX_ACC) my_speed[Y] = old_speed[Y] + MAX_ACC;
if(my_speed[Y] - old_speed[Y] < -MAX_ACC) my_speed[Y] = old_speed[Y] - MAX_ACC;
old_speed[X] = my_speed[X];
old_speed[Y] = my_speed[Y];
if(*velocityRatio < 1.) *velocityRatio += 0.05;
if(utilities_debug()) printf("After readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
// if(utilities_getRobotId() == 8) printf("%f,%f\n", my_speed[X], my_speed[Y]);
//if(utilities_getRobotId() == 8) printf("time step: %d\n", numberPrint);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
#ifndef IS_REAL_ROBOT
wb_robot_step(TIME_STEP);
#else
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
}
\ No newline at end of file
......@@ -5,7 +5,6 @@
/* Description: Controller for epucks */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
......
......@@ -66,7 +66,8 @@
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c
C_SOURCES = ctrl1.c ../commonSrc/laplace.c ../commonSrc/infrared.c ../commonSrc/rangeAndBearing.c \
../commonSrc/motors.c ../commonSrc/deadReckoning.c ../commonSrc/utilities.c ../commonSrc/rob_main.c
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
......
No preview for this file type
......@@ -8,166 +8,32 @@
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
#include "inc/laplace.h"
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#ifdef IS_REAL_ROBOT
#else
#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>
#endif
#include <stdio.h>
#include <math.h>
#include "../commonSrc/inc/rob_main.h"
#include "../commonSrc/inc/measurements.h"
#include <stdlib.h>
#include <string.h>
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR 0
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_TO_MONITOR 2
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.05
#define SPEED_CORR 0.05
#define MAX_ACC 0.03
// ********
#define NB_SENSORS 8 // Number of distance sensors
int state = 0 ; // state for the fsm : 0 -> free & 1 -> obstacle avoidance
static void reset(){
#ifdef IS_REAL_ROBOT
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
#else
wb_robot_init();
if(utilities_getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",utilities_getRobotId());
#endif
laplace_init();
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
motors_init();
}
void migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
if(frontSpeedRatio == 1.){
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
}
my_speed[X] = my_speed[X] + MIGR_SPEED ;
// add constant migration to the other side
}
const double ps_weights[] = {20, 10, 5, 5, 5, 5, 15, 30, 10, 15};
const double laplacian_divider = 12;
int main(){
reset(); // Resetting the robot
rob_main_reset(); // Resetting the robot
double my_position[3] = {0,0,0};
double my_speed[2] = {0,0};
double old_speed[2] = {0,0};
double weights[2] = {1,-20};
int applied_speed[2] = {0,0};
double velocityRatio = 1;
Measurement* list = NULL;
for(;;){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
int lastTheta = my_position[2];
deadReckoning_updateSelfPosition(my_position, applied_speed);
int deltaTheta = my_position[2] - lastTheta;
Measurement* element = list;
Measurement* previousElement = NULL;
while(element != NULL){
element->life--;
if(element->life == 0){
if(!previousElement) list = element->next;
else previousElement->next = element->next;
free(element);
}
else{
element->relativePosition[THETA] -= deltaTheta;
previousElement = element;
}
if(!previousElement) element = list;
else element = previousElement->next;
}
list = rangeAndBearing_getRelativeRobotPositions(list);
list = infrared_updateList(list, &velocityRatio);
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
double laplacianWeight = (0.4-exp(-sqrtf(my_speed[X]*my_speed[X]+my_speed[Y]*my_speed[Y])*20)*0.4);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
if(utilities_debug()) printf("Velocity ratio = %f / Laplacian Weight: %f\n", velocityRatio, laplacianWeight);
migratory(my_speed_tmp, my_position, laplacianWeight, velocityRatio);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
//my_speed[X] = velocityRatio * my_speed[X] + (1-velocityRatio) * old_speed[X];
if(my_speed[X] - old_speed[X] > MAX_ACC) my_speed[X] = old_speed[X] + MAX_ACC;
if(my_speed[X] - old_speed[X] < -MAX_ACC) my_speed[X] = old_speed[X] - MAX_ACC;
if(my_speed[Y] - old_speed[Y] > MAX_ACC) my_speed[Y] = old_speed[Y] + MAX_ACC;
if(my_speed[Y] - old_speed[Y] < -MAX_ACC) my_speed[Y] = old_speed[Y] - MAX_ACC;
old_speed[X] = my_speed[X];
old_speed[Y] = my_speed[Y];
if(velocityRatio < 1.) velocityRatio += 0.05;
if(utilities_debug()) printf("After readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
// if(utilities_getRobotId() == 8) printf("%f,%f\n", my_speed[X], my_speed[Y]);
//if(utilities_getRobotId() == 8) printf("time step: %d\n", numberPrint);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
#ifndef IS_REAL_ROBOT
wb_robot_step(TIME_STEP);
#else
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
rob_main_loop( &list,
my_position,
my_speed,
old_speed,
applied_speed,
ps_weights,
laplacian_divider,
&velocityRatio);
}
}
# 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
###
###-----------------------------------------------------------------------------
C_SOURCES = pso_epuck.c ../commonSrc/laplace.c ../commonSrc/infrared.c ../commonSrc/rangeAndBearing.c \
../commonSrc/motors.c ../commonSrc/deadReckoning.c ../commonSrc/utilities.c ../commonSrc/rob_main.c
### 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
......@@ -4,59 +4,35 @@
#include <string.h>
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <webots/light_sensor.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
#include "inc/laplace.h"
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#include "../commonSrc/inc/rob_main.h"
#include "../commonSrc/inc/infrared.h"
#define MAX_SPEED 1000.0 // Maximum speed of wheels in each direction
#define MAX_SPEED_WEB 6.28 // Maximum speed webots
#define MAX_ACC 1000.0 // Maximum amount speed can change in 128 ms
#define NB_SENSOR 8 // Number of proximity sensors
#define DATASIZE 9 // Number of elements in particle
#define NB_WEIGHT 11
#define DATASIZE 2*NB_WEIGHT+6 // Number of elements in particle
// Fitness definitions
#define MAX_DIFF (2*MAX_SPEED) // Maximum difference between wheel speeds
#define MAX_SENS 4096.0 // Maximum sensor value
WbDeviceTag ds[NB_SENSOR];
WbDeviceTag emitter;
WbDeviceTag rec;
WbDeviceTag left_motor, right_motor;
double good_w[DATASIZE] = {4.0, 10.0, 8.0, 6.0, 3.0, 3.0, 10.0, 14.0, 18.0};;
void reset(void) {
char text[4];
int i;
text[1]='s';
text[3]='\0';
for (i=0;i<NB_SENSOR;i++) {
text[0]='p';
text[2]='0'+i;
ds[i] = wb_robot_get_device(text); // distance sensors
}
const double ps_weights[NB_WEIGHT] = {20., 10., 5., 5., 5., 5., 15., 30., 10., 15.};
void reset_sup_com(void) {
emitter = wb_robot_get_device("emitter_epuck");
rec = wb_robot_get_device("receiver_epuck");
left_motor = wb_robot_get_device("left wheel motor");
right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
wb_receiver_enable(rec,32);
}
// Generate random number from 0 to 1
......@@ -89,110 +65,54 @@ double s(double v) {
}
// Find the fitness for obstacle avoidance of the passed controller
double fitfunc(double weights[DATASIZE],int its) {
float speed[2] = {0,0} // Wheel speeds
float old_speed[2]; // Previous wheel speeds (for recursion)
double fitfunc(const double* infra_obj_w,int its) {
//int left_encoder,right_encoder;
double ds_value[NB_SENSOR];
int i,j;