Commit 4dd5f2d5 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Quite ok

parent 164ae8f0
No preview for this file type
No preview for this file type
......@@ -72,10 +72,10 @@ static void reset(){
void migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
/*if(frontSpeedRatio == 1.){
if(frontSpeedRatio == 1.){
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 ;
// add constant migration to the other side
......
......@@ -91,11 +91,12 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
if (intensity[i] > DISTANCE_THRESHOLD){
double distance = 500/intensity[i]*0.01 ;
if(distance < 0.01) distance = 0.04;
double distance = 1/intensity[i]*10 ;
if(distance < 0.01) distance = 0.01;
double theta = ps_orientations[i];
if(i==0 || i==1 || i==2 || i==5 || i==6 || i== 7) *frontSpeedRatio = 0.8;
if((i == 2)) {
//utilities_minImm(frontSpeedRatio,1);
......@@ -108,11 +109,11 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.02; // relative x pos
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 3.64; // relative y pos
temp->life = 3;
temp->weight = 15;
temp->weight = 10;
measurementList = temp;
fakeObjectAdded= 1;
......@@ -130,7 +131,7 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.02; // relative x pos
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
......@@ -157,9 +158,9 @@ Measurement* infrared_updateList(Measurement* measurementList, double* frontSpee
temp->life = 1;
// //temp->weight = ps_weights[i]*2;
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 40;
temp->weight = 35;
else
temp->weight = 25;
temp->weight = 20;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
......@@ -9,12 +11,24 @@
#include <webots/emitter.h>
#include <webots/receiver.h>
#include "inc/laplace.h"
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#define MAX_SPEED 1000.0 // Maximum speed of wheels in each direction
#define MAX_SPEED_WEB 6.28 // Maximum speed webots
#define MAX_ACC 1000.0 // Maximum amount speed can change in 128 ms
#define NB_SENSOR 8 // Number of proximity sensors
#define DATASIZE 2 // Number of elements in particle
#define DATASIZE 9 // Number of elements in particle
// Fitness definitions
#define MAX_DIFF (2*MAX_SPEED) // Maximum difference between wheel speeds
......@@ -24,11 +38,8 @@ WbDeviceTag ds[NB_SENSOR];
WbDeviceTag emitter;
WbDeviceTag rec;
WbDeviceTag left_motor, right_motor;
double good_w[DATASIZE] = {-11.15, -16.93, -8.20, -18.11, -17.99, 8.55, -8.89, 3.52, 29.74,
-7.48, 5.61, 11.16, -9.54, 4.58, 1.41, 2.09, 26.50, 23.11,
-3.44, -3.78, 23.20, 8.41};
int braiten;
double good_w[DATASIZE] = {4.0, 10.0, 8.0, 6.0, 3.0, 3.0, 10.0, 14.0, 18.0};;
void reset(void) {
char text[4];
......@@ -102,10 +113,12 @@ double fitfunc(double weights[DATASIZE],int its) {
old_left = 0.0;
old_right = 0.0;
double my_position[3] = {0,0,0};
float my_speed[2] = {0,0};
int applied_speed[2] = {0,0};
// Evaluate fitness repeatedly
for (j=0;j<its;j++) {
if (braiten) j--; // Loop forever
ds_value[0] = (double) wb_distance_sensor_get_value(ds[0]);
ds_value[1] = (double) wb_distance_sensor_get_value(ds[1]);
ds_value[2] = (double) wb_distance_sensor_get_value(ds[2]);
......@@ -115,17 +128,52 @@ double fitfunc(double weights[DATASIZE],int its) {
ds_value[6] = (double) wb_distance_sensor_get_value(ds[6]);
ds_value[7] = (double) wb_distance_sensor_get_value(ds[7]);
// Compute the robot velocity according to laplacian method
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
laplace_compute(list, speed);
int obstacle = 0;
Measurement* list = rangeAndBearing_getRelativeRobotPositions(weights[0]);
list = infrared_updateList(list, &obstacle, weights);
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);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
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
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
// 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
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
right_speed = applied_speed[RIGHT];
left_speed = applied_speed[LEFT];
// Make sure we don't accelerate too fast
if (left_speed - old_left > MAX_ACC) left_speed = old_left+MAX_ACC;
if (left_speed - old_left < -MAX_ACC) left_speed = old_left-MAX_ACC;
if (right_speed - old_right > MAX_ACC) left_speed = old_right+MAX_ACC;
if (right_speed - old_right < -MAX_ACC) left_speed = old_right-MAX_ACC;
// Make sure speeds are within bounds
if (left_speed > MAX_SPEED) left_speed = MAX_SPEED;
if (left_speed < -1.0*MAX_SPEED) left_speed = -1.0*MAX_SPEED;
......@@ -133,23 +181,10 @@ double fitfunc(double weights[DATASIZE],int its) {
if (right_speed < -1.0*MAX_SPEED) right_speed = -1.0*MAX_SPEED;
// Set new old speeds
old_speed[0] = speed[0];
old_speed[1] = speed[1];
/*left_encoder = wb_differential_wheels_get_left_encoder();
right_encoder = wb_differential_wheels_get_right_encoder();
if (left_encoder>9000) wb_differential_wheels_set_encoders(0,right_encoder);
if (right_encoder>1000) wb_differential_wheels_set_encoders(left_encoder,0);*/
// Set the motor speeds
//wb_differential_wheels_set_speed((int)left_speed,(int)right_speed);
float msl_w = left_speed*MAX_SPEED_WEB/1000;
float msr_w = right_speed*MAX_SPEED_WEB/1000;
wb_motor_set_velocity(left_motor, (int)msl_w);
wb_motor_set_velocity(right_motor, (int)msr_w);
wb_robot_step(128); // run one step
old_left = left_speed;
old_right = right_speed;
// Get current fitness value
// Average speed
fit_speed += (fabs(left_speed) + fabs(right_speed))/(2.0*MAX_SPEED);
// Difference in speed
......@@ -195,7 +230,7 @@ int main() {
wb_robot_step(64);
while (1) {
// Wait for data
while (wb_receiver_get_queue_length(rec) == 0) {
while (wb_ receiver_get_queue_length(rec) == 0) {
wb_robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(rec);
......@@ -204,7 +239,7 @@ int main() {
if (rbuffer[DATASIZE] == -1.0) {
braiten = 1;
fitfunc(good_w,100);
// Otherwise, run provided controller
} else {
fit = fitfunc(rbuffer,rbuffer[DATASIZE]);
......@@ -214,6 +249,6 @@ int main() {
wb_receiver_next_packet(rec);
}
return 0;
}
......@@ -6,7 +6,7 @@
#include <webots/receiver.h>
#include <webots/supervisor.h>
#define ROBOTS 10
#define ROBOTS 5
#define MAX_ROB 10
#define ROB_RAD 0.035
#define ARENA_SIZE 1.89
......@@ -22,7 +22,7 @@
#define MININIT -20.0 // Lower bound on initialization value
#define MAXINIT 20.0 // Upper bound on initialization value
#define ITS 20 // Number of iterations to run
#define DATASIZE 2 // Number of elements in particle
#define DATASIZE 9 // Number of elements in particle
/* Neighborhood types */
#define STANDARD -1
......@@ -95,7 +95,7 @@ void reset(void) {
/* MAIN - Distribute and test conctrollers */
int main() {
double *weights; // Optimized result
double *weights; // Optimized result
double buffer[255]; // Buffer for emitter
int i,j,k; // Counter variables
......@@ -175,7 +175,7 @@ char valid_locs(int rob_id) {
for (i = 0; i < MAX_ROB; i++) {
if (rob_id == i) continue;
if (pow(new_loc[i][0]-new_loc[rob_id][0],2) +
if (pow(new_loc[i][0]-new_loc[rob_id][0],2) +
pow(new_loc[i][2]-new_loc[rob_id][2],2) < (2*ROB_RAD+0.01)*(2*ROB_RAD+0.01))
return 0;
}
......@@ -189,7 +189,7 @@ void random_pos(int rob_id) {
new_rot[rob_id][1] = 1.0;
new_rot[rob_id][2] = 0.0;
new_rot[rob_id][3] = 2.0*3.14159*rnd();
do {
new_loc[rob_id][0] = ARENA_SIZE*rnd() - ARENA_SIZE/2.0;
new_loc[rob_id][2] = ARENA_SIZE*rnd() - ARENA_SIZE/2.0;
......@@ -324,7 +324,7 @@ void nClosest(int neighbors[SWARMSIZE][SWARMSIZE], int numNB) {
neighbors[i][r[j]] = 1;
}
}
/* Choose all robots within some range */
......
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