//Heads straight, adjusting speed based on
//whether or not a collision is imminent.
void headStraight()
{
currentState = FORWARD;
int speed = collisionPending();
if(speed == 2)
{
wb_differential_wheels_set_speed(200, 200);
wb_led_set(led4, 1);
}
else if(speed == 1)
{
wb_differential_wheels_set_speed(0, 0);
wb_led_set(led4, 1);
}
else
{
wb_differential_wheels_set_speed(MAX_SPEED, MAX_SPEED);
wb_led_set(led4, 0);
}
}
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);
}
static void reset(void)
{
int i;
char text[5]="led0";
for(i=0;i<NB_LEDS;i++) {
led[i]=wb_robot_get_device(text);
text[3]++;
wb_led_set(led[i],0);
}
// enable the camera
cam = wb_robot_get_device("camera");
wb_camera_enable(cam,TIME_STEP_CAM);
width = wb_camera_get_width(cam);
height = wb_camera_get_height(cam);
}
int main(int argc, char **argv)
{
wb_robot_init();
int i;
WbDeviceTag servos[NSERVOS];
WbDeviceTag head_led;
for (i=0; i<NSERVOS; i++)
servos[i] = wb_robot_get_device(servo_names[i]);
head_led = wb_robot_get_device("HeadLed");
wb_led_set(head_led, 0x40C040);
do {
double t = wb_robot_get_time();
for (i=0; i<6; i++)
wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]);
} while (wb_robot_step(TIME_STEP) != -1);
wb_robot_cleanup();
return 0;
}
//Turns right by setting speed
void turnRight()
{
wb_led_set(led4, 0);
currentState = TURNING;
wb_differential_wheels_set_speed(-MAX_SPEED, MAX_SPEED);
}
// 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
}
