...
 
Commits (2)
File added
......@@ -36,13 +36,13 @@
#define USE_SUPERVISOR 0
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_TO_MONITOR 0
#define ROBOT_TO_MONITOR 2
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.1
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.06
#define SPEED_CORR 0.02
#define SPEED_CORR 0.05
#define MAX_ACC 0.05
// ********
#define NB_SENSORS 8 // Number of distance sensors
......@@ -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();
......@@ -71,61 +70,92 @@ static void reset(){
void migratory(float* my_speed, float* my_position){
// realign migration
//if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
//if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
void migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
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)*/;
// add constant migration to the other side
my_speed[X] += MIGR_SPEED;
}
int main(){
reset(); // Resetting the robot
double my_position[3] = {0,0,0};
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
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;
for(;;){
numberPrint++;
int obstacle = 0;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
Measurement* list = rangeAndBearing_getRelativeRobotPositions();
list = infrared_updateList(list, &obstacle);
if(numberPrint%1 == 0){
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
list = rangeAndBearing_getRelativeRobotPositions();
}
list = infrared_updateList(list, &velocityRatio);
deadReckoning_updateSelfPosition(my_position, applied_speed);
float my_speed_tmp[2];
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
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);
migratory(my_speed_tmp, my_position);
// 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);
// 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.
......@@ -33,14 +33,14 @@ void deadReckoning_init(){}
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
// Compute deltas of the robot
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;
double dr = (double) applied_speed[RIGHT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
double dl = (double) applied_speed[LEFT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
double du = (dr + dl)/2.0;
double dtheta = (dr - dl)/AXLE_LENGTH;
// Compute deltas in the environment
float dx = du * cosf(my_position[2]);
float dy = du * sinf(my_position[2]);
double dx = du * cosf(my_position[2]);
double dy = du * sinf(my_position[2]);
// Update position ??? maybe add time step???
......
......@@ -4,6 +4,6 @@
#include "measurements.h"
void infrared_init();
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle);
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio);
#endif
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position);
void laplace_compute(Measurement* objectsInSurround, double* newSpeed, double* weights, double* my_position);
void laplace_init();
#endif
......@@ -11,9 +11,9 @@ typedef struct Measurement_t Measurement;
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;
double relativePosition[2];
double biasPosition[2]; // position in the graph of the robot
double weight;
Measurement* next;
};
......
......@@ -5,6 +5,6 @@
void motors_init();
void motors_applySpeed(float* speed, int* appliedSpeed);
void motors_applySpeed(double* speed, int* appliedSpeed);
#endif
......@@ -7,9 +7,56 @@ extern int numberPrint;
#define LEFT 0
#define RIGHT 1
void utilities_changeDebugState(int enableDebugTemp);
// Debug functions
void utilities_changeDebugState(int enabledDebugTemp);
int utilities_debug();
void utilities_transformReferential(float* speed, float* new_speed, float theta);
// Matrix operations fonctions
void utilities_transformReferential(double* initialReferencial, double* newReferencial, double theta);
void utilities_transformReferentialImm(double* initialReferencial, double theta);
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void utilities_transpose(double* input, double* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void utilities_multiply(double* mat1, double* mat2, double* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void utilities_printMatrix(char* name, double* matrix, int m, int n);
// Min - max number functions
double utilities_min(double a, double b);
void utilities_minImm(double* dest, double in);
double utilities_max(double a, double b);
void utilities_maxImm(double* dest, double in);
// Robot ID functions
int utilities_getRobotId();
int utilities_isInMyTeam(int robot);
int utilities_getTeamID(int robot);
......
......@@ -17,8 +17,7 @@
#include <math.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 200
#define EPS 0.78
#define DISTANCE_THRESHOLD 150
#ifdef IS_REAL_ROBOT
#include "./ircom/e_ad_conv.h"
......@@ -31,9 +30,9 @@ static WbDeviceTag ds[NB_SENSORS];
#endif
float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};
//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(){
......@@ -51,17 +50,17 @@ void infrared_init(){
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
Measurement* infrared_updateList(Measurement* measurementList, double* frontSpeedRatio){
double intensity[NB_SENSORS];
int i;
#ifdef IS_REAL_ROBOT
double sensorMean[NB_SENSORS];
for (i=0; i<NB_SENSORS; i++)
sensorMean[i]=0;
sensorMean[i]=0;
int n;
int n;
for(n=0;n<NB_SAMPLES; n++)
{
......@@ -75,63 +74,72 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
}
#else
double maxIntensity = 0;
int argMaxIntensity;
for(i=0;i<NB_SENSORS;i++)
{
intensity[i] = wb_distance_sensor_get_value(ds[i]);
if (intensity[i] > maxIntensity) {
maxIntensity = intensity[i];
argMaxIntensity = i;
}
}
#endif
for(i=0;i<NB_SENSORS;i++)
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity);
//if(i == 3 || i == 4) continue;
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
if (intensity[i] > DISTANCE_THRESHOLD){
float distance = sqrt(1/intensity[i]);
float theta = ps_orientations[i];
/*if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
if( element->robotID != -1 &&
element->relativePosition[DISTANCE] < 0.12 &&
( element->relativePosition[THETA] > theta - EPS ||
element->relativePosition[THETA] < theta + EPS ) ){
isRobot = true;
break;
}
double distance = sqrt(1/intensity[i]);
double theta = ps_orientations[i];
element = element->next;
}while(element != NULL);
}
if(isRobot) continue;*/
*obstacle = 1;
/*if(i == 2 || i == 5) {
//utilities_minImm(frontSpeedRatio,0.8);
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.02; // relative x pos
temp->relativePosition[THETA] = 3.14; // relative y pos
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;
else
temp->weight = 10;
*/
temp->weight = 15;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
measurementList = temp;
}
}
return measurementList;
}*/
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i];
// //temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 15;
else
temp->weight = 20;*/
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
}
}
return measurementList;
}
......@@ -20,99 +20,33 @@
/***********************
* Local constants definition
*/
#define PRINT_MATRIX 1
//#define IS_TEST
#define ALPHA_FREE 0.7
#define ALPHA_OBST 0.5
#define MAX_ROBOTS 9
/***********************
* Local prototype
*/
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void transpose(float* input, float* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void printMatrix(char* name, float* matrix, int m, int n);
/***********************
* Methods implementation
*/
#define NUMBER_ELEMENT_INT 1
double previousSpeedsRefAbs[NUMBER_ELEMENT_INT][2];
int currentElement = 0;
void transpose(float* input, float* output, int m, int n)
{
int i, j;
for(i=0; i<m; i++)
for(j=0; j<n; j++)
{
output[j*m+i] = input[i*n+j];
}
void laplace_init(){
int i;
for(i = 0; i < NUMBER_ELEMENT_INT; i++){
previousSpeedsRefAbs[i][0] = 0;
previousSpeedsRefAbs[i][1] = 0;
}
}
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)
{
int i, j, k;
for (i = 0; i < m; i++)
{
for (j = 0; j < p; j++)
{
res[i*p+j] = 0;
for (k = 0; k < n; k++)
res[i*p+j] += mat1[i*n+k]*mat2[k*p+j];
}
}
}
void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob, double* weights, double* currentPositionRefRob){
void printMatrix(char* name, float* matrix, int m, int n){
int i, j;
if(!PRINT_MATRIX || !utilities_debug()) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(i= 0; i<m; i++){
for(j= 0; j<n; j++){
printf("%f\t", matrix[i*n+j]);
}
printf("\n");
}
}
updatedSpeedRefRob[X] = 0;
updatedSpeedRefRob[Y] = 0;
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)
// need to save robot id using range and bearing
......@@ -124,12 +58,12 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
element = element->next;
}
float incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
double incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
int i, j;
for(i = 0; i < (numberOfObjects+1) * numberOfObjects; i++)
incidenceMatrix[i] = 0.;
float weightsMatrix[numberOfObjects*numberOfObjects];
double weightsMatrix[numberOfObjects*numberOfObjects];
for(i = 0; i < numberOfObjects * numberOfObjects; i++)
weightsMatrix[i] = 0.;
......@@ -139,7 +73,7 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
incidenceMatrix[j] = 1;
incidenceMatrix[(j+1)*numberOfObjects+j] = -1;
}
printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
utilities_printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
......@@ -158,57 +92,52 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
j++;
}
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
utilities_printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
// float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
double incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
utilities_transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
utilities_printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
double IW[(numberOfObjects+1) * numberOfObjects];
utilities_multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
utilities_printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
float IW[(numberOfObjects+1) * numberOfObjects];
multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
double L[(numberOfObjects+1) * (numberOfObjects+1)];
utilities_multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
utilities_printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float L[(numberOfObjects+1) * (numberOfObjects+1)];
multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float vel_graph[2] = {0, 0};
double 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 bias_trans[2];
double distanceRefRob = nextObject->relativePosition[DISTANCE];
double thetaRefRob = nextObject->relativePosition[THETA];
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;
newSpeed[X] = vel_graph[X]/20;
newSpeed[Y] = vel_graph[Y]/20;
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]);
}
......@@ -53,16 +53,16 @@ void motors_init(){
#endif
}
void motors_transformSpeed(float* speed, int *speed_for_motors){
void motors_transformSpeed(double* speed, int *speed_for_motors){
float range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
double range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
float bearing = atan2( speed[Y], speed[X]);
double bearing = atan2( speed[Y], speed[X]);
if(utilities_debug()) printf("range = %f and bearing = %f\n", range, bearing);
float u = KU*range*cosf(bearing);
float w = KW*bearing;
double u = KU*range*cosf(bearing);
double w = KW*bearing;
speed_for_motors[LEFT] = (u - AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
speed_for_motors[RIGHT]= (u + AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
......@@ -80,7 +80,7 @@ void motors_transformSpeed(float* speed, int *speed_for_motors){
}
void motors_applySpeed(float* speed, int* appliedSpeed){
void motors_applySpeed(double* speed, int* appliedSpeed){
motors_transformSpeed(speed, appliedSpeed);
......
......@@ -36,7 +36,7 @@ static WbDeviceTag emitter;
#define MAX_BOUNDARY 0.3
// define bias matrix with at most 9 robots
float bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
double bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
{2,2},{2,0},{1,1},{1,-1},{0,0}};
/***********************
......@@ -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);
......
/********************************************************************************/
/* File: utilities.c */
/* Version: 1.0 */
/* Date: 11-Dec-19 */
/* Description: Controller for epucks */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
#include "inc/utilities.h"
#include "inc/measurements.h"
#include <math.h>
#include <stdio.h>
#define PRINT_FREQUENCY 1
#define PRINT_MATRIX 1
int enabledDebug = 0;
int numberPrint = 0;
// Debug functions
void utilities_changeDebugState(int enabledDebugTemp){
enabledDebug = enabledDebugTemp;
}
int utilities_debug(){
return ((numberPrint%50 == 0) && enabledDebug);
return ((numberPrint%PRINT_FREQUENCY == 0) && enabledDebug);
}
// Matrix operations fonctions
void utilities_transformReferential(double* initialReferencial, double* newReferencial, double theta){
newReferencial[X] = initialReferencial[X]*cosf(theta) + initialReferencial[Y]*sinf(theta);
newReferencial[Y] = - initialReferencial[X]*sinf(theta) + initialReferencial[Y]*cosf(theta);
}
void utilities_transformReferentialImm(double* initialReferencial, double theta){
double newReferencial[2];
newReferencial[X] = initialReferencial[X]*cosf(theta) + initialReferencial[Y]*sinf(theta);
newReferencial[Y] = - initialReferencial[X]*sinf(theta) + initialReferencial[Y]*cosf(theta);
initialReferencial[X] = newReferencial[X];
initialReferencial[Y] = newReferencial[Y];
}
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_transpose(double* input, double* output, int m, int n)
{
int i, j;
for(i=0; i<m; i++)
for(j=0; j<n; j++)
output[j*m+i] = input[i*n+j];
}
void utilities_multiply(double* mat1, double* mat2, double* res, int m, int n, int p)
{
int i, j, k;
for (i = 0; i < m; i++)
for (j = 0; j < p; j++)
{
res[i*p+j] = 0;
for (k = 0; k < n; k++)
res[i*p+j] += mat1[i*n+k]*mat2[k*p+j];
}
}
void utilities_printMatrix(char* name, double* matrix, int m, int n){
int i, j;
if(!PRINT_MATRIX || !utilities_debug()) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(i= 0; i<m; i++){
for(j= 0; j<n; j++){
printf("%f\t", matrix[i*n+j]);
}
printf("\n");
}
}
// Min - max number functions
double utilities_min(double a, double b){
return (a<b) ? a : b;
}
void utilities_minImm(double* dest, double in){
if(in < *dest) *dest = in;
}
double utilities_max(double a, double b){
return (a<b) ? b : a;
}
void utilities_maxImm(double* dest, double in){
if(in > *dest) *dest = in;
}
// Robot ID functions
int robot_id;
int id_set_up = 0;
......@@ -58,20 +131,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: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001270000020dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000020d000000a200ffffff000000030000073d0000023afc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000006100000020d00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000001180000032a0100000006010000000101
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: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001910000018afc0200000001fb0000001400540065007800740045006400690074006f007201000000160000018a000000a200ffffff000000030000073d000002bdfc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000005a60000018a00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000004a00100000006010000000101
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
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001e8000002b9fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002b9000000a200ffffff000000030000073d0000018efc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff0000054f000002b900000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
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;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000027a00000182fc0200000001fb0000001400540065007800740045006400690074006f0072010000001600000182000000a200ffffff00000003000005000000008cfc0100000001fb0000000e0043006f006e0073006f006c00650100000000000005000000005400ffffff000002800000018200000001000000020000000100000008fc00000000
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000027a000002b9fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002b9000000a200ffffff000000030000073d0000018efc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000004bd000002b900000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
maximizedDockId: -1
......@@ -9,7 +9,7 @@ viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3: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
This diff is collapsed.