// controller initialization
static void reset(void) {
fitness_reset();
int i;
char name[] = "ps0";
for(i = 0; i < NB_SENSORS; i++) {
ps[i]=wb_robot_get_device(name); // get sensor handle
// perform distance measurements every TIME_STEP millisecond
wb_distance_sensor_enable(ps[i], TIME_STEP);
name[2]++; // increase the device name to "ps1", "ps2", etc.
}
}
//
// Reset the robot controller and initiate the sensors and emitter/receiver
//
static int reset()
{
int i;
mode =1;
emitter = wb_robot_get_device("emitter");
//buffer = (double *) emitter_get_buffer(emitter);
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, TIME_STEP);
char text[5]="led0";
for(i=0;i<NB_LEDS;i++) {
led[i]=wb_robot_get_device(text); // get a handler to the sensor
text[3]++; // increase the device name to "ps1", "ps2", etc.
}
text[0]='p';
text[1]='s';
text[3]='\0';
text[2]='0';
ps[0] = wb_robot_get_device(text); // proximity sensors
text[2]='7';
ps[1] = wb_robot_get_device(text); // proximity sensors
text[2]='1';
ps[2] = wb_robot_get_device(text); // proximity sensors
text[2]='6';
ps[3] = wb_robot_get_device(text); // proximity sensors
text[2]='2';
ps[4] = wb_robot_get_device(text); // proximity sensors
text[2]='5';
ps[5] = wb_robot_get_device(text); // proximity sensors
text[2]='3';
ps[6] = wb_robot_get_device(text); // proximity sensors
text[2]='4';
ps[7] = wb_robot_get_device(text); // proximity sensors
// Enable proximity and floor sensors
for(i=0;i<NB_DIST_SENS;i++) {
wb_distance_sensor_enable(ps[i],TIME_STEP);
printf("ps[%d] is active\n",i);
}
// Enable GPS sensor to determine position
gps=wb_robot_get_device("gps");
wb_gps_enable(gps, TIME_STEP);
printf("gps is active\n");
return 1;
}
static void initialize()
{
wb_robot_init();
currentBehavior = RANDOM_MOVER;
ps0 = wb_robot_get_device("ps0");
ps1 = wb_robot_get_device("ps1");
ps2 = wb_robot_get_device("ps2");
ps3 = wb_robot_get_device("ps3");
ps4 = wb_robot_get_device("ps4");
ps5 = wb_robot_get_device("ps5");
ps6 = wb_robot_get_device("ps6");
ps7 = wb_robot_get_device("ps7");
ls0 = wb_robot_get_device("ls0");
ls1 = wb_robot_get_device("ls1");
ls2 = wb_robot_get_device("ls2");
ls3 = wb_robot_get_device("ls3");
ls4 = wb_robot_get_device("ls4");
ls5 = wb_robot_get_device("ls5");
ls6 = wb_robot_get_device("ls6");
ls7 = wb_robot_get_device("ls7");
led4 = wb_robot_get_device("led4");
int sensorTimeout = 100;
wb_distance_sensor_enable(ps0, sensorTimeout);
wb_distance_sensor_enable(ps1, sensorTimeout);
wb_distance_sensor_enable(ps2, sensorTimeout);
wb_distance_sensor_enable(ps3, sensorTimeout);
wb_distance_sensor_enable(ps4, sensorTimeout);
wb_distance_sensor_enable(ps5, sensorTimeout);
wb_distance_sensor_enable(ps6, sensorTimeout);
wb_distance_sensor_enable(ps7, sensorTimeout);
wb_light_sensor_enable(ls0, sensorTimeout);
wb_light_sensor_enable(ls1, sensorTimeout);
wb_light_sensor_enable(ls2, sensorTimeout);
wb_light_sensor_enable(ls3, sensorTimeout);
wb_light_sensor_enable(ls4, sensorTimeout);
wb_light_sensor_enable(ls5, sensorTimeout);
wb_light_sensor_enable(ls6, sensorTimeout);
wb_light_sensor_enable(ls7, sensorTimeout);
wb_led_set(led4, 1);
}
/*
* Reset the robot's devices and get its ID
*/
static void reset()
{
int i;
wb_robot_init();
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
for(i=0; i<FLOCK_SIZE; i++) {
timestamp[i] = 0;
relative_pos[i][0] = 0;
relative_pos[i][1] = 0;
relative_pos[i][2] = 0;
}
maxTimestamp = 1;
robot_name=(char*) wb_robot_get_name();
for(i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
my_position[0] = 0;
my_position[1] = 0;
my_position[2] = 0;
//printf("reset: my pos: %f, %f\n", my_position[0], my_position[1]);
wb_differential_wheels_enable_encoders(64);
//Reading the robot's name. Pay attention to name specification when adding robots to the simulation!
sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name
robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1
sprintf(robot_number, "%d", robot_id_u);
receiver = wb_robot_get_device("receiver");
receiver0 = wb_robot_get_device("receiver0");
emitter = wb_robot_get_device("emitter");
emitter0 = wb_robot_get_device("emitter0");
wb_receiver_enable(receiver,32);
wb_receiver_enable(receiver0,32);
for(i=0; i<FLOCK_SIZE; i++) {
initialized[i] = 0; // Set initialization to 0 (= not yet initialized)
}
}
void reset()
{
int i;
robotName = wb_robot_get_name();
currentState = FORWARD;
// Make sure to initialize the RNG with different values for each thread
srand(time(NULL) + (int)&robotName);
char e_puck_name[] = "ps0";
char sensorsName[5];
// Emitter and receiver device tags
emitterTag = wb_robot_get_device("emitter");
receiverTag = wb_robot_get_device("receiver");
// Configure communication devices
wb_receiver_enable(receiverTag, TIME_STEP);
wb_emitter_set_range(emitterTag, COMM_RADIUS);
wb_emitter_set_channel(emitterTag, COMMUNICATION_CHANNEL);
wb_receiver_set_channel(receiverTag, COMMUNICATION_CHANNEL);
sprintf(sensorsName, "%s", e_puck_name);
for (i = 0; i < NB_SENSORS; i++) {
sensors[i] = wb_robot_get_device(sensorsName);
wb_distance_sensor_enable(sensors[i], TIME_STEP);
if ((i + 1) >= 10) {
sensorsName[2] = '1';
sensorsName[3]++;
if ((i + 1) == 10) {
sensorsName[3] = '0';
sensorsName[4] = (char) '\0';
}
} else {
sensorsName[2]++;
}
}
wb_differential_wheels_enable_encoders(TIME_STEP);
printf("Robot %s is reset\n", robotName);
return;
}
/*
* Reset the robot's devices and get its ID
*/
static void reset()
{
wb_robot_init();
receiver = wb_robot_get_device("receiver");
emitter = wb_robot_get_device("emitter");
compass2 = wb_robot_get_device("compass2");
receiver0 = wb_robot_get_device("receiver0");
emitter0 = wb_robot_get_device("emitter0");
int i;
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
robot_name=(char*) wb_robot_get_name();
for(i=0; i<FLOCK_SIZE; i++) {
relative_pos[i][0] = my_position[0] = 0;
relative_pos[i][1] = my_position[1] = 0;
relative_pos[i][2] = my_position[2] = 0;
}
for(i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
wb_receiver_enable(receiver,64);
wb_receiver_enable(receiver0,64);
wb_compass_enable(compass2, 64);
//Reading the robot's name. Pay attention to name specification when adding robots to the simulation!
sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name
robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1
for(i=0; i<FLOCK_SIZE; i++) {
initialized[i] = 0; // Set initialization to 0 (= not yet initialized)
}
}
int main(){
reset();
int msl,msr; // motor speed left and right
double distances[NB_SENSORS]; // array keeping the distance sensor readings
int elapsed_time; // elapsed time during one time_step
int obstacle_near; // flag which becomes true if an obstacle is nearby
int hit_flag; // flag which becomes true as soon as the robot hits an obstacle
int last_ts_was_hit = 0; // flag memorizing whether the robot was already in a collision during the last
// time step
int time_steps; // counter for simulated time steps
int hit_counter; // counter for time steps involving collisions
int collision_duration; // duration of a single collision (in time steps unit)
double average_collision_duration;// average duration of a single collision
const double* gpsvalues; // matrix containing gps readings
double actx,acty; // current position
double oldx,oldy; // previous position
double speed; // average robot speed in free space
int collision_started = 0;
// iteration variables
int i;
int sensor_nb;
FILE* f; // file handle
char fname[]="/tmp/collision_pos.txt"; // file name to store the positions of the collisions
if(fmod(TIME_STEP,50)!=0){
printf("ERROR: The TIME_STEP needs to be a multiple of 50ms\n");
return(0);
}
for(i=0;i<NB_SENSORS;i++) {
wb_distance_sensor_enable(ps[i],50); // this function wb_*_enable() allows to specify an update interval for the sensor readings in milliseconds
}
wb_gps_enable(gps,50);
wb_robot_step(50);
gpsvalues = wb_gps_get_values(gps); // returns a pointer to three double values. The pointer is the address of an array allocated by the function internally.
wb_robot_step(50);
oldx=gpsvalues[0];
oldy=gpsvalues[2];
time_steps=0;
hit_counter=0;
hit_flag=0;
average_collision_duration=0;
speed=0;
int ts_2=0, ts_3=0, ts_4=0, ts_5=0;
for(;;){
elapsed_time=0; // reset elapsed time and hit flag
ts_5 = ts_4;
ts_4 = ts_3;
ts_3 = ts_2;
ts_2 = last_ts_was_hit;
last_ts_was_hit=hit_flag; // copying the status of the last time step
if(!last_ts_was_hit) {
collision_duration=1; // if not actually colliding, always reset counter
}
hit_flag=0; // every time step
while(elapsed_time<TIME_STEP){
msl=0; msr=0;
for(sensor_nb=0;sensor_nb<NB_SENSORS;sensor_nb++){ // read sensor values and calculate motor speeds
distances[sensor_nb] = wb_distance_sensor_get_value(ps[sensor_nb]);
//printf("t=%d, distance[%d] = %f\n",time_steps,sensor_nb,distances[sensor_nb]);
msr += distances[sensor_nb] * Interconn[sensor_nb];
msl += distances[sensor_nb] * Interconn[sensor_nb + NB_SENSORS];
}
msl /= 350; msr /= 350; // Normalizing speeds
actx=gpsvalues[0];
acty=gpsvalues[2];
//printf("x = %f, y = %f\n",actx, acty);
obstacle_near=(abs(msl)>10 || abs(msr)>10);
if(obstacle_near){ // if motor speed changes (if obstacle is hit)
//printf("obstacle_near=%d msr = %d msl = %d \n", obstacle_near, msr, msl);
hit_flag=1;
f=fopen(fname,"a");
fprintf(f,"%f %f\n",
actx,
acty);
fclose(f);
} else { // measure speed only when no obstacles near
double dist = sqrt((actx-oldx)*(actx-oldx)+(acty-oldy)*(acty-oldy)); // distance traveled in 50ms
if(dist<0.01) // don't consider fast speed due to wrap around
speed=speed*0.999+0.001*dist*1000.0/50.0;
}
oldx=actx;
oldy=acty;
msl += (BIAS_SPEED);
msr += (BIAS_SPEED);
wb_differential_wheels_set_speed(3*msl,3*msr);
gpsvalues = wb_gps_get_values(gps);
wb_robot_step(50); // Executing the simulation for 50ms
elapsed_time+=50;
} // end time step
time_steps++;
if(!(last_ts_was_hit || ts_2 || ts_3 || ts_4 || ts_5) && hit_flag){// increase hit counter only for new collisions
collision_started = 1;
hit_counter++;
//printf("hit_counter=%d msr = %d msl = %d \n",hit_counter, msr, msl);
}
else if(collision_started && hit_flag){
collision_duration++; // otherwise measure duration of a collision
//printf("collision_duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
}
if(!hit_flag && collision_started){ // at the end of a collision, we calculate the running average
if(collision_duration>4){
collision_started = 0;
printf("end of collision, duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
collision_duration = collision_duration + 4;
average_collision_duration= (average_collision_duration*(hit_counter-1)+collision_duration)/hit_counter;
f=fopen("/tmp/collision_duration_webots.txt","a");
fprintf(f,"%d \n",collision_duration);
fclose(f);}
else{
//printf("collision too short = %d, thus removed \n", collision_duration);
collision_started = 0;
hit_counter--;
}
}
if(fmod(time_steps,20*60*1) == 0){ // every 1 minutes
printf("Hits: %d Time [time steps]): %d p: %f Avg. Ta [time steps]: %0.2f v (m/s):%0.3f\n",
hit_counter, time_steps,
(double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)),
average_collision_duration,
speed);
f=fopen("/tmp/collision_probability.txt","a");
fprintf(f,"%f\n", (double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)));
fclose(f);
}
}
}
int main(int argc, const char *argv[]) {
WbDeviceTag speaker, microphone;
WbDeviceTag sensors[NUM_SENSORS];
int role = UNKNOWN;
int prev_audible = -1;
int i, j; // for loops
// initialize webots
wb_robot_init();
// determine role
if (argc >= 2) {
if (! strcmp(argv[1], "speaker"))
role = SPEAKER;
else if (! strcmp(argv[1], "microphone"))
role = MICROPHONE;
}
// use speaker or microphone according to specified role
if (role == SPEAKER)
speaker = wb_robot_get_device("speaker");
else if (role == MICROPHONE) {
// we only use "mic0" in this demo
microphone = wb_robot_get_device("mic0");
wb_microphone_enable(microphone, TIME_STEP / 2);
}
// find distance sensors
for (i = 0; i < NUM_SENSORS; i++) {
char sensor_name[64];
sprintf(sensor_name, "ps%d", i);
sensors[i] = wb_robot_get_device(sensor_name);
wb_distance_sensor_enable(sensors[i], TIME_STEP);
}
// main loop
for (;;) {
if (role == SPEAKER) {
wb_speaker_emit_sample(speaker, SAMPLE, sizeof(SAMPLE));
}
else if (role == MICROPHONE) {
const short *rec_buffer = (const short *)wb_microphone_get_sample_data(microphone);
int numSamples = wb_microphone_get_sample_size(microphone) / sizeof(SAMPLE[0]);
if (rec_buffer) {
int audible = 0;
for (i = 0; i < numSamples; i++) {
// warning this demo assumes no noise and therefore depends on the sound plugin configuration !
if (rec_buffer[i] != 0)
audible = 1;
}
if (audible != prev_audible) {
printf(audible ? "I hear you now !\n" : "I can't hear you !\n");
prev_audible = audible;
}
}
else
printf("received: nothing this time ...\n");
}
// read distance sensor values
double sensor_values[NUM_SENSORS];
for (i = 0; i < NUM_SENSORS; i++)
sensor_values[i] = wb_distance_sensor_get_value(sensors[i]);
// compute braitenberg collision avoidance
double speed[2];
for (i = 0; i < 2; i++) {
speed[i] = 0.0;
for (j = 0; j < NUM_SENSORS; j++)
speed[i] += EPUCK_MATRIX[j][i] * (1 - (sensor_values[j] / RANGE));
}
// set the motors speed
wb_differential_wheels_set_speed(speed[0], speed[1]);
// simulation step
wb_robot_step(TIME_STEP);
}
return 0;
}
//
// Reset the robot controller and initiate the sensors and emitter/receiver
//
static int reset()
{
int i;
mode =1;
emitter = wb_robot_get_device("emitter");
//no emitter_enable here on purpose!
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, TIME_STEP);
char text[5]="led0";
for(i=0;i<NB_LEDS;i++) {
led[i]=wb_robot_get_device(text); // get a handler to the sensor
text[3]++; // increase the device name to "ps1", "ps2", etc.
}
text[0]='p';
text[1]='s';
text[3]='\0';
text[2]='0';
ps[0] = wb_robot_get_device(text); // proximity sensors
text[2]='7';
ps[1] = wb_robot_get_device(text); // proximity sensors
text[2]='6';
ps[2] = wb_robot_get_device(text); // proximity sensors
text[2]='5';
ps[3] = wb_robot_get_device(text);
text[2]='4';
ps[4] = wb_robot_get_device(text);
text[2]='3';
ps[5] = wb_robot_get_device(text);
text[2]='2';
ps[6] = wb_robot_get_device(text);
text[2]='1';
ps[7] = wb_robot_get_device(text);
// Enable proximity and floor sensors
for(i=0;i<NB_DIST_SENS;i++) {
wb_distance_sensor_enable(ps[i],TIME_STEP);
//printf("ps[%d] is active\n",i);
}
//get handles for all the floor sensors
text[0]='f';
text[2]='1';
fs[0]=wb_robot_get_device(text); //note that fs[0] is a 1x1 array that holds the center floor sensor DeviceTag
//enable the center fs just to try
wb_distance_sensor_enable(fs[0],TIME_STEP); //enable center floor sensor
//printf("fs[%d] is active\n",1);
//if this sensor is active increase the fitness
// open files for writing
//pF1=fopen("act_in.txt","w+");
pF2=fopen("decoded_pop.txt","w+");
// pF3=fopen("nn_values.txt","w+");
return 1;
}
////////////////////////////////////////////
// Main
int main()
{
int i, speed[2], ps_offset[NB_DIST_SENS]={0,0,0,0,0,0,0,0}, Mode=1;
/* intialize Webots */
wb_robot_init();
/* initialization */
char name[20];
for (i = 0; i < NB_LEDS; i++) {
sprintf(name, "led%d", i);
led[i] = wb_robot_get_device(name); /* get a handler to the sensor */
}
for (i = 0; i < NB_DIST_SENS; i++) {
sprintf(name, "ps%d", i);
ps[i] = wb_robot_get_device(name); /* proximity sensors */
wb_distance_sensor_enable(ps[i],TIME_STEP);
}
for (i = 0; i < NB_GROUND_SENS; i++) {
sprintf(name, "gs%d", i);
gs[i] = wb_robot_get_device(name); /* ground sensors */
wb_distance_sensor_enable(gs[i],TIME_STEP);
}
for(;;) // Main loop
{
// Run one simulation step
wb_robot_step(TIME_STEP);
// Reset all BB variables when switching from simulation to real robot and back
if (Mode!=wb_robot_get_mode())
{
oam_reset = TRUE;
llm_active = FALSE;
llm_past_side = NO_SIDE;
ofm_active = FALSE;
lem_active = FALSE;
lem_state = LEM_STATE_STANDBY;
Mode = wb_robot_get_mode();
if (Mode == SIMULATION) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i];
wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values
printf("\n\n\nSwitching to SIMULATION and reseting all BB variables.\n\n");
} else if (Mode == REALITY) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i];
wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values
printf("\n\n\nSwitching to REALITY and reseting all BB variables.\n\n");
}
}
// read sensors value
for(i=0;i<NB_DIST_SENS;i++) ps_value[i] = (((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i])<0)?0:((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i]);
for(i=0;i<NB_GROUND_SENS;i++) gs_value[i] = wb_distance_sensor_get_value(gs[i]);
// Reset all BB variables when switching from simulation to real robot and back
if (Mode!=wb_robot_get_mode())
{
oam_reset = TRUE;
llm_active = FALSE;
llm_past_side = NO_SIDE;
ofm_active = FALSE;
lem_active = FALSE;
lem_state = LEM_STATE_STANDBY;
Mode = wb_robot_get_mode();
if (Mode == SIMULATION) printf("\nSwitching to SIMULATION and reseting all BB variables.\n\n");
else if (Mode == REALITY) printf("\nSwitching to REALITY and reseting all BB variables.\n\n");
}
// Speed initialization
speed[LEFT] = 0;
speed[RIGHT] = 0;
// *** START OF SUBSUMPTION ARCHITECTURE ***
// LFM - Line Following Module
LineFollowingModule();
// Speed computation
speed[LEFT] = lfm_speed[LEFT];
speed[RIGHT] = lfm_speed[RIGHT];
// *** END OF SUBSUMPTION ARCHITECTURE ***
// Debug display
printf("OAM %d side %d \n", oam_active, oam_side);
// Set wheel speeds
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
return 0;
}
// entry point of the controller
int main(int argc, char **argv)
{
// initialize the Webots API
wb_robot_init();
// internal variables
int i;
WbDeviceTag ps[8];
char ps_names[8][4] = {
"ps0", "ps1", "ps2", "ps3",
"ps4", "ps5", "ps6", "ps7"
};
// initialize devices
for (i=0; i<8 ; i++) {
ps[i] = wb_robot_get_device(ps_names[i]);
wb_distance_sensor_enable(ps[i], TIME_STEP);
}
WbDeviceTag ls[8];
char ls_names[8][4] = {
"ls0", "ls1", "ls2", "ls3",
"ls4", "ls5", "ls6", "ls7"
};
// initialize devices
for (i=0; i<8 ; i++) {
ls[i] = wb_robot_get_device(ls_names[i]);
wb_light_sensor_enable(ls[i], TIME_STEP);
}
WbDeviceTag led[8];
char led_names[8][5] = {
"led0", "led1", "led2", "led3",
"led4", "led5", "led6", "led7"
};
// initialize devices
for (i=0; i<8 ; i++) {
led[i] = wb_robot_get_device(led_names[i]);
}
// feedback loop
while (1) {
// step simulation
int delay = wb_robot_step(TIME_STEP);
if (delay == -1) // exit event from webots
break;
// read sensors outputs
double ps_values[8];
for (i=0; i<8 ; i++)
ps_values[i] = wb_distance_sensor_get_value(ps[i]);
update_search_speed(ps_values, 250);
// set speeds
double left_speed = get_search_left_wheel_speed();
double right_speed = get_search_right_wheel_speed();
// read IR sensors outputs
double ls_values[8];
for (i=0; i<8 ; i++){
ls_values[i] = wb_light_sensor_get_value(ls[i]);
}
int active_ir = FALSE;
for(i=0; i<8; i++){
if(ls_values[i] < 2275){
active_ir = TRUE;
}
}
if(active_ir == TRUE){
swarm_retrieval(ls_values, ps_values, 2275);
left_speed = get_retrieval_left_wheel_speed();
right_speed = get_retrieval_right_wheel_speed();
}
if(is_pushing() == TRUE || stagnation == TRUE){
// check for stagnation
stagnation_counter = stagnation_counter + 1;
if(stagnation_counter == min((50 + positive_feedback * 50), 300) && stagnation == FALSE){
stagnation_counter = 0; // reset counter
stagnation_check = TRUE;
for(i=0; i<8; i++)
prev_dist_values[i] = ps_values[i];
}
if(stagnation_check == TRUE){
left_speed = 0;
right_speed = 0;
}
if(stagnation_check == TRUE && stagnation_counter == 5){
stagnation_counter = 0; // reset counter
reset_stagnation();
valuate_pushing(ps_values, prev_dist_values);
stagnation = get_stagnation_state();
stagnation_check = FALSE;
if(stagnation == TRUE)
positive_feedback = 0;
else
positive_feedback = positive_feedback + 1;
}
if(stagnation == TRUE){
stagnation_recovery(ps_values, 300);
left_speed = get_stagnation_left_wheel_speed();
right_speed = get_stagnation_right_wheel_speed();
if(get_stagnation_state() == FALSE){
reset_stagnation();
stagnation = FALSE;
stagnation_counter = 0;
}
}
}
// write actuators inputs
wb_differential_wheels_set_speed(left_speed, right_speed);
for(i=0; i<8; i++){
wb_led_set(led[i], get_LED_state(i));
}
}
// cleanup the Webots API
wb_robot_cleanup();
return 0; //EXIT_SUCCESS
}
// Main function
int main(int argc, char **argv)
{
// Initialize webots
wb_robot_init();
// GPS tick data
int red_line_tick = 0;
int green_circle_tick = 0;
// Get robot devices
WbDeviceTag left_wheel = wb_robot_get_device("left_wheel");
WbDeviceTag right_wheel = wb_robot_get_device("right_wheel");
// Get robot sensors
WbDeviceTag forward_left_sensor = wb_robot_get_device("so3");
wb_distance_sensor_enable(forward_left_sensor, TIME_STEP);
WbDeviceTag forward_right_sensor = wb_robot_get_device("so4");
wb_distance_sensor_enable(forward_right_sensor, TIME_STEP);
WbDeviceTag left_sensor = wb_robot_get_device("so1");
wb_distance_sensor_enable(left_sensor, TIME_STEP);
// Get the compass
WbDeviceTag compass = wb_robot_get_device("compass");
wb_compass_enable(compass, TIME_STEP);
// Get the GPS data
WbDeviceTag gps = wb_robot_get_device("gps");
wb_gps_enable(gps, TIME_STEP);
// Prepare robot for velocity commands
wb_motor_set_position(left_wheel, INFINITY);
wb_motor_set_position(right_wheel, INFINITY);
wb_motor_set_velocity(left_wheel, 0.0);
wb_motor_set_velocity(right_wheel, 0.0);
// Begin in mode 0, moving forward
int mode = 0;
// Main loop
while (wb_robot_step(TIME_STEP) != -1) {
// Get the sensor data
double forward_left_value = wb_distance_sensor_get_value(forward_left_sensor);
double forward_right_value = wb_distance_sensor_get_value(forward_right_sensor);
double left_value = wb_distance_sensor_get_value(left_sensor);
// Read compass and convert to angle
const double *compass_val = wb_compass_get_values(compass);
double compass_angle = convert_bearing_to_degrees(compass_val);
// Read in the GPS data
const double *gps_val = wb_gps_get_values(gps);
// Debug
printf("Sensor input values:\n");
printf("- Forward left: %.2f.\n",forward_left_value);
printf("- Forward right: %.2f.\n",forward_right_value);
printf("- Right: %.2f.\n",left_value);
printf("- Compass angle (degrees): %.2f.\n", compass_angle);
printf("- GPS values (x,z): %.2f, %.2f.\n", gps_val[0], gps_val[2]);
// Send acuator commands
double left_speed, right_speed;
left_speed = 0;
right_speed = 0;
// List the current modes
printf("Mode: %d.\n", mode);
printf("Action: ");
/*
* There are four modes for this controller.
* They are listed as below:
* 0: Finding initial correct angle
* 1: Moving forward
* 2: Wall following
* 3: Rotating after correct point
* 4: Finding green circle + moving on
*/
// If it reaches GPS coords past the red line
if (gps_val[2] > 8.0) {
// Up the GPS tick
red_line_tick = red_line_tick + 1;
}
// If the red line tick tolerance reaches
// more than 10 per cycle, begin mode 3
if (red_line_tick > 10) {
mode = 3;
}
if (mode == 0) { // Mode 0: Find correct angle
printf("Finding correct angle\n");
if (compass_angle < (DESIRED_ANGLE - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = 0;
} else if (compass_angle > (DESIRED_ANGLE + 1.0)) {
// Turn left
left_speed = 0;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
mode = 1;
}
} else if(mode == 1) { // Mode 1: Move forward
printf("Moving forward.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
// When sufficiently close to a wall in front of robot, switch mode to wall following
if ((forward_right_value > 500) || (forward_left_value > 500)) {
mode = 2;
}
}
else if (mode == 2) { // Mode 2: Wall following
if ((forward_right_value > 200) || (forward_left_value > 200)) {
printf("Backing up and turning right.\n");
left_speed = MAX_SPEED / 4.0;
right_speed = - MAX_SPEED / 2.0;
}
else {
if (left_value > 300) {
printf("Turning right away from wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
}
else {
if (left_value < 200) {
printf("Turning left towards wall.\n");
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
}
else {
printf("Moving forward along wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
}
}
} else if (mode == 3) {
// Once arrived, turn to the right
printf("Finding correct angle (again)\n");
if (compass_angle < (90 - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
} else if (compass_angle > (90 + 1.0)) {
// Turn left
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
if (green_circle_tick > 10) {
left_speed = 0;
right_speed = 0;
} else {
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
if (gps_val[0] < -3.2) {
green_circle_tick = green_circle_tick + 1;
}
}
}
// Set the motor speeds.
wb_motor_set_velocity(left_wheel, left_speed);
wb_motor_set_velocity(right_wheel, right_speed);
// Perform simple simulation step
} while (wb_robot_step(TIME_STEP) != -1);
// Clean up webots
wb_robot_cleanup();
return 0;
}
static void find_and_enable_devices() {
// camera
CameraTop = wb_robot_get_device("CameraTop");
CameraBottom = wb_robot_get_device("CameraBottom");
wb_camera_enable(CameraTop, 4 * time_step);
wb_camera_enable(CameraBottom, 4 * time_step);
// accelerometer
accelerometer = wb_robot_get_device("accelerometer");
wb_accelerometer_enable(accelerometer, time_step);
// gyro
gyro = wb_robot_get_device("gyro");
wb_gyro_enable(gyro, time_step);
// inertial unit
inertial_unit = wb_robot_get_device("inertial unit");
wb_inertial_unit_enable(inertial_unit, time_step);
// ultrasound sensors
us[0] = wb_robot_get_device("USSensor1");
us[1] = wb_robot_get_device("USSensor2");
us[2] = wb_robot_get_device("USSensor3");
us[3] = wb_robot_get_device("USSensor4");
int i;
for (i = 0; i < 4; i++)
wb_distance_sensor_enable(us[i], time_step);
// foot sensors
fsr3d[0] = wb_robot_get_device("LFsr");
wb_touch_sensor_enable(fsr3d[0], time_step);
fsr3d[1] = wb_robot_get_device("RFsr");
wb_touch_sensor_enable(fsr3d[1], time_step);
// foot bumpers
lfoot_lbumper = wb_robot_get_device("BumperLFootLeft");
lfoot_rbumper = wb_robot_get_device("BumperLFootRight");
rfoot_lbumper = wb_robot_get_device("BumperRFootLeft");
rfoot_rbumper = wb_robot_get_device("BumperRFootRight");
wb_touch_sensor_enable(lfoot_lbumper, time_step);
wb_touch_sensor_enable(lfoot_rbumper, time_step);
wb_touch_sensor_enable(rfoot_lbumper, time_step);
wb_touch_sensor_enable(rfoot_rbumper, time_step);
// There are 7 controlable LED groups in Webots
leds[0] = wb_robot_get_device("ChestBoard/Led");
leds[1] = wb_robot_get_device("RFoot/Led");
leds[2] = wb_robot_get_device("LFoot/Led");
leds[3] = wb_robot_get_device("Face/Led/Right");
leds[4] = wb_robot_get_device("Face/Led/Left");
leds[5] = wb_robot_get_device("Ears/Led/Right");
leds[6] = wb_robot_get_device("Ears/Led/Left");
// get phalanx motor tags
// the real Nao has only 2 motors for RHand/LHand
// but in Webots we must implement RHand/LHand with 2x8 motors
for (i = 0; i < PHALANX_MAX; i++) {
char name[32];
sprintf(name, "LPhalanx%d", i + 1);
lphalanx[i] = wb_robot_get_device(name);
sprintf(name, "RPhalanx%d", i + 1);
rphalanx[i] = wb_robot_get_device(name);
}
// shoulder pitch motors
RShoulderPitch = wb_robot_get_device("RShoulderPitch");
LShoulderPitch = wb_robot_get_device("LShoulderPitch");
// shoulder pitch motors
HeadYaw = wb_robot_get_device("HeadYaw");
HeadPitch = wb_robot_get_device("HeadPitch");
//LaserHead = wb_robot_get_device("URG-04LX-UG01");
//wb_camera_enable(LaserHead, 4 * time_step);
// keyboard
//wb_robot_keyboard_enable(10 * time_step);
}
