Commit efd7ac0c authored by Hugaguett's avatar Hugaguett

trigo tricks and fixing bugs

parent 56da1b07
......@@ -46,6 +46,8 @@
static char* robot_name;
static int robot_id_u, robot_id;
bool state = 0 ; // state for the fsm : 0 -> free & 1 -> obstacle avoidance
static void reset(){
wb_robot_init();
......@@ -63,29 +65,55 @@ static void reset(){
motors_init();
}
void transformSpeedReferential(double* speed, double theta){
double tmp_speed[2];
tmp_speed[0] = speed[0]*cos(theta) + speed[1]*sin(theta);
tmp_speed[1] = - speed[0]*sin(theta) + speed[1]*cos(theta);
speed[0] = tmp_speed[0];
speed[1] = tmp_speed[1];
}
void migratory(my_speed, my_position){
// realign migration
if (my_position[1] > EPS) my_speed[1]-=SPEED_CORR;
if (my_position[1] < -EPS) my_speed[1]+=SPEED_CORR;
// add constant migration to the other side
my_position[0] = MIGR_SPEED;
}
int main(){
reset(); // Resetting the robot
float new_speed[2] = {0,0};
float my_speed[2] = {0,0};
float vel_migr[2] = {0.5,0};
float weights[2] = {1,-2};
float my_position[3] = {0,0,0};
int msr = 0;
int msl = 0;
for(;;){
(int msr, int msl, float my_position, float* vel_mig)
for(;;){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
// realign robot in direction of the migratory urge
deadReckoning(vel_migr);
deadReckoning_updateSelfPosition(my_position, msr, msl);
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights);
transformSpeedReferential(my_speed, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
migratory(my_speed, my_position);
// compute new speed
laplace_compute(list, new_speed, vel_migr, weights);
// change speed in inital referential to current referential
transformSpeedReferential(my_speed, my_position[2]); //from R0 to Rt
// apply speed to motors
motors_applySpeed(new_speed);
motors_applySpeed(my_speed, &msr, &msl);
wb_robot_step(TIME_STEP);
}
......
......@@ -7,39 +7,75 @@
#include <time.h>
#define V0 0.5
#define V0 0.5 // Migratory urge speed
#define AXLE_LENGTH 0.052 // 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)
void deadReckoning_init(){
}
/*
void deadReckoning(float* vel_mig){
double x;
double y;
double theta;
// update dead reckoning via motor steps since last call
deadReckoning_do_dr();
// retrieve new position
deadReckoning_get_dr_position(&x, &y, &theta);
void deadReckoning_updateSelfPosition(float my_position, int msr, int msl){
// Compute deltas of the robot
float dr = (float)msr * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float dl = (float)msl * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float du = (dr + dl)/2.0;
float dtheta = (dr - dl)/AXLE_LENGTH;
// compute new velocity to realign according to the migratory urge
vel_mig[X] = V0 * cos(dr_theta);
vel_mig[Y] = V0 * sin(dr_theta);
if(utilities_debug()) printf("Dear reckoning: %f, %f, %f\n", x, y, theta );
// Compute deltas in the environment
float dx = -du * sinf(theta);
float dy = -du * cosf(theta);
// Update position ??? maybe add time step???
my_position[0] += dx;
my_position[1] += dy;
my_position[2] += dtheta;
// Keep orientation within 0, 2pi
if (my_position[2] > 2*M_PI) my_position[2] -= 2.0*M_PI;
if (my_position[2] < 0) my_position[2] += 2.0*M_PI;
}
*/
void deadReckoning_realign(int msr, int msl, float my_position, float* vel_mig){
// webots method : updates robot position with wheel speeds
void deadReckoning(float* vel_mig){
// real world method
// update dead reckoning via motor steps since last call
// e_do_dr();
// retrieve new position
// e_get_dr_position(&x, &y, &theta);
// compute new velocity to realign according to the migratory urge
vel_mig[X] = 0.5;
vel_mig[Y] = 0;
if (dy < -EPS) // too low
{
vel_mig[Y] = V0;
}
if (dy > EPS) // too high
{
vel_mig[Y] = -V0;
}
vel_mig[X] = V0;
if(utilities_debug()) printf("Dear reckoning: %f, %f, %f\n", x, y, theta );
}
//void deadReckoning(float* vel_mig){
//
//
// // compute new velocity to realign according to the migratory urge
// vel_mig[X] = 0.5;
// vel_mig[Y] = 0;
//
//}
void deadReckoning_updateSteps(float* speed, double time){
......
......@@ -9,6 +9,7 @@ typedef struct Measurement_t Measurement;
struct Measurement_t{
int robotID; // -1 if not a robot
float relativePosition[2];
int biasPosition[2]; // position in the graph of the robot
Measurement* next;
};
......
......@@ -22,8 +22,9 @@
*/
#define PRINT_MATRIX 1
//#define IS_TEST
#define ALPHA_FREE 0.7
#define ALPHA_OBST 0.5
#define ALPHA_FREE 0.7
#define ALPHA_OBST 0.5
#define MAX_ROBOTS 9
/***********************
* Local prototype
......@@ -69,6 +70,14 @@ void printMatrix(char* name, float* matrix, int m, int n);
* Methods implementation
*/
void transformBiases(my_id, int* b){
b[0] = bias[id];
for (int i=1;i<my_id;i++){
b[i+1] = bias[i];
}
}
void transpose(float* input, float* output, int m, int n)
{
for(int i=0; i<m; i++)
......@@ -106,6 +115,12 @@ void printMatrix(char* name, float* matrix, int m, int n){
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* velMigration, float* weights){
// 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
int numberOfObjects = 0;
Measurement* element = objectsInSurround;
......@@ -149,7 +164,7 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* vel
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
// float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
......@@ -172,8 +187,9 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* vel
Measurement* nextObject = objectsInSurround;
int i = 0;
while(nextObject){
vel_graph[X] += -L[i] * nextObject->relativePosition[X];
vel_graph[Y] += -L[i] * nextObject->relativePosition[Y];
// if it does not work like that, maybe try to rotate the bias position to get right referential (orientation)
vel_graph[X] += -L[i] * (nextObject->relativePosition[X] - nextObject->biasPosition[X]);
vel_graph[Y] += -L[i] * (nextObject->relativePosition[Y] - nextObject->biasPosition[Y]);
nextObject = nextObject->next;
i++;
}
......@@ -181,8 +197,9 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* vel
if(utilities_debug()) printf("Vel graph x = %f, y = %f\n", vel_graph[X], vel_graph[Y]);
if(utilities_debug()) printf("Vel migration x = %f, y = %f\n", velMigration[X], velMigration[Y]);
newSpeed[X] = alpha * vel_graph[X] + (1-alpha) * velMigration[X]; // tot velocity is a weighted sum of graph and migration velocities
newSpeed[Y] = alpha * vel_graph[Y] + (1-alpha) * velMigration[Y];
newSpeed[X] = vel_graph[X];
newSpeed[Y] = vel_graph[Y];
if(utilities_debug()) printf("Speed x = %f, y = %f\n", newSpeed[X], newSpeed[Y]);
......
......@@ -40,9 +40,7 @@ void motors_init(){
wb_motor_set_position(right_motor, 0);
}
void motors_transformSpeed(float* speed, int *msl, int *msr){
void motors_transformSpeed(float* speed, int *msr, int *msl){
float linearSpeed = KU * sqrt(pow(speed[X],2)+pow(speed[Y],2)) * cos(atan2(speed[Y],speed[X]));
float rotationalSpeed = KW * atan2(speed[Y],speed[X]);
......@@ -55,22 +53,17 @@ void motors_transformSpeed(float* speed, int *msl, int *msr){
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", *msl, *msr);
}
int last_v_left = 0;
int last_v_right = 0;
int last_msl = 0;
int last_msr = 0;
double start_time = 0;
void motors_applySpeed(float* speed){
int v_right;
int v_left;
motors_transformSpeed(speed, &v_right, &v_left);
deadReckoning_updateSteps(speed, wb_robot_get_time()-start_time);
void motors_applySpeed(float* speed, int* msr, int* msl){
motors_transformSpeed(speed, msr, msl);
last_v_left = v_left;
last_v_right = v_right;
last_msl = *msl;
last_msr = *msr;
start_time = wb_robot_get_time();
wb_motor_set_position(left_motor, v_left);
wb_motor_set_position(right_motor, v_right);
wb_motor_set_position(left_motor, *msl);
wb_motor_set_position(right_motor, *msr);
}
\ No newline at end of file
......@@ -25,7 +25,12 @@
#include <stdio.h>
#include <math.h>
#define MAX_BOUNDARY 0.3
#define MAX_BOUNDARY 0.3
#define FIRST_GROUP_SIZE 10
// define bias matrix with at most 9 robots
int bias[10][2] = {{4,0},{3,1},{3,-1},{2,0},{2,-2}
{2,2},{1,1},{1,-1},{0,0}};
/***********************
* Local prototype
......@@ -67,6 +72,7 @@ void rangeAndBearing_init(){
}
bias =
static void send_ping(char* robot_name)
{
......@@ -106,7 +112,6 @@ static Measurement* process_received_ping_messages(char* robot_name)
double y_rel = -1.0 * range*sin(theta);
if(sqrt(x_rel*x_rel + y_rel*y_rel) < MAX_BOUNDARY){
if(listStart == NULL){
element = malloc(sizeof(Measurement));
listStart = element;
......@@ -117,15 +122,21 @@ static Measurement* process_received_ping_messages(char* robot_name)
int receivedId = (int)(inbuffer[5]-'0');
if((myId < 5 && receivedId < 5) || (myId >= 5 && receivedId >= 5) ){
element->robotID = receivedId;
element->biasPosition[X] = bias[myId%FIRST_GROUP_SIZE][0];
element->biasPosition[Y] = bias[myId%FIRST_GROUP_SIZE][1];
if(utilities_debug()) printf("%d: ",receivedId);
}
else{
element->robotID = -1;
element->biasPosition[X] = 0;
element->biasPosition[Y] = 0;
if(utilities_debug()) printf("%d (ennemy): ",receivedId);
}
}
element->relativePosition[X] = x_rel; // relative x pos
element->relativePosition[Y] = y_rel; // relative y pos
element->next = NULL;
if(utilities_debug()) printf("%f, %f\n", element->relativePosition[X], element->relativePosition[Y] );
}
......
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