/*! \brief Stop blinking all LED
*
* This function use \ref e_destroy_agenda(void (*func)(void))
* \sa e_destroy_agenda
*/
void e_stop_led_blinking(void)
{
e_destroy_agenda(e_blink_led);
}
/*! \brief The "main" function of the program */
void run_wallfollow_hl() {
int leftwheel, rightwheel; // motor speed left and right
int distances[NB_SENSORS]; // array keeping the distance sensor readings
int i; // FOR-loop counters
int gostraight;
int loopcount;
unsigned char selector_change;
e_init_port();
e_init_ad_scan(ALL_ADC);
e_init_motors();
e_start_agendas_processing();
//follow_sensor_calibrate();
e_activate_agenda(left_led, 2500);
e_activate_agenda(right_led, 2500);
e_pause_agenda(left_led);
e_pause_agenda(right_led);
e_calibrate_ir();
loopcount=0;
selector_change = !(followgetSelectorValue_hl() & 0x0001);
check_in_corner_fw( );
while (corner_present_fw < 4) {
followGetSensorValues_hl(distances); // read sensor values
check_in_corner_fw( );
gostraight=0;
if ((followgetSelectorValue_hl() & 0x0001) == RIGHT_FOLLOW) {
if(selector_change == LEFT_FOLLOW) {
selector_change = RIGHT_FOLLOW;
e_led_clear();
e_pause_agenda(left_led);
e_restart_agenda(right_led);
}
for (i=0; i<8; i++) {
if (distances[i]>50) {
break;
}
}
if (i==8) {
gostraight=1;
} else {
follow_weightleft_hl[0]=-10;
follow_weightleft_hl[7]=-10;
follow_weightright_hl[0]=10;
follow_weightright_hl[7]=10;
if (distances[2]>300) {
distances[1]-=200;
distances[2]-=600;
distances[3]-=100;
}
}
} else {
if(selector_change == RIGHT_FOLLOW) {
selector_change = LEFT_FOLLOW;
e_led_clear();
e_pause_agenda(right_led);
e_restart_agenda(left_led);
}
for (i=0; i<8; i++) {
if (distances[i]>50) {
break;
}
}
if (i==8) {
gostraight=1;
} else {
follow_weightleft_hl[0]=10;
follow_weightleft_hl[7]=10;
follow_weightright_hl[0]=-10;
follow_weightright_hl[7]=-10;
if (distances[5]>300) {
distances[4]-=100;
distances[5]-=600;
distances[6]-=200;
}
}
}
leftwheel=BIAS_SPEED;
rightwheel=BIAS_SPEED;
if (gostraight==0) {
for (i=0; i<8; i++) {
leftwheel+=follow_weightleft_hl[i]*(distances[i]>>4);
rightwheel+=follow_weightright_hl[i]*(distances[i]>>4);
}
}
// set robot speed
followsetSpeed_hl(leftwheel, rightwheel);
wait(15000);
}
e_destroy_agenda(left_led);
e_destroy_agenda(right_led);
}
