4 files changed, 34 insertions, 22 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index c41c0f7..47391a1 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,10 +1,12 @@
 #include "mode_chrg.h"
+#include "../shared/bool.h"
+#include "hypervisor.h"
 #include "mode_grid.h"
 #include "modes.h"
 #include "movement.h"
 #include "orangutan_shim.h"
 
-int g_w2_charged_status; // used to detect the charging station (once)
+bool g_w2_chrg_aligned;
 
 void w2_short_drive() {
 	set_motors(50, 50);
@@ -12,19 +14,6 @@ void w2_short_drive() {
 	set_motors(0, 0);
 }
 
-// charging station
-void w2_home() {
-	set_motors(0, 0);
-	delay_ms(150);
-	set_motors(30, 30);
-	delay_ms(600);
-	set_motors(0, 0);
-	delay_ms(600);
-	g_w2_position		= read_line(g_w2_sensors, IR_EMITTERS_ON);
-	g_w2_charged_status = 1;
-	delay_ms(2000);
-}
-
 // crosswalk from charging station back to maze
 void w2_charge_cross_walk() {
 	if (g_w2_transition == 0) {
@@ -55,6 +44,8 @@ void w2_charge_cross_walk() {
 	}
 }
 
+void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; }
+
 // main function for charge mode
 void w2_mode_chrg() {
 	unsigned int last_proportional = 0;
@@ -99,13 +90,23 @@ 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) &&
-				 g_w2_charged_status == 0) {
-			w2_home();
-			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) {
+					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] >= 200 && g_w2_sensors[4] < 100) {
 			w2_maze_rotation_half_left();
 		}
diff --git a/robot/mode_chrg.h b/robot/mode_chrg.h
index a870e58..1015813 100644
--- a/robot/mode_chrg.h
+++ b/robot/mode_chrg.h
@@ -9,3 +9,6 @@
  * black circle is found
  */
 void w2_mode_chrg();
+
+/** called when mode is switched to charge station mode */
+void w2_mode_chrg_onswitch();
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 7e6db49..cfecae5 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,8 +1,13 @@
 #include "mode_grid.h"
+#include "hypervisor.h"
 #include "modes.h"
 #include "movement.h"
 #include "orangutan_shim.h"
 
+/**
+ * TODO: mode_grid g_w2_target_area laten volgen
+ */
+
 int g_w2_order_number;
 
 int g_w2_maze_status = 0;
@@ -30,7 +35,8 @@ void w2_crosswalk_stroll() {
 		set_motors(15, 15);
 		delay(290);
 		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) {
+		if ((g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) &&
+			(g_w2_target_area != W2_AREA_MAZE)) {
 			set_motors(0, 0);
 			g_w2_transition++;
 			if (g_w2_transition == 3) {
diff --git a/robot/modes.c b/robot/modes.c
index 559d81c..a54bc43 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -44,6 +44,8 @@ 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;