Commit 29d7aa6f authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Adding supervisor

parent 24ed4153
......@@ -2,3 +2,4 @@
build/
old/
*.d
ctrl1/
......@@ -12,6 +12,6 @@
*/
Measurement* rangeAndBearing_getRelativeRobotPositions(Measurement* list);
void rangeAndBearing_init();
int rangeAndBearing_getNumberOfRobotInRange();
#endif
......@@ -66,7 +66,7 @@ static Measurement* process_received_ping_messages();
* Methods implementation
*/
int numberOfRobotInRange = 0;
void rangeAndBearing_init(){
......@@ -96,11 +96,16 @@ static void send_ping()
}
#endif
int rangeAndBearing_getNumberOfRobotInRange(){
return numberOfRobotInRange;
}
static Measurement* process_received_ping_messages(Measurement* listStart)
{
Measurement* element = listStart;
if(element) while(element->next) element = element->next;
numberOfRobotInRange = 0;
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
double theta;
......@@ -119,6 +124,7 @@ static Measurement* process_received_ping_messages(Measurement* listStart)
theta = (double)imsg.direction;
#else
char *inbuffer; // Buffer for the receiver node
while (wb_receiver_get_queue_length(receiver) > 0) {
......@@ -132,7 +138,9 @@ static Measurement* process_received_ping_messages(Measurement* listStart)
theta = -atan2(y,x) - 1.5707;
range = sqrt((1/message_rssi));
if(range < 0.5){
numberOfRobotInRange++;
}
if(utilities_isInMyTeam(receivedId)){
......
......@@ -121,7 +121,7 @@ void rob_main_loop(Measurement** list,
// 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\n", my_speed[X], my_speed[Y]);
if(utilities_debug()) printf("Speed x = %f, y = %f / Robots in range: %d\n", my_speed[X], my_speed[Y], rangeAndBearing_getNumberOfRobotInRange());
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;
......
......@@ -66,7 +66,7 @@
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = ctrl1.c ../commonSrc/laplace.c ../commonSrc/infrared.c ../commonSrc/rangeAndBearing.c \
C_SOURCES = normal_epuck.c ../commonSrc/laplace.c ../commonSrc/infrared.c ../commonSrc/rangeAndBearing.c \
../commonSrc/motors.c ../commonSrc/deadReckoning.c ../commonSrc/utilities.c ../commonSrc/rob_main.c
### Do not modify: this includes Webots global Makefile.include
space :=
......
......@@ -8,7 +8,7 @@
# TODO: Figure out why LD won't link C++ object files with standard args.
# ==============================================================================
PROG = ctrl1
PROG = normal_epuck
EPUCKLIBROOT = ../../epuck_libIRcom/e-puck/src
# ==============================================================================
......@@ -32,7 +32,7 @@ EXTERNAL_OBJS += $(wildcard $(EPUCKLIBROOT)/ircom/*.o)
# ------------------------------------------------------------------------------
# first, find all the pertinent filenames
ASMSRCS = $(wildcard *.s)
CSRCS = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c e_robot_id.c
CSRCS = normal_epuck.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c e_robot_id.c
CXXSRCS = $(wildcard *.cc) $(wildcard *.cpp) $(wildcard *.C)
SRCS = $(CSRCS) $(CXXSRCS) $(ASMSRCS)
HDRS = $(wildcard *.h) $(wildcard *.hh)
......
No preview for this file type
......@@ -9,6 +9,7 @@
#include "../commonSrc/inc/rob_main.h"
#include "../commonSrc/inc/infrared.h"
#include "../commonSrc/inc/rangeAndBearing.h"
#define MAX_SPEED 1000.0 // Maximum speed of wheels in each direction
......@@ -75,6 +76,7 @@ double fitfunc(const double* infra_obj_w,int its) {
double fit_speed = 0.0; // Speed aspect of fitness
double fit_diff = 0.0; // Speed difference between wheels aspect of fitness
double fit_sens = 0.0; // Proximity sensing aspect of fitness
double fit_rb = 0.0;
double ds_value[NB_SENSOR];
double sens_val[NB_SENSOR]; // Average values for each proximity sensor
......@@ -111,6 +113,8 @@ double fitfunc(const double* infra_obj_w,int its) {
fit_speed += (abs(applied_speed[0]) + abs(applied_speed[1]))/MAX_DIFF;
// Difference in speed
fit_diff += abs(applied_speed[0] - applied_speed[1])/MAX_DIFF;
fit_rb += ((double)rangeAndBearing_getNumberOfRobotInRange())/4;
// Sensor values
}
......@@ -123,9 +127,9 @@ double fitfunc(const double* infra_obj_w,int its) {
fit_speed /= its;
fit_diff /= its;
fit_sens /= its;
fit_rb /= its;
// Better fitness should be higher
return fit_speed*(1.0 - sqrt(fit_diff))*(1.0 - fit_sens);
return fit_speed*(1.0 - sqrt(fit_diff))*(1.0 - fit_sens) * fit_rb;
}
//-------------MAIN------------//
......
......@@ -10,15 +10,15 @@
#define MAX_ROB 5
#define ROB_RAD 0.035
#define ARENA_SIZE 2.5
#define DEPOSITION_AREA 1
#define DEPOSITION_AREA 0.5
/* PSO definitions */
#define SWARMSIZE 10 // Number of particles in swarm
#define NB 1 // Number of neighbors on each side
#define LWEIGHT 2.0 // Weight of attraction to personal best
#define NBWEIGHT 2.0 // Weight of attraction to neighborhood best
#define VMAX 20.0 // Maximum velocity particle can attain
#define MININIT -20.0 // Lower bound on initialization value
#define MAXINIT 20.0 // Upper bound on initialization value
#define MININIT 0 // Lower bound on initialization value
#define MAXINIT 50.0 // Upper bound on initialization value
#define ITS 20 // Number of iterations to run
/* Neighborhood types */
......@@ -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<10;j++) {
for (j=0;j<40;j++) {
// Get result of optimization
weights = pso(SWARMSIZE,NB,LWEIGHT,NBWEIGHT,VMAX,MININIT,MAXINIT,ITS,DATASIZE,ROBOTS);
......
No preview for this file type
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
### Generic Makefile.include for Webots controllers, physics plugins, robot
### window libraries, remote control libraries and other libraries
### to be used with GNU make
###
### Platforms: Windows, macOS, Linux
### Languages: C, C++
###
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
### Edmund Ronald, Sergei Poskriakov
###
###-----------------------------------------------------------------------------
###
### This file is meant to be included from the Makefile files located in the
### Webots projects subdirectories. It is possible to set a number of variables
### to customize the build process, i.e., add source files, compilation flags,
### include paths, libraries, etc. These variables should be set in your local
### Makefile just before including this Makefile.include. This Makefile.include
### should never be modified.
###
### Here is a description of the variables you may set in your local Makefile:
###
### ---- C Sources ----
### if your program uses several C source files:
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
###
### ---- C++ Sources ----
### if your program uses several C++ source files:
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
###
### ---- Compilation options ----
### if special compilation flags are necessary:
### CFLAGS = -Wno-unused-result
###
### ---- Linked libraries ----
### if your program needs additional libraries:
### INCLUDE = -I"/my_library_path/include"
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
###
### ---- Linking options ----
### if special linking flags are needed:
### LFLAGS = -s
###
### ---- Webots included libraries ----
### if you want to use the Webots C API in your C++ controller program:
### USE_C_API = true
### if you want to link with the Qt framework embedded in Webots:
### QT = core gui widgets network
###
### ---- Debug mode ----
### if you want to display the gcc command line for compilation and link, as
### well as the rm command details used for cleaning:
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = supervisor.c
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include
#ifndef __SUP_METRICS_
#define __SUP_METRICS_
// Functions
compute_fitness(float* fit_c, float* fit_o, float* fit_v, float* fit_tot);
#endif
#include <stdio.h>
#include <math.h>
#include <complex.h> /* Standard Library of Complex Numbers */
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include "inc/supervisor_metrics.h"
#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
WbNodeRef robs[FLOCK_SIZE]; // Robots nodes
WbFieldRef robs_trans[FLOCK_SIZE]; // Robots translation fields
WbFieldRef robs_rotation[FLOCK_SIZE]; // Robots rotation fields
float loc[FLOCK_SIZE][3]; // Location of everybody in the flock
float old_loc_com[2];
int t;
int offset = 0;
/*
* Initialize flock position and devices
*/
void reset(void) {
wb_robot_init();
t = 0;
char rob[7] = "epuck0";
int i;
for (i=0;i<FLOCK_SIZE;i++) {
sprintf(rob,"epuck%d",i+offset);
robs[i] = wb_supervisor_node_get_from_def(rob);
robs_trans[i] = wb_supervisor_node_get_field(robs[i],"translation");
robs_rotation[i] = wb_supervisor_node_get_field(robs[i],"rotation");
old_loc_com[0] += wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]/FLOCK_SIZE; // X
old_loc_com[1] += wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]/FLOCK_SIZE; // Z
}
printf("init : x = %f y = %f\n",old_loc_com[0],old_loc_com[1]);
}
/*
* Compute performance metric.
*/
void compute_fitness(float* fit_c, float* fit_o, float* fit_v, float* fit_tot) {
//printf("old pos : posx = %f posy = %f\n", old_loc_com[0], old_loc_com[1]);
*fit_o = 0.0;
*fit_c = 0.0;
*fit_v = 0.0;
float complex o_cum = 0.0 + I * 0.0;
float c_cum = 0;
float loc_com[2] = {0,0};
for (int i=0;i<FLOCK_SIZE;i++) {
loc_com[0] += loc[i][0];
loc_com[1] += loc[i][1];
}
loc_com[0] /= FLOCK_SIZE;
loc_com[1] /= FLOCK_SIZE;
for (int i=0;i<FLOCK_SIZE;i++) {
o_cum += cexp(I*loc[i][2]);
c_cum += sqrtf(powf(loc[i][0]-loc_com[0],2) + powf(loc[i][1]-loc_com[1],2));
}
//printf("delta p : vx = %f vy = %f\n",loc_com[0]-old_loc_com[0],loc_com[1]-old_loc_com[1]);
*fit_v = fmax((loc_com[0]-old_loc_com[0])/TIME_STEP,0)/VMAX;
*fit_o = sqrtf(powf(creal(o_cum),2)+ powf(cimag(o_cum),2))/FLOCK_SIZE;
*fit_c = 1/(1+c_cum/FLOCK_SIZE);
*fit_tot = *fit_o * *fit_c * *fit_v;
old_loc_com[0] = loc_com[0];
old_loc_com[1] = loc_com[1];
}
/*
* Main function.
*/
int main(int argc, char *args[]) {
// Compute reference fitness values
float fit_o; // Performance metric for orientation
float fit_c; // Performance metric for aggregation
float fit_v; // Performance metric for velocity
float fit_tot; // Global performance metric
reset();
for(;;) {
wb_robot_step(TIME_STEP);
for (int i=0;i<FLOCK_SIZE;i++) {
// Get data
loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X
loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z
loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA
}
//Compute and normalize fitness values
compute_fitness(&fit_c, &fit_o, &fit_v, &fit_tot);
printf("%d, %f, %f, %f, %f\n", t, fit_o, fit_c, fit_v, fit_tot);
t++;
}
}
epuck @ 582da7b2
Subproject commit 582da7b2ca3e527b855a32f5dcada26a7430d58a
......@@ -7,9 +7,9 @@ centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
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: epuck3: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: epuck1:camera;1;1;0;0
......@@ -768,7 +768,7 @@ DEF epuck0 E-puck {
translation -2.9 0 0
rotation 0 1 0 -1.5700000000112309
name "epuck0"
controller "ctrl1"
controller "normal_epuck"
emitter_channel 0
receiver_channel 0
groundSensorsSlot [
......@@ -788,7 +788,7 @@ DEF epuck1 E-puck {
translation -2.9 0 0.1
rotation 0 1 0 -1.5700000000112309
name "epuck1"
controller "ctrl1"
controller "normal_epuck"
emitter_channel 0
receiver_channel 0
groundSensorsSlot [
......@@ -808,7 +808,7 @@ DEF epuck2 E-puck {
translation -2.9 0 -0.1
rotation 0 1 0 -1.5700000000112309
name "epuck2"
controller "ctrl1"
controller "normal_epuck"
emitter_channel 0
receiver_channel 0
groundSensorsSlot [
......@@ -828,7 +828,7 @@ DEF epuck3 E-puck {
translation -2.9 0 0.213892
rotation 0 1 0 -1.5700000000112309
name "epuck3"
controller "ctrl1"
controller "normal_epuck"
emitter_channel 0
receiver_channel 0
groundSensorsSlot [
......@@ -848,7 +848,7 @@ DEF epuck4 E-puck {
translation -2.9 0 -0.2
rotation 0 1 0 -1.5700000000112309
name "epuck4"
controller "ctrl1"
controller "normal_epuck"
emitter_channel 0
receiver_channel 0
groundSensorsSlot [
......
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