Commit 6a1c5382 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Test de modifs non concluant

parent 1885564b
No preview for this file type
......@@ -38,8 +38,7 @@
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.8
#define MIGR_SPEED 0.5
#define EPS 0.6
#define SPEED_CORR 0.05
// ********
......@@ -58,8 +57,8 @@ static void reset(){
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"epuck%d",&robot_id_u);
robot_id = robot_id_u%FLOCK_SIZE;
//if((int)(robot_name[5]-'0') == ROBOT_TO_MONITOR)
// utilities_changeDebugState(1);
if((int)(robot_name[5]-'0') == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",robot_id_u);
deadReckoning_init();
......@@ -70,23 +69,22 @@ static void reset(){
void migratory(float* my_speed, float* my_position){
void migratory(float* my_speed, float* my_position, float migrSpeed){
// realign migration
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
if (my_position[Y] > EPS) {my_speed[Y] -= SPEED_CORR;};
if (my_position[Y] < -EPS) {my_speed[Y] += SPEED_CORR;};
// add constant migration to the other side
my_speed[X] += MIGR_SPEED;
my_speed[X] += migrSpeed;
}
int main(){
reset(); // Resetting the robot
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
float my_position[3] = {0,0,0};
int applied_speed[2] = {0,0};
float velMigr = 0.04;
for(;;){
numberPrint++;
int obstacle = 0;
......@@ -98,14 +96,20 @@ int main(){
deadReckoning_updateSelfPosition(my_position, applied_speed);
float my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
if(numberPrint==100){
velMigr = 0.05;
//printf("glou\n");
}
laplace_compute(list, my_speed, my_position);
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("Before migr x = %f, y = %f\n", my_speed_tmp[X], my_speed_tmp[Y]);
if(!obstacle) migratory(my_speed_tmp, my_position);
migratory(my_speed_tmp, my_position, velMigr);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
......
......@@ -8,7 +8,7 @@
#define V0 0.5 // Migratory urge speed
#define AXLE_LENGTH 0.052 // Distance between wheels of robot (meters)
#define AXLE_LENGTH 0.053 // Distance between wheels of robot (meters)
#define SPEED_UNIT_RADS 0.00628 // Conversion factor from speed unit to radian per second
#define WHEEL_RADIUS 0.0205 // Wheel radius (meters)
#define DELTA_T 0.064 // Timestep (seconds)
......@@ -22,11 +22,11 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
float dr = (float) applied_speed[RIGHT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float dl = (float) applied_speed[LEFT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float du = (dr + dl)/2.0;
float dtheta = (dr - dl)/AXLE_LENGTH;
float dtheta = (dr - dl)/AXLE_LENGTH;///1.325;
// Compute deltas in the environment
float dx = du * cosf(dtheta);
float dy = du * sinf(dtheta);
float dx = du * cosf(my_position[2]+dtheta/2);
float dy = du * sinf(my_position[2]+dtheta/2);
// Update position ??? maybe add time step???
my_position[0] += dx;
......
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position);
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* my_position);
#endif
......@@ -13,6 +13,7 @@ struct Measurement_t{
int robotID; // -1 if not a robot
float relativePosition[2];
float biasPosition[2]; // position in the graph of the robot
float weight;
Measurement* next;
};
......
......@@ -20,7 +20,7 @@
#include <stdio.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 150
#define DISTANCE_THRESHOLD 200
#define EPS 0.78
float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
......@@ -43,11 +43,10 @@ void infrared_init(){
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
double intensity;
for(int i=0;i<NB_SENSORS;i++)
{
intensity = wb_distance_sensor_get_value(ds[i]);
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity);
if (intensity > DISTANCE_THRESHOLD){
......@@ -56,22 +55,25 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
bool isRobot = false;
if(measurementList){ // BUG ifno element in list
/* if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
/*if( element->robotID != -1 &&
*//*if( element->robotID != -1 &&
element->relativePosition[DISTANCE] < 0.12 &&
( element->relativePosition[THETA] > theta - EPS ||
element->relativePosition[THETA] < theta + EPS ) ){
isRobot = true;
element->robotID = -1;
break;
}*/
}*//*
element = element->next;
}while(element != NULL);
}
if(isRobot) continue;
if(isRobot) continue;*/
*obstacle = 1;
......@@ -87,6 +89,12 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926)
temp->weight = 30;
else
temp->weight = 10;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
......
......@@ -105,7 +105,7 @@ void printMatrix(char* name, float* matrix, int m, int n){
}
}
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position){
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, 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)
......@@ -146,9 +146,9 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
while(nextObject){
if (nextObject->robotID != -1)
weightsMatrix[j*numberOfObjects+j] = fabs(weights[0]);
weightsMatrix[j*numberOfObjects+j] = fabs(nextObject->weight);
else{
weightsMatrix[j*numberOfObjects+j] = -fabs(weights[1]);
weightsMatrix[j*numberOfObjects+j] = -fabs(nextObject->weight);
obstacleInSurround = 1;
}
nextObject = nextObject->next;
......@@ -186,8 +186,8 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
float bias_trans[2];
nextObject->biasPosition[X] /= 10;
nextObject->biasPosition[Y] /= 10;
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)
......@@ -203,8 +203,8 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
}
}
newSpeed[X] = vel_graph[X];
newSpeed[Y] = vel_graph[Y];
newSpeed[X] = vel_graph[X]/10;
newSpeed[Y] = vel_graph[Y]/10;
if(utilities_debug()) printf("Speed laplacian x = %f, y = %f\n", newSpeed[X], newSpeed[Y]);
......
......@@ -21,8 +21,7 @@
#define KW 1
#define AXLE_LENGTH 0.052
#define WHEEL_RADIUS 0.0205
//#define MAX_SPEED 1000
#define MAX_SPEED 628
#define MAX_SPEED 1000
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
......@@ -51,8 +50,8 @@ void motors_transformSpeed(float* speed, int *speed_for_motors){
if(utilities_debug()) printf("linearSpeed = %f and rotationalSpeed = %f\n", linearSpeed, rotationalSpeed);
speed_for_motors[RIGHT] = (linearSpeed + AXLE_LENGTH * rotationalSpeed)/2.0 * (10 / WHEEL_RADIUS);
speed_for_motors[LEFT] = (linearSpeed - AXLE_LENGTH * rotationalSpeed)/2.0 * (10 / WHEEL_RADIUS);
speed_for_motors[RIGHT] = (linearSpeed + AXLE_LENGTH * rotationalSpeed)/2.0 * (1000 / WHEEL_RADIUS);
speed_for_motors[LEFT] = (linearSpeed - AXLE_LENGTH * rotationalSpeed)/2.0 * (1000 / WHEEL_RADIUS);
if(utilities_debug()) printf("After apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
......@@ -67,11 +66,9 @@ void motors_transformSpeed(float* speed, int *speed_for_motors){
void motors_applySpeed(float* speed, int* appliedSpeed){
motors_transformSpeed(speed, appliedSpeed);
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", appliedSpeed[LEFT], appliedSpeed[RIGHT]);
wb_motor_set_velocity(left_motor, (double) appliedSpeed[LEFT]/100);
wb_motor_set_velocity(right_motor, (double) appliedSpeed[RIGHT]/100);
wb_motor_set_velocity(left_motor, appliedSpeed[LEFT]/160);
wb_motor_set_velocity(right_motor, appliedSpeed[RIGHT]/160);
}
\ No newline at end of file
......@@ -29,9 +29,12 @@
#define FIRST_GROUP_SIZE 5
// define bias matrix with at most 9 robots
float bias[10][2] = {{4,0},{3,1},{3,-1},{2,0},{2,-2},
{2,2},{1,1},{1,-1},{0,0}};
float bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
{2,2},{2,0},{1,1},{1,-1},{0,0}};
//float bias[10][2] = {{1,0.5},{2,-0.5},{3,0.5},{4,-0.5},
// {5,0.5},{6,0},{7,0},{8,0},{9,0}};
/***********************
* Local prototype
*/
......@@ -127,6 +130,8 @@ static Measurement* process_received_ping_messages(char* robot_name)
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 1;
if(utilities_debug()) printf("%d: ",receivedId);
......
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000023600000430fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000004300000008b00ffffff000000030000078000000259fc0100000001fb0000000e0043006f006e0073006f006c006503000000000000036b0000078000000145000005480000043000000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff000000010000000200000198000000000100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck9: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: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
......@@ -10,13 +10,13 @@ selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck9: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: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
......@@ -9,7 +9,7 @@ viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 2 "controllers/ctrl1/ctrl1.c" "../../../../../Applications/Webots.app/projects/default/controllers/braitenberg/braitenberg.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck4: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: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000023600000430fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000004300000008b00ffffff000000030000078000000259fc0100000001fb0000000e0043006f006e0073006f006c006503fffffffc000002ad0000078000000259000005480000043000000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000000100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
This diff is collapsed.
This diff is collapsed.
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