/******************************************************************************* * Purpose: Moves the car in a triangle for however many triangle segments * designated by the numTriangles parameter. * Passed: int numTriangles - number of triangles to do. * Locals: unsigned int turn - the amount of an arc to turn between segements. * unsigned int delay - the amount of delay between segments. * unsigned int tri_leg - the amount of time doing a segment. * Returned: No values returned. * Author: Will Flores [email protected] *******************************************************************************/ void triangle (int numTriangles) { unsigned int turn = 150; // orig = 200 unsigned int delay = 100; unsigned int tri_leg = 300; for(; numTriangles > NO_MORE_SHAPES; --numTriangles) { // first leg set_lWheelCount(tri_leg); set_rWheelCount(tri_leg); start_wheelTimers(); move_forward(); while(timerB0_started || timerA0_started); timerDelay(delay); // a second delay while (timerA1_started); // turn some set_rWheelCount(turn); start_rightWheel(); right_motor_forward(); while(timerB0_started || timerA0_started); timerDelay(delay); // a second delay while (timerA1_started); // next leg set_lWheelCount(tri_leg); set_rWheelCount(tri_leg); start_wheelTimers(); move_forward(); while(timerB0_started || timerA0_started); timerDelay(delay); // a second delay while (timerA1_started); // turn somemore set_rWheelCount(turn); start_rightWheel(); right_motor_forward(); while(timerB0_started || timerA0_started); timerDelay(delay); // a second delay while (timerA1_started); // home stretch set_lWheelCount(tri_leg); set_rWheelCount(tri_leg); start_wheelTimers(); move_forward(); while(timerB0_started || timerA0_started); timerDelay(delay); // a second delay while (timerA1_started); // turn a bit and done set_rWheelCount(turn); start_rightWheel(); right_motor_forward(); while(timerB0_started || timerA0_started); timerDelay(2*delay); // a 2 second delay while (timerA1_started); } return; }
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 left 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 leftCircle (int numCircles) { while (numCircles > NO_MORE_SHAPES){ set_lWheelCount(LC_LEFTON); set_rWheelCount(LC_RIGHTON); start_wheelTimers(); move_forward(); while(timerB0_started || timerA0_started) { if (!timerB0_started) { set_rWheelCount(LC_RIGHTON); timerDelay(LC_SMALLDELAY); start_rightWheel(); right_motor_forward(); } }// end inner while loop numCircles--; } // end outer while loop return; }
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;
}
