void Robot::turn(Direction towards) {
uint16_t steps = STEPS::TURN;
if (towards == Directions::left(facing)) {
left_motor_backward();
right_motor_forward();
} else if (towards == Directions::right(facing)) {
left_motor_forward();
right_motor_backward();
} else if (towards == Directions::opposite(facing)) {
left_motor_forward();
right_motor_backward();
steps *= 2;
} else {
return; //don't need to turn because already facing the right way
}
for (uint16_t i = 0; i < steps; i++) {
step_motors(STEPS::DELAY);
}
facing = towards;
}
void Robot::move(Direction dir) {
turn(dir);
left_motor_forward();
right_motor_forward();
for (uint16_t i = 0; i < STEPS::CELL; i++) {
if (readings.is_wall_front_close) {
return;
}
if (readings.is_wall_left_close) {
step_left(STEPS::DELAY / 2);
} else if (readings.is_wall_right_close) {
step_right(STEPS::DELAY / 2);
}
step_motors(STEPS::DELAY);
}
}
/******************************************************************************* * Purpose: Moves the car in a right circle however many circle segments * designated by the 'numCircles' parameter. * Passed: int numCircles - number of segments of a circle to move. * Locals: No locals variables used. * Returned: No values returned. * Author: Will Flores [email protected] *******************************************************************************/ void rightCircle (int numCircles) { while (numCircles > NO_MORE_SHAPES){ set_rWheelCount(RC_RIGHTON); set_lWheelCount(RC_LEFTON); start_wheelTimers(); move_forward(); while(timerB0_started || timerA0_started) { if (!timerA0_started) { set_lWheelCount(RC_LEFTON); timerDelay(RC_SMALLDELAY); while(timerA1_started); // pause for 25 ms start_leftWheel(); left_motor_forward(); } }// end inner while loop numCircles--; } // end outer while loop return; }
