fix charging station wait until full

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;