void loop()
{
// Set LED green.
set_color(RGB(0, 1, 0));
// Spinup motors to overcome friction.
spinup_motors();
// Move straight for 2 seconds (2000 ms).
set_motors(kilo_straight_left, kilo_straight_right);
delay(2000);
// Set LED red.
set_color(RGB(1, 0, 0));
// Spinup motors to overcome friction.
spinup_motors();
// Turn left for 2 seconds (2000 ms).
set_motors(kilo_turn_left, 0);
delay(2000);
// Set LED blue.
set_color(RGB(0, 0, 1));
// Spinup motors to overcome friction.
spinup_motors();
// Turn right for 2 seconds (2000 ms).
set_motors(0, kilo_turn_right);
delay(2000);
// Set LED off.
set_color(RGB(0, 0, 0));
// Stop for half a second (500 ms).
set_motors(0, 0);
delay(500);
}
/*-------------------------------------------------------------------*/
void set_motion( motion_t new_motion_type ) {
if( current_motion_type != new_motion_type ){
switch( new_motion_type ) {
case FORWARD:
spinup_motors();
set_motors(kilo_straight_left,kilo_straight_right);
break;
case TURN_LEFT:
spinup_motors();
set_motors(kilo_turn_left,0);
break;
case TURN_RIGHT:
spinup_motors();
set_motors(0,kilo_turn_right);
break;
case STOP:
default:
set_motors(0,0);
}
current_motion_type = new_motion_type;
}
}
void loop()
{
printf("kilo_ticks = %u\n", kilo_ticks);
// Update the state every 64 clock ticks (roughly 2 seconds).
if (kilo_ticks > (last_state_update + 64))
{
last_state_update = kilo_ticks;
if (state == FORWARD_GREEN)
{
set_color(RGB(0, 1, 0));
spinup_motors();
set_motors(kilo_straight_left, kilo_straight_right);
// Set the next state.
state = LEFT_RED;
}
else if (state == LEFT_RED)
{
set_color(RGB(1, 0, 0));
spinup_motors();
set_motors(kilo_turn_left, 0);
// Set the next state.
state = RIGHT_BLUE;
}
else if (state == RIGHT_BLUE)
{
set_color(RGB(0, 0, 1));
spinup_motors();
set_motors(0, kilo_turn_right);
// Set the next state.
state = FORWARD_GREEN;
}
}
}
void goForward (){
spinup_motors();
set_motors(kilo_straight_left, kilo_straight_right);
set_color (RGB(0,0,1));
}
void goRight (){
spinup_motors();
set_motors(0, kilo_turn_right);
set_color (RGB(0,0,1));
}
void goLeft (){
spinup_motors();
set_motors(kilo_turn_left, 0);
set_color (RGB(1,0,0));
}
