Commit 1610d454 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

yolo

parent e596245a
......@@ -39,8 +39,8 @@
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.1
#define MIGR_SPEED 0.06
#define EPS 0.08
#define MIGR_SPEED 0.055
#define SPEED_CORR 0.02
// ********
......@@ -50,19 +50,18 @@ int state = 0 ; // state for the fsm : 0 -> free & 1 -> obstacle avoidance
static void reset(){
#ifdef IS_REAL_ROBOT
#else
wb_robot_init();
if(getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",getRobotId());
#endif
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();
......@@ -87,13 +86,23 @@ int main(){
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
int applied_speed[2] = {0,0};
Measurement* list;
for(;;){
numberPrint++;
int obstacle = 0;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
Measurement* list = rangeAndBearing_getRelativeRobotPositions();
if(numberPrint%1 == 0){
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
list = rangeAndBearing_getRelativeRobotPositions();
}
list = infrared_updateList(list, &obstacle);
deadReckoning_updateSelfPosition(my_position, applied_speed);
......@@ -114,18 +123,18 @@ int main(){
if(utilities_debug()) printf("After migr x = %f, y = %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);
// Emptying data
/* // 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
......
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -12,6 +12,6 @@
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position);
void laplace_init();
#endif
......@@ -17,7 +17,7 @@
#include <math.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 200
#define DISTANCE_THRESHOLD 150
#define EPS 0.78
#ifdef IS_REAL_ROBOT
......@@ -119,12 +119,12 @@ temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 && my_position[Y] > 0 )
temp->weight = 15;
//temp->weight = ps_weights[i]*2;
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 4;
else
temp->weight = 10;
*/
temp->weight = 6;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
......@@ -107,12 +107,25 @@ void printMatrix(char* name, float* matrix, int m, int n){
}
}
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position){
// Compute neighborhood by range and bearing (contains neighbors robots and obstacles)
// X contains relative positions of each neighbors (size = (num_neighbors * 2)
#define NUMBER_ELEMENT_INT 1
float previousSpeedsRefAbs[NUMBER_ELEMENT_INT][2];
int currentElement = 0;
void laplace_init(){
int i;
for(i = 0; i < NUMBER_ELEMENT_INT; i++){
previousSpeedsRefAbs[i][0] = 0;
previousSpeedsRefAbs[i][1] = 0;
}
}
void laplace_compute(Measurement* objectsInSurround, float* updatedSpeedRefRob, float* weights, float* currentPositionRefRob){
updatedSpeedRefRob[X] = 0;
updatedSpeedRefRob[Y] = 0;
// Compute neighborhood by range and bearing (contains neighbors robots and obstacles)
// X contains relative positions of each neighbors (size = (num_neighbors * 2)
// need to save robot id using range and bearing
......@@ -177,38 +190,57 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float vel_graph[2] = {0, 0};
float velLaplaceRefRob[2] = {0, 0};
// Compute velocity of current robot with v = -L(x-B)
{
Measurement* nextObject = objectsInSurround;
int i = 1;
while(nextObject){
float distance = nextObject->relativePosition[DISTANCE];
float theta = nextObject->relativePosition[THETA];
float distanceRefRob = nextObject->relativePosition[DISTANCE];
float thetaRefRob = nextObject->relativePosition[THETA];
float bias_trans[2];
nextObject->biasPosition[X] /= 12;
nextObject->biasPosition[Y] /= 12;
utilities_transformReferential(nextObject->biasPosition, bias_trans, my_position[2]);
// if it does not work like that, maybe try to rotate the bias position to get right referential (orientation)
if(utilities_debug()) printf("id = %d, x = %f, y = %f\n",nextObject->robotID,nextObject->biasPosition[0], nextObject->biasPosition[1]);
if(utilities_debug()) printf("bias x = %f, y = %f\n", bias_trans[X], bias_trans[Y]);
if(utilities_debug()) printf("sub x = %f, y = %f\n", distance*cosf(theta), distance*sinf(theta));
if(utilities_debug()) printf("L = %f\n", L[i]);
if(utilities_debug()) printf("Object integration in Laplace id = %d\nBias x = %f, y = %f\n",nextObject->robotID,nextObject->biasPosition[0], nextObject->biasPosition[1]);
if(utilities_debug()) printf("Position RefRob x = %f, y = %f\n", distanceRefRob*cosf(thetaRefRob), distanceRefRob*sinf(thetaRefRob));
if(utilities_debug()) printf("L matrix output = %f\n", L[i]);
vel_graph[X] += -L[i] * ( distance*cosf(theta) - bias_trans[X]);
vel_graph[Y] += -L[i] * ( distance*sinf(theta) - bias_trans[Y]);
velLaplaceRefRob[X] += -L[i] * ( distanceRefRob*cosf(thetaRefRob) - nextObject->biasPosition[X]);
velLaplaceRefRob[Y] += -L[i] * ( distanceRefRob*sinf(thetaRefRob) - nextObject->biasPosition[Y]);
nextObject = nextObject->next;
i++;
}
}
//updatedSpeedRefRob[X] = velLaplaceRefRob[X]/10;
//updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/10;
float velLaplaceRefAbs[2];
utilities_transformReferential(velLaplaceRefRob, velLaplaceRefAbs, -currentPositionRefRob[2]);
// Update of the speed in the table
previousSpeedsRefAbs[currentElement][X] = velLaplaceRefAbs[X]/10;
previousSpeedsRefAbs[currentElement][Y] = velLaplaceRefAbs[Y]/10;
currentElement = (++currentElement)%NUMBER_ELEMENT_INT;
float temp[2] = {0,0};
for(i = 0; i < NUMBER_ELEMENT_INT; i++){
temp[X] += previousSpeedsRefAbs[i][X];
temp[Y] += previousSpeedsRefAbs[i][Y];
}
temp[X] /= NUMBER_ELEMENT_INT;
temp[Y] /= NUMBER_ELEMENT_INT;
newSpeed[X] = vel_graph[X]/20;
newSpeed[Y] = vel_graph[Y]/20;
utilities_transformReferential(temp, updatedSpeedRefRob, currentPositionRefRob[2]);
if(utilities_debug()) printf("Speed laplacian x = %f, y = %f\n", newSpeed[X], newSpeed[Y]);
if(utilities_getRobotId() == 8) printf("%f,%f\n", updatedSpeedRefRob[X], updatedSpeedRefRob[Y]);
}
......@@ -156,7 +156,7 @@ static Measurement* process_received_ping_messages()
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 4;
element->weight = 1;
if(utilities_debug()) printf("%d: ",receivedId);
......
......@@ -17,9 +17,9 @@ int utilities_debug(){
}
void utilities_transformReferential(float* speed, float* new_speed, float theta){
new_speed[X] = speed[X]*cosf(theta) + speed[Y]*sinf(theta);
new_speed[Y] = - speed[X]*sinf(theta) + speed[Y]*cosf(theta);
void utilities_transformReferential(float* initialReferencial, float* newReferencial, float theta){
newReferencial[X] = initialReferencial[X]*cosf(theta) + initialReferencial[Y]*sinf(theta);
newReferencial[Y] = - initialReferencial[X]*sinf(theta) + initialReferencial[Y]*cosf(theta);
}
......@@ -57,21 +57,23 @@ int getselector() {
}
#else
#include <webots/robot.h>
#include <stdio.h>
#define FIRST_GROUP_SIZE 5
int utilities_getRobotId(){
if(!id_set_up){
static char* robot_name;
robot_name=(char*) wb_robot_get_name();
char* robot_name;
robot_name = (char*) wb_robot_get_name();
sscanf(robot_name,"epuck%d",&robot_id);
robot_id = robot_id;
}
return robot_id;
}
int utilities_isInMyTeam(int robot){
return (getRobotId() < FIRST_GROUP_SIZE && robot < FIRST_GROUP_SIZE) ||
(getRobotId() >= FIRST_GROUP_SIZE && robot >= FIRST_GROUP_SIZE);
return (utilities_getRobotId() < FIRST_GROUP_SIZE && robot < FIRST_GROUP_SIZE) ||
(utilities_getRobotId() >= FIRST_GROUP_SIZE && robot >= FIRST_GROUP_SIZE);
}
int utilities_getTeamID(int robot){
......
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff0000000100000236000002e4fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002e4000000a200ffffff000000030000073d00000163fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff00000501000002e400000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff0000000100000127000001b7fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000001b7000000a200ffffff000000030000073d00000290fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff00000610000001b700000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000000450100000006010000000101
sceneTreePerspectives: 000000ff000000010000000200000177000000000100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
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: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck8:camera;1;0.820513;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff0000000100000236000002e4fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002e4000000a200ffffff000000030000073d00000163fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff00000501000002e400000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000000d20000018afc0200000001fb0000001400540065007800740045006400690074006f007201000000160000018a000000a200ffffff000000030000039f000002bdfc0100000001fb0000000e0043006f006e0073006f006c006501000000000000039f0000005400ffffff000002c70000018a00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000000000002c10100000006010000000101
sceneTreePerspectives: 000000ff00000001000000020000014a000000000100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
projectionMode: PERSPECTIVE
......@@ -10,13 +10,13 @@ selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;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