implement setup and hypervisor

11 files changed, 43 insertions, 10 deletions

diff --git a/robot/calibration.h b/robot/calibration.h
index 13b80b9..5c1af38 100644
--- a/robot/calibration.h
+++ b/robot/calibration.h
@@ -6,4 +6,4 @@
  * turns robot on its own axis 360 degress, and aligns the front sensors with
  * the line if found, else triggers halt mode (emergency)
  */
-void w2_mode_calb_main();
+void w2_mode_calb();
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 5d2b33f..5b42bb4 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,3 +1,7 @@
 #include "errcatch.h"
 
 void w2_errcatch_main() {}
+
+void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) {}
+
+void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); }
diff --git a/robot/errcatch.h b/robot/errcatch.h
index c816051..48e2a75 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -1,4 +1,14 @@
 #pragma once
 
+#include <stdint.h>
+
+#include "consts.h"
+
 /** error-handler module main */
 void w2_errcatch_main();
+
+/** append error to error buffer */
+void w2_errcatch_throw(enum w2_e_errorcodes code);
+
+/** append error to error buffer (with debug message) */
+void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message);
diff --git a/robot/grid.h b/robot/grid.h
index 3fd54d9..fcf9100 100644
--- a/robot/grid.h
+++ b/robot/grid.h
@@ -5,4 +5,4 @@
  *
  * processes orders from the order buffer
  */
-void w2_mode_grid_main();
+void w2_mode_grid();
diff --git a/robot/halt.c b/robot/halt.c
index 6bf1cad..2f159f0 100644
--- a/robot/halt.c
+++ b/robot/halt.c
@@ -1 +1,5 @@
+#include <stdbool.h>
+
 #include "halt.h"
+
+void w2_mode_halt() { return; }
diff --git a/robot/halt.h b/robot/halt.h
index e88bde4..d92905e 100644
--- a/robot/halt.h
+++ b/robot/halt.h
@@ -5,4 +5,4 @@
  *
  * stops all execution until emergency status is manually cleared by the user
  */
-void w2_mode_halt_main();
+void w2_mode_halt();
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index db0700d..381d9af 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -1,5 +1,8 @@
-#include "hypervisor.h"
+#include <pololu/orangutan.h>
+
+#include "consts.h"
 #include "errcatch.h"
+#include "hypervisor.h"
 #include "io.h"
 #include "modes.h"
 #include "sercomm.h"
@@ -9,8 +12,8 @@ void w2_hypervisor_main() {
 	w2_errcatch_main();
 	w2_io_main();
 
-	// start timer
+	time_reset();
 	w2_modes_main();
-	// stop timer
-	// throw error if cycle expired
+	unsigned long elapsed_ms = get_ms();
+	if (elapsed_ms > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
 }
diff --git a/robot/maze.h b/robot/maze.h
index 640bf9c..9fbeb8c 100644
--- a/robot/maze.h
+++ b/robot/maze.h
@@ -5,4 +5,4 @@
  *
  * finds route out of maze
  */
-void w2_mode_maze_main();
+void w2_mode_maze();
diff --git a/robot/modes.c b/robot/modes.c
index c5b979c..c22875c 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,3 +1,6 @@
 #include "modes.h"
+#include "halt.h"
 
-void w2_modes_main() {}
+void (*g_w2_current_mode)() = &w2_mode_halt;
+
+void w2_modes_main() { (*g_w2_current_mode)(); }
diff --git a/robot/setup.c b/robot/setup.c
index 0652911..10001c7 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -1,11 +1,21 @@
 #include <pololu/orangutan.h>
+#include <stdlib.h>
 
 #include "consts.h"
+#include "halt.h"
+#include "modes.h"
 #include "setup.h"
 
 void w2_setup_main() {
 	serial_set_baud_rate(W2_SERIAL_BAUD);
 
+	// reset underside leds
+	red_led(0);
+	green_led(0);
+
+	// clear lcd
+	clear();
+
 	// indicate startup done
 	play("L50 c>c");
 }
diff --git a/robot/setup.h b/robot/setup.h
index 9d94326..17ac78d 100644
--- a/robot/setup.h
+++ b/robot/setup.h
@@ -5,7 +5,6 @@
  *
  * configures:
  * - serial connection (wixel)
- * - timer0 for cycle duration measurement
  * - lcd display
  * - underside leds
  */