void turn_direction(float direction)
{
int end_turn;
if (direction < PI) // turn clockwise
{
e_set_steps_left(0);
e_set_speed_left(TURN_SPEED); // motor speed in steps/s
e_set_speed_right(-TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*(direction/(2*PI)));
while(e_get_steps_left() < end_turn){
flick_led();
} // turn until done
}
else // turn counterclockwise
{
e_set_steps_right(0);
e_set_speed_left(-TURN_SPEED); // motor speed in steps/s
e_set_speed_right(TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*((2*PI-direction)/(2*PI)));
while(e_get_steps_right() < end_turn){
flick_led();
} // turn until done
}
// stop motors
e_set_speed_left(0); // motor speed in steps/s
e_set_speed_right(0); // motor speed in steps/s
}
/** Stop the motor activities
*
* Set the speed for zero and clean the step counter
*/
void stop_motors()
{
e_set_speed_right(0);
e_set_speed_left(0);
e_set_steps_right(0);
e_set_steps_left(0);
send_char(1);/* It signalize the end of operation. */
}
void send_steps()
{
int steps_right,steps_left;
steps_right = e_get_steps_right();
steps_left = e_get_steps_left();
e_set_steps_right(0);
e_set_steps_left(0);
ve_send_int(steps_right);
ve_send_int(steps_left);
}
/*! \brief The "main" function of the program */
void run_wallfollow() {
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;
int selectionValue;
// selector_change = !(followgetSelectorValue() & 0x0001);
e_set_steps_right(0);
selectionValue = followgetSelectorValue();
while (doesWallExist(selectionValue)) {
followGetSensorValues(distances); // read sensor values
gostraight=0;
if (selectionValue == 1) {
//// 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[0]=-10;
follow_weightleft[7]=-10;
follow_weightright[0]=10;
follow_weightright[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[0]=10;
follow_weightleft[7]=10;
follow_weightright[0]=-10;
follow_weightright[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[i]*(distances[i]>>4);
rightwheel+=follow_weightright[i]*(distances[i]>>4);
}
}
// set robot speed
followsetSpeed(leftwheel, rightwheel);
wait(15000);
//doesWallExist(selectionValue);
}
}
