// Rotate 90 degrees clockwise n times.
void turn_right(int8_t n) {
motor_set_direction('l', 'f');
motor_set_direction('r', 'r');
OCR1B = 100;
OCR1A = 100;
right_turns = 0;
while (right_turns < 200) {
OCR1B += 100;
OCR1A += 100;
}
right_turns = 0;
kill_motors();
}
// Move n squares forward.
void move_forward(int8_t n) {
uint8_t i;
motor_set_direction('r', 'f');
motor_set_direction('l', 'f');
for (i = 0; i < n; i++) {
right_turns = 0;
left_turns = 0;
while ((right_turns < 800 || left_turns < 800) && (PINC & _BV(PINC5))) {
uint8_t dir = norm(LEFT) > norm(RIGHT) ? LEFT : RIGHT;
float error = 0.5 - norm(dir);
float k_p = 800;
if (error > 0) {
OCR1A = (int16_t) k_p * error;
OCR1B = 1023 - OCR1A;
PORTC |= _BV(PORTC3);
} else {
error = -error;
OCR1B = (int16_t) k_p * error;
OCR1A = 1023 - OCR1B;
PORTC &= ~_BV(PORTC3);
}
error = right_turns - left_turns;
k_p = 0.5;
if (error > 0) {
OCR1B -= k_p * error;
OCR1A += k_p * error;
} else {
error = -error;
OCR1A -= k_p * error;
OCR1B += k_p * error;
}
}
}
kill_motors();
}
void state_idle_init()
{
motor_set_power(0);
motor_set_enabled(1);
motor_set_direction(MOTOR_FORWARDS);
steering_set_direction(0);
steering_set_enabled(1);
lcd_set_touch_region(0, 0, 240, 320);
lcd_draw_header("IDLE");
measurer_print_info();
lcd_set_transparent_font(1);
lcd_draw_button(1, 30, 250, 0, 0, 31, 3, 31, 63, 31, 2, 2, " DRIVE ");
lcd_set_transparent_font(0);
}
void state_drive_init()
{
motor_set_power(0);
motor_set_enabled(1);
motor_set_direction(MOTOR_FORWARDS);
steering_set_direction(0);
steering_set_enabled(1);
controller_steering_reset();
controller_motor_reset();
irsens_reset();
tacho_reset();
measurer_reset();
lcd_draw_header("DRIVE");
measurer_print_info();
}
