void ahead(void)
{
reset_counter(LEFT);
reset_counter(RIGHT);
while(read_encoder_counter(RIGHT) < MOVE_DURATION && read_encoder_counter(LEFT) < MOVE_DURATION)
{
ahead_R();
ahead_L();
control();
}
if (read_encoder_counter(RIGHT) < MOVE_DURATION )
{
stop_motor_L();
while (read_encoder_counter(RIGHT) < MOVE_DURATION )
{
ahead_R();
}
stop_motor_R();
}
else if (read_encoder_counter(LEFT) < MOVE_DURATION )
{
stop_motor_R();
while (read_encoder_counter(LEFT) < MOVE_DURATION )
{
ahead_L();
}
stop_motor_L();
}
}
void turn_left(void)
{
reset_counter(LEFT);
reset_counter(RIGHT);
while(read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) && read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5))
{
ahead_R();
back_L();
control();
}
if (read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) )
{
stop_motor_L();
while (read_encoder_counter(RIGHT) < (int)(QUARTER_TURN+0.5) )
{
ahead_R();
}
stop_motor_R();
}
else if (read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5) )
{
stop_motor_R();
while (read_encoder_counter(LEFT) < (int)(QUARTER_TURN-0.5) )
{
back_L();
}
stop_motor_L();
}
}
void update_powers()
{
if (pow_right > 0)
{
ahead_R(pow_right);
}
else if (pow_right < 0)
{
back_R(-pow_right);
}
else if (pow_right == 0)
{
stop_motor_R();
}
if (pow_left >= 0)
{
ahead_L(pow_left);
}
else if (pow_left < 0)
{
back_L(-pow_left);
}
else if (pow_left == 0)
{
stop_motor_L();
}
}