Commit 46601104 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

ok for presentation, real epuck not working due to bugs with r&b lib

parent 53660a57
File deleted
......@@ -18,7 +18,12 @@ void deadReckoning_init(){
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
(void) applied_speed;
e_do_dr();
e_get_dr_position(&my_position[0],&my_position[1],&my_position[2]);
e_get_dr_position(&(my_position[0]),&(my_position[1]),&(my_position[2]));
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"deadReck location: x = %f, y= %f, theta= %f\n", my_position[0], my_position[1], my_position[2]);
utilities_print(tmp);
}
}
#else
......@@ -32,7 +37,7 @@ void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
void deadReckoning_init(){}
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
// Compute deltas of the robot
// Compute deltas of the robot
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;
......@@ -40,7 +45,7 @@ void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
// Compute deltas in the environment
double dx = du * cosf(my_position[2]);
double dy = du * sinf(my_position[2]);
double dy = du * sinf(my_position[2]);
// Update position ??? maybe add time step???
......@@ -52,8 +57,11 @@ void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
while (my_position[2] > 2*M_PI) my_position[2] -= 2.0*M_PI;
while (my_position[2] < 0) my_position[2] += 2.0*M_PI;
if(utilities_debug()) printf("deadReck location: x = %f, y= %f, theta= %f\n", my_position[0], my_position[1], my_position[2]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"deadReck location: x = %f, y= %f, theta= %f\n", my_position[0], my_position[1], my_position[2]);
utilities_print(tmp);
}
}
#endif
......@@ -61,6 +61,8 @@ int utilities_getRobotId();
int utilities_isInMyTeam(int robot);
int utilities_getTeamID(int robot);
void utilities_print(char* string);
#ifdef IS_REAL_ROBOT
void wait(long num);
int getselector() ;
......
......@@ -17,7 +17,7 @@
#include <math.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 200
#define DISTANCE_THRESHOLD 175
#ifdef IS_REAL_ROBOT
#include "./ircom/e_ad_conv.h"
......@@ -93,10 +93,16 @@ Measurement* infrared_updateList(Measurement* measurementList, const double* wei
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(utilities_debug()){
char tmp[80];
sprintf(tmp,"sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
utilities_print(tmp);
}
if (intensity[i] > DISTANCE_THRESHOLD){
double distance = 1/intensity[i]*10 ;
if(distance < 0.01) distance = 0.01;
if(distance < 0.02) distance = 0.02;
double theta = ps_orientations[i];
......@@ -155,8 +161,11 @@ Measurement* infrared_updateList(Measurement* measurementList, const double* wei
temp->weight = weights[i];
temp->life = 1;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
utilities_print(tmp);
}
measurementList = temp;
}
}
......
......@@ -21,8 +21,6 @@
* Local constants definition
*/
//#define IS_TEST
#define MAX_ROBOTS 9
/***********************
* Methods implementation
......@@ -121,10 +119,15 @@ void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob,
nextObject->biasPosition[Y] /= 12;
// if it does not work like that, maybe try to rotate the bias position to get right referential (orientation)
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]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"Object integration in Laplace id = %d\nBias x = %f, y = %f\n",nextObject->robotID,nextObject->biasPosition[0], nextObject->biasPosition[1]);
utilities_print(tmp);
sprintf(tmp,"Position RefRob x = %f, y = %f\n", distanceRefRob*cosf(thetaRefRob), distanceRefRob*sinf(thetaRefRob));
utilities_print(tmp);
sprintf(tmp,"L matrix output = %f\n", L[i]);
utilities_print(tmp);
}
velLaplaceRefRob[X] += -L[i] * ( distanceRefRob*cosf(thetaRefRob) - nextObject->biasPosition[X]);
velLaplaceRefRob[Y] += -L[i] * ( distanceRefRob*sinf(thetaRefRob) - nextObject->biasPosition[Y]);
nextObject = nextObject->next;
......
......@@ -58,9 +58,11 @@ void motors_transformSpeed(double* speed, int *speed_for_motors){
double range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
double bearing = atan2( speed[Y], speed[X]);
if(utilities_debug()) printf("range = %f and bearing = %f\n", range, bearing);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"range = %f and bearing = %f\n", range, bearing);
utilities_print(tmp);
}
double u = KU*range*cosf(bearing);
double w = KW*bearing;
......@@ -68,14 +70,20 @@ void motors_transformSpeed(double* speed, int *speed_for_motors){
speed_for_motors[RIGHT]= (u + AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
if(utilities_debug()) printf("After apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"After apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
utilities_print(tmp);
}
//For epuck
motors_limit(&(speed_for_motors[LEFT]), MAX_SPEED_EPUCK);
motors_limit(&(speed_for_motors[RIGHT]), MAX_SPEED_EPUCK);
if(utilities_debug()) printf("After limiting apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"After limiting apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
utilities_print(tmp);
}
}
......@@ -84,7 +92,11 @@ void motors_applySpeed(double* speed, int* appliedSpeed){
motors_transformSpeed(speed, appliedSpeed);
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", appliedSpeed[LEFT], appliedSpeed[RIGHT]);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"Motor speed left = %d, right = %d\n", appliedSpeed[LEFT], appliedSpeed[RIGHT]);
utilities_print(tmp);
}
#ifdef IS_REAL_ROBOT
e_set_speed_left(appliedSpeed[LEFT]);
e_set_speed_right(appliedSpeed[RIGHT]);
......
......@@ -85,8 +85,13 @@ void rangeAndBearing_init(){
#ifdef IS_REAL_ROBOT
static void send_ping()
{
ircomSend(utilities_getRobotId());
while (ircomSendDone() == 0);
int i, j;
for (i = 0; i < 10; i++)
{
for(j = 0; j < 100000; j++) asm("nop");
ircomSend(utilities_getRobotId());
while (ircomSendDone() == 0);
}
}
#else
static void send_ping()
......@@ -111,28 +116,35 @@ static Measurement* process_received_ping_messages(Measurement* listStart)
double theta;
double range;
#ifdef IS_REAL_ROBOT
int i =0;
int receivedId;
while (i < 200)
{
IrcomMessage imsg;
ircomPopMessage(&imsg);
if (imsg.error == 0)
{
receivedId = (int) imsg.value;
range = (double)imsg.distance;
theta = (double)imsg.direction;
IrcomMessage imsg;
while(1){
ircomPopMessage(&imsg);
if (imsg.error != 0)
{
break;
}
int receivedId = (int) imsg.value;
range = (double)imsg.distance;
theta = (double)imsg.direction;
#else
}
else if (imsg.error > 0)
{
break;
}
// else imsg.error == -1 -> no message available in the queue
if (imsg.error != -1) {i++;continue;};
#else
char *inbuffer; // Buffer for the receiver node
while (wb_receiver_get_queue_length(receiver) > 0) {
char *inbuffer; // Buffer for the receiver node
while (wb_receiver_get_queue_length(receiver) > 0) {
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
int receivedId = (int)inbuffer[0];
#endif
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
int receivedId = (int)inbuffer[0];
#endif
double y = message_direction[2];
double x = message_direction[1];
......@@ -141,6 +153,11 @@ static Measurement* process_received_ping_messages(Measurement* listStart)
if(range < 0.5){
numberOfRobotInRange++;
}
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"r&b before process %d: distance: %f, theta:%f\n", receivedId, range, theta );
utilities_print(tmp);
}
if(utilities_isInMyTeam(receivedId)){
......@@ -166,12 +183,17 @@ static Measurement* process_received_ping_messages(Measurement* listStart)
element->weight = 1;
element->life = 1;
if(utilities_debug()) printf("%d: ",receivedId);
if(utilities_debug()){
char tmp[80];
sprintf(tmp,"Received id: %d: ",receivedId);
utilities_print(tmp);
sprintf(tmp,"r&b %d: distance: %f, theta:%f\n", receivedId, element->relativePosition[DISTANCE], element->relativePosition[THETA] );
utilities_print(tmp);
sprintf(tmp,"bias: x: %f, y:%f\n", element->biasPosition[X], element->biasPosition[Y]);
utilities_print(tmp);
}
element->next = NULL;
if(utilities_debug()) printf("r&b %d: distance: %f, theta:%f\n", receivedId, element->relativePosition[DISTANCE], element->relativePosition[THETA] );
if(utilities_debug()) printf("bias: x: %f, y:%f\n", element->biasPosition[X], element->biasPosition[Y]);
}
#ifndef IS_REAL_ROBOT
wb_receiver_next_packet(receiver);
......
......@@ -40,10 +40,10 @@
void rob_main_reset(){
#ifdef IS_REAL_ROBOT
e_init_robot_id();
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
#else
wb_robot_init();
if(utilities_getRobotId() == ROBOT_TO_MONITOR)
......@@ -53,8 +53,10 @@ void rob_main_reset(){
laplace_init();
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
#ifndef IS_REAL_ROBOT
rangeAndBearing_init();
#endif
motors_init();
}
......@@ -78,18 +80,19 @@ Measurement* rob_main_addMigrationWeight(Measurement* list, double* my_position,
}
void rob_main_loop(Measurement** list,
double* my_position,
double* my_speed,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
double weight_migration,
const double laplace_divider){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
double* my_position,
double* my_speed,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
double weight_migration,
const double laplace_divider){
numberPrint++;
if(utilities_debug()){
char tmp[50];
sprintf(tmp, "_______\nRobot %d\n",utilities_getRobotId());
utilities_print(tmp);
}
int lastTheta = my_position[2];
deadReckoning_updateSelfPosition(my_position, applied_speed);
int deltaTheta = my_position[2] - lastTheta;
......@@ -98,41 +101,50 @@ void rob_main_loop(Measurement** 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;
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 = rob_main_addMigrationWeight(*list, my_position, weight_migration);
#ifndef IS_REAL_ROBOT
*list = rangeAndBearing_getRelativeRobotPositions(*list);
#endif
*list = infrared_updateList(*list, infrared_objects_w);
// compute new speed according to laplace method in initial referential
laplace_compute(*list, my_speed, laplace_divider);
if(utilities_debug()) printf("Speed x = %f, y = %f / Robots in range: %d\n", my_speed[X], my_speed[Y], rangeAndBearing_getNumberOfRobotInRange());
if(utilities_debug()){
char tmp[100];
sprintf(tmp, "Speed x = %f, y = %f / Robots in range: %d\n", my_speed[X], my_speed[Y], rangeAndBearing_getNumberOfRobotInRange());
utilities_print(tmp);
}
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(utilities_debug()) printf("After speed readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
if(utilities_debug()){
char tmp[100];
sprintf(tmp, "After speed readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_print(tmp);
}
motors_applySpeed(my_speed, applied_speed);
......@@ -143,4 +155,4 @@ void rob_main_loop(Measurement** list,
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
}
}
......@@ -13,8 +13,8 @@
#include <math.h>
#include <stdio.h>
#define PRINT_FREQUENCY 1
#define PRINT_MATRIX 1
#define PRINT_FREQUENCY 5
#define PRINT_MATRIX 0
int enabledDebug = 0;
int numberPrint = 0;
......@@ -25,7 +25,7 @@ void utilities_changeDebugState(int enabledDebugTemp){
}
int utilities_debug(){
return ((numberPrint%PRINT_FREQUENCY == 0) && enabledDebug);
return ((numberPrint%PRINT_FREQUENCY == 0 && enabledDebug));
}
// Matrix operations fonctions
......@@ -47,32 +47,38 @@ 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];
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];
}
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");
if(!PRINT_MATRIX || !utilities_debug()) return;
char tmp[80];
sprintf(tmp,"Matrix %s (%d * %d)\n", name, m, n);
utilities_print(tmp);
for(i= 0; i<m; i++){
for(j= 0; j<n; j++){
sprintf(tmp,"%f\t", matrix[i*n+j]);
utilities_print(tmp);
}
sprintf(tmp,"\n");
utilities_print(tmp);
}
}
// Min - max number functions
......@@ -102,11 +108,12 @@ int id_set_up = 0;
#include "inc/e_robot_id.h"
#include "./epfl/e_epuck_ports.h"
#include <btcom/btcom.h>
int utilities_getRobotId(){
if(!id_set_up){
robot_id = e_get_robot_id()%10;
id_set_up = 1;
id_set_up = 1;
}
return robot_id;
}
......@@ -127,6 +134,9 @@ void wait(long num) {
int getselector() {
return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
}
void utilities_print(char* string){
btcomSendString(string);
}
#else
......@@ -146,11 +156,15 @@ int utilities_getRobotId(){
int utilities_isInMyTeam(int robot){
return (utilities_getRobotId() < FIRST_GROUP_SIZE && robot < FIRST_GROUP_SIZE) ||
(utilities_getRobotId() >= FIRST_GROUP_SIZE && robot >= FIRST_GROUP_SIZE);
(utilities_getRobotId() >= FIRST_GROUP_SIZE && robot >= FIRST_GROUP_SIZE);
}
int utilities_getTeamID(int robot){
return robot%FIRST_GROUP_SIZE;
}
void utilities_print(char* string){
printf("%s", string);
}
#endif
......@@ -31,8 +31,8 @@ EXTERNAL_OBJS += $(wildcard $(EPUCKLIBROOT)/ircom/*.o)
# Local variables
# ------------------------------------------------------------------------------
# first, find all the pertinent filenames
ASMSRCS = $(wildcard *.s)
CSRCS = normal_epuck.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c e_robot_id.c
ASMSRCS = $(wildcard *.s) ../commonSrc/e_robot_id_asm.s
CSRCS = normal_epuck.c ../commonSrc/rob_main.c ../commonSrc/laplace.c ../commonSrc/infrared.c ../commonSrc/rangeAndBearing.c ../commonSrc/motors.c ../commonSrc/deadReckoning.c ../commonSrc/utilities.c ../commonSrc/e_robot_id.c
CXXSRCS = $(wildcard *.cc) $(wildcard *.cpp) $(wildcard *.C)
SRCS = $(CSRCS) $(CXXSRCS) $(ASMSRCS)
HDRS = $(wildcard *.h) $(wildcard *.hh)
......
......@@ -12,7 +12,7 @@
#include "../commonSrc/inc/measurements.h"
#include <stdlib.h>
const double ps_weights[] = {15, 15, 15, 0, 0, 35, 35, 35, 10, 15};
const double ps_weights[] = {25, 15, 15, 5, 5, 30, 30, 35, 15, 20};
const double laplacian_divider = 12;
const double weight_migration = 5;
......
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
......@@ -16,7 +16,7 @@
#define NB_SENSOR 8 // Number of proximity sensors
#define NB_WEIGHT 11
#define NB_WEIGHT 10
#define DATASIZE NB_WEIGHT // Number of elements in particle
// Fitness definitions
......@@ -90,7 +90,7 @@ double fitfunc(const double* infra_obj_w,int its) {
Measurement* list = NULL;
//const double weight_migration = 5;
const double weight_migration = fabs(infra_obj_w[10]);
const double weight_migration = 5 ;//fabs(infra_obj_w[10]);
// Evaluate fitness repeatedly
for (j=0;j<its;j++) {
......
......@@ -8,7 +8,7 @@
/**************************************************/
#define FONT "Arial"
#define NB_WEIGHT 11
#define NB_WEIGHT 10
#define DATASIZE NB_WEIGHT
#define SWARMSIZE 10
......
......@@ -66,7 +66,7 @@
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = supervisor.c
C_SOURCES = supervisor_metrics.c
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
......
......@@ -3,6 +3,6 @@
#define __SUP_METRICS_
// Functions
compute_fitness(float* fit_c, float* fit_o, float* fit_v, float* fit_tot);
void compute_fitness(float* fit_c, float* fit_o, float* fit_v, float* fit_tot);
#endif
......@@ -11,16 +11,16 @@
#include <webots/robot.h>
#include <webots/supervisor.h>
#define FLOCK_SIZE 5 // Number of robots in flock
#define VMAX 0.1287 // EPuck maximum speed
#define TIME_STEP 64 // [ms] Length of time step
#define FLOCK_SIZE 5 // Number of robots in flock
#define TIME_STEP 64.0 // [ms] Length of time step
#define TEAM 2
WbNodeRef robs[FLOCK_SIZE]; // Robots nodes
WbFieldRef robs_trans[FLOCK_SIZE]; // Robots translation fields
WbFieldRef robs_rotation[FLOCK_SIZE]; // Robots rotation fields
WbNodeRef robs[FLOCK_SIZE*TEAM]; // Robots nodes