/*! \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); }