scal and spin done, removed lcal

5 files changed, 19 insertions, 16 deletions

diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index f3178d7..53cbf67 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -1,19 +1,20 @@
 #include "mode_scal.h"
+#include "modes.h"
+#include "orangutan_shim.h"
 
 void w2_mode_scal() {
-	// TODO ???
-	/* pololu_3pi_init(2000);
+	pololu_3pi_init(2000);
 	for (int counter = 0; counter < 80; counter++) {
 		if (counter < 20 || counter >= 60) {
-			g_w2_io.motor_left.speed  = 40;
-			g_w2_io.motor_right.speed = -40;
+			set_motors(40, -40);
 		} else {
-			g_w2_io.motor_left.speed  = -40;
-			g_w2_io.motor_right.speed = 40;
+			set_motors(-40, 40);
 		}
 
 		calibrate_line_sensors(IR_EMITTERS_ON);
 
-		delay_ms(20); // TODO foei
-	} */
+		delay_ms(20);
+	}
+
+	w2_modes_call(W2_M_PREV);
 }
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
index 9145eb3..9ee83b0 100644
--- a/robot/mode_spin.c
+++ b/robot/mode_spin.c
@@ -1,3 +1,4 @@
 #include "mode_spin.h"
+#include "orangutan_shim.h"
 
-void w2_mode_spin() {}
+void w2_mode_spin() { set_motors(255, -255); }
diff --git a/robot/modes.c b/robot/modes.c
index 7decf47..fd062a8 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -22,7 +22,6 @@ void w2_modes_init() {
 	g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
 	g_w2_modes[W2_M_GRID] = &w2_mode_grid;
 	g_w2_modes[W2_M_HALT] = &w2_mode_halt;
-	g_w2_modes[W2_M_LCAL] = &w2_mode_lcal;
 	g_w2_modes[W2_M_MAZE] = &w2_mode_maze;
 	g_w2_modes[W2_M_SCAL] = &w2_mode_scal;
 	g_w2_modes[W2_M_SPIN] = &w2_mode_spin;
diff --git a/robot/readme.md b/robot/readme.md
index f54af21..e8316e3 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -50,8 +50,7 @@ organizational and form more of a software 'skeleton', while the 'maze' and
                                                           Maze ─┤
                                                      Warehouse ─┤
                                                 Emergency stop ─┤
-                        *logic modes* ->          Line finding ─┤
-                                                Charge station ─┤
+                          *logic modes* ->      Charge station ─┤
                                                 Direct control ─┤
                                                      Wet floor ─┤
                                             Sensor calibration ─┘
@@ -74,11 +73,10 @@ what they're supposed to do:
 |maze              |`mode_maze  `|done|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module|
 |warehouse         |`mode_grid  `|may 31|Loek| controls robot during warehouse portion of map; hands off control to maze module|
 |emergency stop    |`mode_halt  `|done|Fiona| stops all execution until emergency mode is reset by software or user|
-|line finding      |`mode_lcal  `|may 31|Fiona| find line by turning on own axis if lost|
 |charge station    |`mode_chrg  `|may 31|Fiona| go to the charging station transition in the grid, and continue until a black circle is found|
 |direct control    |`mode_dirc  `|done|Loek| respond to [DIRC](../protocol.md#DIRC) commands|
-|wet floor         |`mode_spin  `|may 31|Fiona| spin uncontrollably (simulating wet floor??)|
-|sensor calibration|`mode_scal  `|may 31|Jorn & Abdullaahi| calibrate underside uv sensors|
+|wet floor         |`mode_spin  `|done|Fiona| spin uncontrollably (simulating wet floor??)|
+|sensor calibration|`mode_scal  `|done|Jorn & Abdullaahi| calibrate underside uv sensors|
 
 ## some standards
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index c50dd15..f52eb7e 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -99,7 +99,11 @@ void w2_cmd_ping_rx(w2_s_bin *data) { w2_sercomm_append_msg(data); }
 
 void w2_cmd_mode_rx(w2_s_bin *data) {
 	W2_CAST_BIN(w2_s_cmd_mode_rx, data, req);
-	w2_modes_swap(req->mode);
+	if (req->mode == W2_M_SCAL) {
+		w2_modes_call(req->mode);
+	} else {
+		w2_modes_swap(req->mode);
+	}
 }
 
 void w2_cmd_sped_rx(w2_s_bin *data) { return; }