Commit d1d33f9a authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Correction PSO

parent 29d7aa6f
......@@ -96,7 +96,7 @@ Measurement* infrared_updateList(Measurement* measurementList, const double* wei
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 = 1/intensity[i]*10 ;
if(distance < 0.03) distance = 0.03;
if(distance < 0.01) distance = 0.01;
double theta = ps_orientations[i];
......
......@@ -48,7 +48,7 @@ void rob_main_reset(){
wb_robot_init();
if(utilities_getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",utilities_getRobotId());
//printf("Reset: robot %d\n",utilities_getRobotId());
#endif
laplace_init();
......
......@@ -101,7 +101,7 @@ int main() {
endfit = 0.0;
bestfit = 0.0;
// Do 10 runs and send the best controller found to the robot
for (j=0;j<40;j++) {
for (j=0;j<10;j++) {
// Get result of optimization
weights = pso(SWARMSIZE,NB,LWEIGHT,NBWEIGHT,VMAX,MININIT,MAXINIT,ITS,DATASIZE,ROBOTS);
......@@ -129,15 +129,19 @@ int main() {
for (i = 0; i < DATASIZE; i++)
bestw[i] = weights[i];
}
for (i = 0; i < NB_WEIGHT; i++){
printf("%d %.3f |",i , (double) weights[i]);
}
printf("\n");
printf("Performance: %.3f\n",fit);
endfit += fit/10; // average over the 10 runs
wb_robot_step(500);
wb_supervisor_simulation_reset();
}
for (i = 0; i < NB_WEIGHT; i++){
printf("Weight: %d %.3f\n",i, (double) bestw[i]);
}
printf("Average performance: %.3f\n",endfit);
/* Send best controller to robots */
......@@ -184,6 +188,7 @@ void random_pos(int rob_id) {
wb_supervisor_field_set_sf_vec3f(wb_supervisor_node_get_field(robs[rob_id],"translation"), new_loc[rob_id]);
wb_supervisor_field_set_sf_rotation(wb_supervisor_node_get_field(robs[rob_id],"rotation"), new_rot[rob_id]);
wb_supervisor_simulation_reset();
}
void random_placement(double* pos) {
......
......@@ -8,8 +8,8 @@ selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 1 "controllers/ctrl1/ctrl1.c" "controllers/normal_epuck/normal_epuck.c"
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: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
......@@ -8,8 +8,8 @@ selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/pso_epuck/pso_epuck.c" "controllers/pso_osb_sup/pso_obs_sup.c"
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
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