Commit 164ae8f0 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Not perfect but acceptable, top be optimized with PSO

parent 2b0925c0
No preview for this file type
No preview for this file type
No preview for this file type
......@@ -40,9 +40,9 @@
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.06
#define MIGR_SPEED 0.05
#define SPEED_CORR 0.05
#define MAX_ACC 0.05
#define MAX_ACC 0.03
// ********
#define NB_SENSORS 8 // Number of distance sensors
......@@ -72,10 +72,11 @@ static void reset(){
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 /* * (1- laplacianWeight)*/;
}*/
my_speed[X] = my_speed[X] + MIGR_SPEED ;
// add constant migration to the other side
}
......@@ -91,25 +92,36 @@ int main(){
int applied_speed[2] = {0,0};
double velocityRatio = 1;
Measurement* list;
Measurement* list = NULL;
for(;;){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
if(numberPrint%1 == 0){
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
list = rangeAndBearing_getRelativeRobotPositions();
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);
deadReckoning_updateSelfPosition(my_position, applied_speed);
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
......@@ -148,14 +160,8 @@ int main(){
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
/* // Emptying data
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
*/
#ifndef IS_REAL_ROBOT
wb_robot_step(TIME_STEP);
#else
......
......@@ -15,6 +15,7 @@ struct Measurement_t{
double biasPosition[2]; // position in the graph of the robot
double weight;
Measurement* next;
int life;
};
#endif
......@@ -10,7 +10,7 @@
* Returns:
* - The mesurments list with robot IDs
*/
Measurement* rangeAndBearing_getRelativeRobotPositions();
Measurement* rangeAndBearing_getRelativeRobotPositions(Measurement* list);
void rangeAndBearing_init();
......
......@@ -17,7 +17,7 @@
#include <math.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 150
#define DISTANCE_THRESHOLD 200
#ifdef IS_REAL_ROBOT
#include "./ircom/e_ad_conv.h"
......@@ -85,16 +85,20 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
}
}
#endif
int fakeObjectAdded =0;
for(i=0;i<NB_SENSORS;i++)
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
if (intensity[i] > DISTANCE_THRESHOLD){
double distance = sqrt(1/intensity[i]);
double distance = 500/intensity[i]*0.01 ;
if(distance < 0.01) distance = 0.04;
double theta = ps_orientations[i];
/*if(i == 2 || i == 5) {
//utilities_minImm(frontSpeedRatio,0.8);
if((i == 2)) {
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
......@@ -105,14 +109,37 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.02; // relative x pos
temp->relativePosition[THETA] = 3.14; // relative y pos
temp->relativePosition[THETA] = 3.64; // relative y pos
temp->life = 3;
temp->weight = 15;
measurementList = temp;
fakeObjectAdded= 1;
}
if((i == 5)) {
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
}*/
temp->relativePosition[DISTANCE] = 0.02; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
temp->weight = 15;
measurementList = temp;
fakeObjectAdded= 1;
}
Measurement* temp = malloc(sizeof(Measurement));
......@@ -127,12 +154,12 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i];
temp->life = 1;
// //temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 15;
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 40;
else
temp->weight = 20;*/
temp->weight = 25;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
......@@ -134,8 +134,8 @@ void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob,
i++;
}
}
updatedSpeedRefRob[X] = velLaplaceRefRob[X]/10;
updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/10;
updatedSpeedRefRob[X] = velLaplaceRefRob[X]/12;
updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/12;
//if(utilities_getRobotId() == 8) printf("%f,%f\n", updatedSpeedRefRob[X], updatedSpeedRefRob[Y]);
......
......@@ -96,11 +96,10 @@ static void send_ping()
}
#endif
static Measurement* process_received_ping_messages()
static Measurement* process_received_ping_messages(Measurement* listStart)
{
Measurement* listStart = NULL;
Measurement* element = NULL;
Measurement* element = listStart;
if(element) while(element->next) element = element->next;
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
......@@ -157,6 +156,7 @@ static Measurement* process_received_ping_messages()
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 1;
element->life = 1;
if(utilities_debug()) printf("%d: ",receivedId);
......@@ -173,7 +173,7 @@ static Measurement* process_received_ping_messages()
return listStart;
}
Measurement* rangeAndBearing_getRelativeRobotPositions(){
Measurement* rangeAndBearing_getRelativeRobotPositions(Measurement* list){
send_ping();
return process_received_ping_messages();
return process_received_ping_messages(list);
}
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001270000020dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000020d000000a200ffffff000000030000073d0000023afc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000006100000020d00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000001180000032a0100000006010000000101
sceneTreePerspectives: 000000ff000000010000000200000177000000000100000006010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000127000002e4fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002e40000008b00ffffff0000000300000a000000023afc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000a000000008700ffffff000008d7000002e400000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000001180000032a0100000002010000000101
sceneTreePerspectives: 000000ff000000010000000200000177000000000100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
......@@ -9,12 +9,12 @@ viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck7:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck6:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck5:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck9:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck5:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck8:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck6:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000027a000002b9fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002b9000000a200ffffff000000030000073d0000018efc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000004bd000002b900000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000027a00000352fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003520000008b00ffffff0000000300000a00000001ccfc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000a000000008700ffffff000007840000035200000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment