diff options
| -rw-r--r-- | robot/mode_chrg.c | 35 | ||||
| -rw-r--r-- | robot/modes.c | 2 |
2 files changed, 19 insertions, 18 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c index c7349ab..201fbdf 100644 --- a/robot/mode_chrg.c +++ b/robot/mode_chrg.c @@ -47,13 +47,22 @@ void w2_charge_cross_walk() { } } -void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; } - // main function for charge mode void w2_mode_chrg() { static unsigned int last_proportional = 0; static long integral = 0; + if (g_w2_chrg_aligned) { + if (g_w2_target_area == W2_AREA_CHRG) return; + + delay(200); + w2_maze_rotation_full(); + w2_short_drive(); + + g_w2_chrg_aligned = false; + return; + } + g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); // The "proportional" term should be 0 when we are on the line. @@ -91,20 +100,14 @@ void w2_mode_chrg() { 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 { - delay(200); - w2_maze_rotation_full(); - w2_short_drive(); + 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(); diff --git a/robot/modes.c b/robot/modes.c index ea65e2d..80a0ee8 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -44,8 +44,6 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) { g_w2_mode_history[g_w2_mode_history_index] = new_mode; } - if (new_mode == W2_M_CHRG) w2_mode_chrg_onswitch(); - // forward mode change to sercomm W2_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin); msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; |