aboutsummaryrefslogtreecommitdiff
path: root/robot/mode_chrg.c
diff options
context:
space:
mode:
Diffstat (limited to 'robot/mode_chrg.c')
-rw-r--r--robot/mode_chrg.c151
1 files changed, 75 insertions, 76 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 47391a1..c7349ab 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,4 +1,5 @@
#include "mode_chrg.h"
+#include "io.h"
#include "../shared/bool.h"
#include "hypervisor.h"
#include "mode_grid.h"
@@ -6,32 +7,34 @@
#include "movement.h"
#include "orangutan_shim.h"
+#include <stdio.h>
+
bool g_w2_chrg_aligned;
void w2_short_drive() {
- set_motors(50, 50);
+ w2_set_motors(50, 50);
delay(150);
- set_motors(0, 0);
+ w2_set_motors(0, 0);
}
// crosswalk from charging station back to maze
void w2_charge_cross_walk() {
if (g_w2_transition == 0) {
- set_motors(-30, 30);
+ w2_set_motors(-30, 30);
delay(50);
}
while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
- set_motors(15, 15);
+ w2_set_motors(15, 15);
delay(550);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) {
- set_motors(0, 0);
+ w2_set_motors(0, 0);
g_w2_transition++;
if (g_w2_transition == 3) { // TODO: document g_w2_transition
- set_motors(40, 40);
+ w2_set_motors(40, 40);
delay(600);
- set_motors(0, 0);
+ w2_set_motors(0, 0);
g_w2_transition = 0;
w2_modes_swap(W2_M_MAZE);
@@ -48,80 +51,76 @@ void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; }
// main function for charge mode
void w2_mode_chrg() {
- unsigned int last_proportional = 0;
- long integral = 0;
-
- while (1) {
- // Get the position of the line. Note that we *must* provide
- // the "sensors" argument to read_line() here, even though we
- // are not interested in the individual sensor readings.
- g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-
- // The "proportional" term should be 0 when we are on the line.
- int proportional = ((int)g_w2_position) - 2000;
-
- // Compute the derivative (change) and integral (sum) of the
- // position.
- int derivative = proportional - last_proportional;
- integral += proportional;
-
- // Remember the last position.
- last_proportional = proportional;
-
- // Compute the difference between the two motor power settings,
- // m1 - m2. If this is a positive number the robot will turn
- // to the right. If it is a negative number, the robot will
- // turn to the left, and the magnitude of the number determines
- // the sharpness of the turn.
- int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2;
-
- // Compute the actual motor settings. We never set either motor
- // to a negative value.
-
- const int max = 60;
- if (power_difference > max) power_difference = max;
- if (power_difference < -max) power_difference = -max;
-
- if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
- g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
- w2_charge_cross_walk();
- if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
- break;
- }
+ static unsigned int last_proportional = 0;
+ static long integral = 0;
+
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+
+ // The "proportional" term should be 0 when we are on the line.
+ int proportional = ((int)g_w2_position) - 2000;
+
+ // Compute the derivative (change) and integral (sum) of the
+ // position.
+ int derivative = proportional - last_proportional;
+ integral += proportional;
+
+ // Remember the last position.
+ last_proportional = proportional;
+
+ // Compute the difference between the two motor power settings,
+ // m1 - m2. If this is a positive number the robot will turn
+ // to the right. If it is a negative number, the robot will
+ // turn to the left, and the magnitude of the number determines
+ // the sharpness of the turn.
+ int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2;
+
+ // Compute the actual motor settings. We never set either motor
+ // to a negative value.
+
+ const int max = 60;
+ if (power_difference > max) power_difference = max;
+ if (power_difference < -max) power_difference = -max;
+
+ if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
+ g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
+ w2_charge_cross_walk();
+ if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
+ return;
}
+ }
- else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
- g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) {
- if (g_w2_target_area == W2_AREA_CHRG) {
- if (!g_w2_chrg_aligned) {
- set_motors(0, 0);
- delay_ms(150);
- set_motors(30, 30);
- delay_ms(600);
- set_motors(0, 0);
- delay_ms(600);
- g_w2_chrg_aligned = true;
- }
- } else {
- delay(200);
- w2_maze_rotation_full();
- w2_short_drive();
+ else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
+ g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) {
+ if (g_w2_target_area == W2_AREA_CHRG) {
+ if (!g_w2_chrg_aligned) {
+ w2_set_motors(0, 0);
+ delay_ms(150);
+ w2_set_motors(30, 30);
+ delay_ms(600);
+ w2_set_motors(0, 0);
+ delay_ms(600);
+ g_w2_chrg_aligned = true;
}
- } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
- w2_maze_rotation_half_left();
+ } else {
+ delay(200);
+ w2_maze_rotation_full();
+ w2_short_drive();
}
+ } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+ w2_maze_rotation_half_left();
+ }
- else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
- g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
- w2_maze_rotation_half_left();
- } else {
- if (power_difference < 0 &&
- (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
- set_motors(max + power_difference, max);
- } else if (power_difference > 0 &&
- (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
- set_motors(max, max - power_difference);
- }
+ else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
+ g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
+ w2_maze_rotation_half_left();
+ } else {
+ if (power_difference < 0 &&
+ (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
+ w2_set_motors(max + power_difference, max);
+ } else if (power_difference > 0 &&
+ (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
+ w2_set_motors(max, max - power_difference);
}
}
}
+