// 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(); }