merge dev into master

44 files changed, 545 insertions, 128 deletions

diff --git a/robot/calibration.c b/robot/calibration.c
deleted file mode 100644
index 7788532..0000000
--- a/robot/calibration.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "calibration.h"
diff --git a/robot/consts.h b/robot/consts.h
deleted file mode 100644
index a81908d..0000000
--- a/robot/consts.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#ifndef W2_BUILD_STR
-// is defined by CFLAGS += -DW2_BUILD_STR in makefile
-#define W2_BUILD_STR ("????????")
-#endif
-
-#define W2_MAX_MODULE_CYCLE_MS (20)
-#define W2_SERIAL_BAUD (9600)
-#define W2_ERROR_BUFFER_SIZE (16)
-
-#define W2_ERR_TYPE_CRIT (0b00 << 6)
-#define W2_ERR_TYPE_WARN (0b01 << 6)
-#define W2_ERR_TYPE_INFO (0b10 << 6)
-#define W2_ERR_TYPE_VERB (0b11 << 6)
-#define W2_ERR_TYPE_MASK (0b11 << 6)
-
-/**
- * enum storing all error codes
- *
- * error codes are between 0-63 because the two most significant bits are
- * reserved for error type checking
- */
-enum w2_e_errorcodes {
-	// critical error codes
-	W2_ERR_CONN_LOST	   = 0x00 | W2_ERR_TYPE_CRIT,
-	W2_ERR_COM_UNAVAILABLE = 0x01 | W2_ERR_TYPE_CRIT, // client-only
-	W2_ERR_LINE_LOST	   = 0x02 | W2_ERR_TYPE_CRIT,
-	W2_ERR_OBSTACLE_STUCK  = 0x03 | W2_ERR_TYPE_CRIT,
-
-	// warnings
-	W2_ERR_BATTERY_LOW		 = 0x00 | W2_ERR_TYPE_WARN,
-	W2_ERR_OBSTACLE_DETECTED = 0x01 | W2_ERR_TYPE_WARN,
-	W2_ERR_CYCLE_EXPIRED	 = 0x02 | W2_ERR_TYPE_WARN,
-	W2_ERR_UNCAUGHT_ERROR	 = 0x03 | W2_ERR_TYPE_WARN,
-};
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 0dc60e9..8df90b8 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,61 +1,82 @@
-#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
-#include "consts.h"
 #include "errcatch.h"
-#include "halt.h"
 #include "modes.h"
 #include "orangutan_shim.h"
+#include "sercomm.h"
 
 w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {};
 uint8_t g_w2_error_index							= 0;
 uint8_t g_w2_error_offset							= 0;
+uint8_t g_w2_error_buffer_full						= 0;
+uint8_t g_w2_error_uncaught							= 0;
 
 void w2_errcatch_main() {
-	while (g_w2_error_index != g_w2_error_offset) {
+	while (g_w2_error_offset != g_w2_error_index) {
 		w2_s_error *error = g_w2_error_buffer[g_w2_error_offset];
 		w2_errcatch_handle_error(error);
 		g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
 	}
+	if (g_w2_error_buffer_full) {
+		w2_errcatch_throw(W2_E_WARN_ERR_BUFFER_FULL);
+		g_w2_error_buffer_full = 0;
+	}
+	if (g_w2_error_uncaught) {
+		w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR);
+		g_w2_error_uncaught = 0;
+	}
 }
 
-w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message) {
-	w2_s_error *error = calloc(sizeof(w2_s_error) + length, 1);
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message) {
+	w2_s_error *error = malloc(sizeof(w2_s_error) + length);
 	memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error));
 	strncpy(error->message, message, length);
 
 	return error;
 }
 
-void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); }
-void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) {
+void w2_errcatch_throw(w2_e_errorcode code) { w2_errcatch_throw_msg(code, 0, ""); }
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message) {
+	uint8_t next_index	   = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
+	g_w2_error_buffer_full = next_index == g_w2_error_offset;
 	free(g_w2_error_buffer[g_w2_error_index]);
 	w2_s_error *error					= w2_alloc_error(code, length, message);
 	g_w2_error_buffer[g_w2_error_index] = error;
-	g_w2_error_index					= (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
+	if (g_w2_error_buffer_full) return;
+	g_w2_error_index = next_index;
 }
 
 void w2_errcatch_handle_error(w2_s_error *error) {
-	uint8_t severity = error->code & W2_ERR_TYPE_MASK;
+	uint8_t severity = error->code & W2_E_TYPE_MASK;
 
 	// trigger emergency mode for critical errors
-	if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt;
+	if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT);
 
 	// TODO: handle more error types
 	switch (error->code) {
-		case W2_ERR_UNCAUGHT_ERROR: {
+		case W2_E_WARN_UNCAUGHT_ERROR: {
 			break;
 		}
 		default: {
-			w2_errcatch_throw(W2_ERR_UNCAUGHT_ERROR);
+			g_w2_error_uncaught = 1;
 #ifdef W2_SIM
 			simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code);
 #endif
 		}
 	}
 
-	// TODO: forward error to sercomm
+	// forward error to sercomm
+	size_t msg_size		  = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length;
+	w2_s_cmd_expt_tx *msg = malloc(msg_size);
+	msg->opcode			  = W2_CMD_EXPT | W2_CMDDIR_TX;
+	msg->error			  = error->code;
+	msg->length			  = error->message_length;
+	memcpy(msg->message, error->message, error->message_length);
+	w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+	w2_sercomm_append_msg(msg_bin);
+	free(msg);
+	free(msg_bin);
 
 	return;
 }
diff --git a/robot/errcatch.h b/robot/errcatch.h
index fa7a15b..836da1b 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -2,19 +2,8 @@
 
 #include <stdint.h>
 
-#include "consts.h"
-
-/**
- * error struct
- *
- * holds an error with type `code`, and an optional `message` with length
- * `message_length`
- */
-typedef struct {
-	enum w2_e_errorcodes code;
-	uint8_t message_length;
-	char message[];
-} w2_s_error;
+#include "../shared/consts.h"
+#include "../shared/errors.h"
 
 /** error ring buffer */
 extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE];
@@ -30,14 +19,14 @@ void w2_errcatch_main();
 void w2_errcatch_handle_error(w2_s_error *error);
 
 /** append error to error buffer */
-void w2_errcatch_throw(enum w2_e_errorcodes code);
+void w2_errcatch_throw(w2_e_errorcode 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);
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message);
 
 /**
  * allocate and initialize error struct
  *
- * TODO: doesn't handle null pointers from calloc
+ * TODO: doesn't handle null pointers from malloc
  */
-w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message);
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message);
diff --git a/robot/grid.c b/robot/grid.c
deleted file mode 100644
index 0c83272..0000000
--- a/robot/grid.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "grid.h"
diff --git a/robot/halt.c b/robot/halt.c
deleted file mode 100644
index 2f159f0..0000000
--- a/robot/halt.c
+++ /dev/null
@@ -1,5 +0,0 @@
-#include <stdbool.h>
-
-#include "halt.h"
-
-void w2_mode_halt() { return; }
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 6b32776..0baa406 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -1,12 +1,25 @@
 #include "hypervisor.h"
-#include "consts.h"
+#include "../shared/util.h"
 #include "errcatch.h"
 #include "io.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
+uint64_t g_w2_hypervisor_cycles				  = 0;
+uint64_t g_w2_hypervisor_uptime_ms			  = 0;
+unsigned long g_w2_hypervisor_ema_sercomm_ms  = 0;
+unsigned long g_w2_hypervisor_ema_errcatch_ms = 0;
+unsigned long g_w2_hypervisor_ema_io_ms		  = 0;
+unsigned long g_w2_hypervisor_ema_mode_ms	  = 0;
+
 void w2_hypervisor_main() {
+#ifdef W2_SIM
+	w2_sim_cycle_begin();
+	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n");
+#endif
+
+	g_w2_hypervisor_uptime_ms += get_ms();
 	time_reset();
 
 	w2_sercomm_main();
@@ -18,14 +31,23 @@ void w2_hypervisor_main() {
 	w2_modes_main();
 	unsigned long mode_time = get_ms() - io_time;
 
+	// calculate exponential moving averages
+	g_w2_hypervisor_ema_sercomm_ms =
+		w2_util_exp_mov_avg(g_w2_hypervisor_ema_sercomm_ms, sercomm_time);
+	g_w2_hypervisor_ema_errcatch_ms =
+		w2_util_exp_mov_avg(g_w2_hypervisor_ema_errcatch_ms, errcatch_time);
+	g_w2_hypervisor_ema_io_ms	= w2_util_exp_mov_avg(g_w2_hypervisor_ema_io_ms, io_time);
+	g_w2_hypervisor_ema_mode_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_mode_ms, mode_time);
+
+	if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED);
+
 #ifdef W2_SIM
-	siminfo("sercomm:  %lums\n", sercomm_time);
-	siminfo("errcatch: %lums\n", errcatch_time);
-	siminfo("io:       %lums\n", io_time);
-	siminfo("mode:     %lums\n", mode_time);
+	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
+	if (!g_w2_sim_headless) usleep(100e3);
 
-	usleep(100e3);
+	if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES)
+		exit(0);
 #endif
 
-	if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
+	g_w2_hypervisor_cycles++;
 }
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index 0e73259..5008c8f 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -1,5 +1,15 @@
 #pragma once
 
+#include <stdint.h>
+
+extern uint64_t g_w2_hypervisor_cycles;
+extern uint64_t g_w2_hypervisor_uptime_ms;
+
+extern unsigned long g_w2_hypervisor_ema_sercomm_ms;
+extern unsigned long g_w2_hypervisor_ema_errcatch_ms;
+extern unsigned long g_w2_hypervisor_ema_io_ms;
+extern unsigned long g_w2_hypervisor_ema_mode_ms;
+
 /**
  * backbone of all other modules
  *
diff --git a/robot/main.c b/robot/main.c
index fbfd38b..d76dbaf 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -1,8 +1,15 @@
 #include "main.h"
 #include "hypervisor.h"
 #include "setup.h"
+#ifdef W2_SIM
+#include "sim.h"
+#endif
+
+int main(int argc, char **argv) {
+#ifdef W2_SIM
+	w2_sim_setup(argc, argv);
+#endif
 
-int main() {
 	w2_setup_main();
 
 	for (;;) w2_hypervisor_main();
diff --git a/robot/makefile b/robot/makefile
index 6f50519..a6e96ee 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -10,11 +10,14 @@ PORT ?= /dev/ttyACM0
 CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os
 LDFLAGS=-Wl,-gc-sections -Wl,-relax
 
+all: $(if $(SIM), a.out, out.hex)
+
 SOURCES := $(filter-out sim.c, $(wildcard *.c))
 HEADERS := $(filter-out sim.h, $(wildcard *.h))
+include ../shared/makefile
 
 # simulation
-# SIM = true
+SIM = true
 CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU))
 LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE))
 PREFIX := $(if $(SIM), , avr-)
@@ -29,12 +32,10 @@ OBJ2HEX=$(PREFIX)objcopy
 
 # debug build info string
 BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20)
-CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)"
-
-all: $(if $(SIM), a.out, out.hex)
+CFLAGS += -DW2_BUILD_STR=\"$(BUILD_STR)\"
 
-clean:
-	rm -f *.o out.hex a.out compile_commands.json
+clean::
+	rm -f *.o out.hex a.out
 
 a.out: $(OBJECTS)
 	$(CC) $(OBJECTS) $(CFLAGS) $(LDFLAGS)
diff --git a/robot/maze.c b/robot/maze.c
deleted file mode 100644
index a27414f..0000000
--- a/robot/maze.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "maze.h"
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
new file mode 100644
index 0000000..81808d5
--- /dev/null
+++ b/robot/mode_chrg.c
@@ -0,0 +1,3 @@
+#include "mode_chrg.h"
+
+void w2_mode_chrg() {}
diff --git a/robot/mode_chrg.h b/robot/mode_chrg.h
new file mode 100644
index 0000000..d9b5cc0
--- /dev/null
+++ b/robot/mode_chrg.h
@@ -0,0 +1,9 @@
+#pragma once
+
+/**
+ * charge station mode
+ *
+ * go to the charging station transition in the grid, and continue until a
+ * black circle is found
+ */
+void w2_mode_chrg();
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
new file mode 100644
index 0000000..0bbf3cb
--- /dev/null
+++ b/robot/mode_dirc.c
@@ -0,0 +1,7 @@
+#include "mode_dirc.h"
+#include "orangutan_shim.h"
+
+int16_t g_w2_mode_dirc_motor_l = 0;
+int16_t g_w2_mode_dirc_motor_r = 0;
+
+void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); }
diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h
new file mode 100644
index 0000000..5b9bbf4
--- /dev/null
+++ b/robot/mode_dirc.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <stdint.h>
+
+extern int16_t g_w2_mode_dirc_motor_l;
+extern int16_t g_w2_mode_dirc_motor_r;
+
+/**
+ * direct control mode
+ *
+ * respond to DIRC commands
+ */
+void w2_mode_dirc();
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
new file mode 100644
index 0000000..8526499
--- /dev/null
+++ b/robot/mode_grid.c
@@ -0,0 +1,3 @@
+#include "mode_grid.h"
+
+void w2_mode_grid() {}
diff --git a/robot/grid.h b/robot/mode_grid.h
index fcf9100..fcf9100 100644
--- a/robot/grid.h
+++ b/robot/mode_grid.h
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
new file mode 100644
index 0000000..88d6183
--- /dev/null
+++ b/robot/mode_halt.c
@@ -0,0 +1,3 @@
+#include "mode_halt.h"
+
+void w2_mode_halt() { return; }
diff --git a/robot/halt.h b/robot/mode_halt.h
index d92905e..d92905e 100644
--- a/robot/halt.h
+++ b/robot/mode_halt.h
diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c
new file mode 100644
index 0000000..6b4e736
--- /dev/null
+++ b/robot/mode_lcal.c
@@ -0,0 +1 @@
+#include "mode_lcal.h"
diff --git a/robot/calibration.h b/robot/mode_lcal.h
index 5c1af38..dd373f0 100644
--- a/robot/calibration.h
+++ b/robot/mode_lcal.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();
+void w2_mode_lcal();
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
new file mode 100644
index 0000000..6ae62be
--- /dev/null
+++ b/robot/mode_maze.c
@@ -0,0 +1,3 @@
+#include "mode_maze.h"
+
+void w2_mode_maze() {}
diff --git a/robot/maze.h b/robot/mode_maze.h
index 9fbeb8c..9fbeb8c 100644
--- a/robot/maze.h
+++ b/robot/mode_maze.h
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
new file mode 100644
index 0000000..e3a9c97
--- /dev/null
+++ b/robot/mode_scal.c
@@ -0,0 +1,3 @@
+#include "mode_scal.h"
+
+void w2_mode_scal() {}
diff --git a/robot/mode_scal.h b/robot/mode_scal.h
new file mode 100644
index 0000000..4f1f04d
--- /dev/null
+++ b/robot/mode_scal.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * sensor calibration mode
+ *
+ * calibrate underside uv sensors
+ */
+void w2_mode_scal();
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
new file mode 100644
index 0000000..9145eb3
--- /dev/null
+++ b/robot/mode_spin.c
@@ -0,0 +1,3 @@
+#include "mode_spin.h"
+
+void w2_mode_spin() {}
diff --git a/robot/mode_spin.h b/robot/mode_spin.h
new file mode 100644
index 0000000..926137e
--- /dev/null
+++ b/robot/mode_spin.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * wet floor simulation
+ *
+ * spin uncontrollably (simulating wet floor??)
+ */
+void w2_mode_spin();
diff --git a/robot/modes.c b/robot/modes.c
index c22875c..8d3a099 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,6 +1,44 @@
+#include <stdbool.h>
+
+#include "../shared/util.h"
+#include "errcatch.h"
 #include "modes.h"
-#include "halt.h"
+#include "sercomm.h"
+
+/** function pointer to current mode */
+// static void (*g_w2_current_mode)() = &w2_mode_halt;
+
+static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])();
+static uint8_t g_w2_mode_history_index = 0;
+
+void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); }
+
+void w2_modes_switch(w2_e_mode new_mode, bool replace) {
+	int16_t next_history_index =
+		g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1);
+	if (next_history_index == -1 || next_history_index == W2_MODE_HISTORY_BUFFER_SIZE - 1) {
+		next_history_index = W2_RANGE(0, next_history_index, W2_MODE_HISTORY_BUFFER_SIZE);
+		w2_errcatch_throw(W2_E_WARN_MODE_HISTORY_BUFFER_IOB);
+	}
+
+	if (new_mode == W2_M_PREV) {
+		g_w2_mode_history_index = next_history_index;
+	} else {
+		g_w2_mode_history_index					   = next_history_index;
+		g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode];
+	}
+
+	// forward mode change to sercomm
+	size_t msg_size		  = sizeof(w2_s_cmd_mode_tx);
+	w2_s_cmd_mode_tx *msg = malloc(msg_size);
+	msg->opcode			  = W2_CMD_MODE | W2_CMDDIR_TX;
+	msg->mode			  = (uint8_t)new_mode;
+	w2_s_bin *msg_bin	  = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+	w2_sercomm_append_msg(msg_bin);
+	free(msg);
+	free(msg_bin);
+}
 
-void (*g_w2_current_mode)() = &w2_mode_halt;
+void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); }
 
-void w2_modes_main() { (*g_w2_current_mode)(); }
+void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); }
diff --git a/robot/modes.h b/robot/modes.h
index dd34690..3423a0f 100644
--- a/robot/modes.h
+++ b/robot/modes.h
@@ -1,6 +1,15 @@
 #pragma once
 
-extern void (*g_w2_current_mode)();
+#include "../shared/consts.h"
+
+#include "mode_chrg.h"
+#include "mode_dirc.h"
+#include "mode_grid.h"
+#include "mode_halt.h"
+#include "mode_lcal.h"
+#include "mode_maze.h"
+#include "mode_scal.h"
+#include "mode_spin.h"
 
 /**
  * mode logic
@@ -8,3 +17,27 @@ extern void (*g_w2_current_mode)();
  * executes mode in g_w2_current_mode
  */
 void w2_modes_main();
+
+/** mode constants */
+typedef enum {
+	W2_M_PREV = -1,
+	W2_M_MAZE = 0,
+	W2_M_GRID = 1,
+	W2_M_HALT = 2,
+	W2_M_LCAL = 3,
+	W2_M_CHRG = 4,
+	W2_M_DIRC = 5,
+	W2_M_SPIN = 6,
+	W2_M_SCAL = 7,
+} w2_e_mode;
+
+/** array that maps w2_e_mode to mode function pointers */
+static const void(*const W2_MODES[]) = {
+	&w2_mode_maze, &w2_mode_grid, &w2_mode_grid, &w2_mode_halt,
+	&w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal,
+};
+
+/** switch current mode (allow switching back to previous mode) */
+void w2_modes_call(w2_e_mode mode);
+/** switch current mode (replace current mode keeping history index) */
+void w2_modes_swap(w2_e_mode mode);
diff --git a/robot/orangutan_shim.h b/robot/orangutan_shim.h
index de57c98..cd624d0 100644
--- a/robot/orangutan_shim.h
+++ b/robot/orangutan_shim.h
@@ -3,5 +3,8 @@
 #ifdef W2_SIM
 #include "sim.h"
 #else
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Warray-bounds"
 #include <pololu/orangutan.h>
+#pragma GCC diagnostic pop
 #endif
diff --git a/robot/readme.md b/robot/readme.md
index 5daa826..eb1dd37 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -62,13 +62,12 @@ what they're supposed to do:
 |maze              |`mode_maze  `|may 31|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  `|may 31|Fiona| stops all execution until emergency mode is reset by software or user|
-|line finding      |`mode_calb  `|may 31|Fiona| find line by turning on own axis if lost|
+|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  `|may 31|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|
 
-
 ## some standards
 
 this list will probably get updated from time to time:
@@ -109,10 +108,14 @@ global todo:
 - [ ] assume robot starts in maze
 - [ ] 'crosswalk' transition detection in seperate file (used by grid and maze
   mode)
-- [ ] calibrate (line-detecting) light sensors manually by placing the robot
-  and pressing a button
 - [ ] client software architecture
-- [ ] mode 'return' buffer
+- [x] mode 'return' buffer
+- [x] clear global timer at start of cycle instead of just for mode selection
+  module (for last ping time measurement)
+- [ ] calibrate (line-detecting) light sensors in setup.c, or manually by
+  placing the robot and pressing a button (maybe make this a seperate mode)
+- [ ] create labeled timer functions like nodejs `console.time()` and
+  `console.timeEnd()` (use for serial read timeout constraint)
 
 ### hypervisor
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index a0eed3a..2786b85 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -1,3 +1,133 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "../shared/bin.h"
+#include "../shared/serial_parse.h"
+#include "hypervisor.h"
+#include "mode_dirc.h"
+#include "modes.h"
+#include "orangutan_shim.h"
 #include "sercomm.h"
 
-void w2_sercomm_main() {}
+w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {0};
+uint8_t g_w2_sercomm_index							  = 0;
+uint8_t g_w2_sercomm_offset							  = 0;
+uint8_t g_w2_sercomm_buffer_full					  = 0;
+
+char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0};
+uint8_t g_w2_serial_buffer_index					= 0;
+uint8_t g_w2_serial_buffer_head						= 0;
+
+void w2_sercomm_main() {
+#ifdef W2_SIM
+	simprintfunc("w2_sercomm_main", "");
+#endif
+	// read and parse data
+	while (serial_get_received_bytes() != g_w2_serial_buffer_index) {
+		w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]);
+		g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE;
+	}
+
+	// send data
+	while (g_w2_sercomm_offset != g_w2_sercomm_index) {
+		w2_s_bin *data	= g_w2_sercomm_buffer[g_w2_sercomm_offset];
+		char *data_cast = malloc(data->bytes);
+		memcpy(data_cast, data->data, data->bytes);
+		serial_send(data_cast, data->bytes);
+		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
+	}
+}
+
+void w2_sercomm_append_msg(w2_s_bin *data) {
+#ifdef W2_SIM
+	simprintfunc("w2_sercomm_append_msg", "");
+#endif
+	uint8_t next_index		 = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE;
+	g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset;
+	free(g_w2_sercomm_buffer[g_w2_sercomm_index]);
+	g_w2_sercomm_buffer[g_w2_sercomm_index] = w2_bin_s_alloc(data->bytes, data->data);
+	if (g_w2_sercomm_buffer_full) return;
+	g_w2_sercomm_index = next_index;
+}
+
+void w2_cmd_ping_rx(w2_s_bin *data) {
+	w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	size_t return_size				 = sizeof(w2_s_cmd_ping_tx);
+	w2_s_cmd_ping_tx *return_message = malloc(return_size);
+	return_message->opcode			 = W2_CMD_PING | W2_CMDDIR_TX;
+	return_message->id				 = message->id;
+
+	w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message);
+
+	w2_sercomm_append_msg(return_message_bin);
+
+	free(message);
+	free(return_message);
+	free(return_message_bin);
+}
+
+void w2_cmd_mode_rx(w2_s_bin *data) {
+	w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	w2_modes_swap(message->mode);
+}
+
+void w2_cmd_sped_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_dirc_rx(w2_s_bin *data) {
+	w2_s_cmd_dirc_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left);
+	g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right);
+}
+
+void w2_cmd_cord_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_bomd_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_sres_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_mcfg_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_sens_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_info_rx(w2_s_bin *data) {
+	w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	size_t return_size				 = sizeof(w2_s_cmd_info_tx);
+	w2_s_cmd_info_tx *return_message = malloc(return_size);
+	return_message->opcode			 = W2_CMD_INFO | W2_CMDDIR_TX;
+	strncpy((char *)return_message->build_str, W2_BUILD_STR, sizeof(return_message->build_str));
+	return_message->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms;
+	return_message->io_ms		= (uint8_t)g_w2_hypervisor_ema_io_ms;
+	return_message->sercomm_ms	= (uint8_t)g_w2_hypervisor_ema_sercomm_ms;
+	return_message->mode_ms		= (uint8_t)g_w2_hypervisor_ema_mode_ms;
+	return_message->uptime_s	= w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3));
+
+	w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message);
+
+	w2_sercomm_append_msg(return_message_bin);
+
+	free(message);
+	free(return_message);
+	free(return_message_bin);
+}
+
+void w2_cmd_disp_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_play_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_cled_rx(w2_s_bin *data) { return; }
+
+void w2_cmd_ping_tx(w2_s_bin *data) {}
+void w2_cmd_expt_tx(w2_s_bin *data) {}
+void w2_cmd_mode_tx(w2_s_bin *data) {}
+void w2_cmd_cord_tx(w2_s_bin *data) {}
+void w2_cmd_bomd_tx(w2_s_bin *data) {}
+void w2_cmd_sens_tx(w2_s_bin *data) {}
+void w2_cmd_info_tx(w2_s_bin *data) {}
diff --git a/robot/sercomm.h b/robot/sercomm.h
index 58c79b9..b1f69c7 100644
--- a/robot/sercomm.h
+++ b/robot/sercomm.h
@@ -1,5 +1,23 @@
 #pragma once
 
+#include "../shared/bin.h"
+#include "../shared/consts.h"
+#include "../shared/protocol.h"
+
+/** sercomm ring buffer */
+extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE];
+/** stores head of ring buffer */
+extern uint8_t g_w2_sercomm_index;
+/** stores start of ring buffer */
+extern uint8_t g_w2_sercomm_offset;
+
+/** serial input (receive) buffer */
+extern char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE];
+/** serial input (receive) buffer current position */
+extern uint8_t g_w2_serial_buffer_index;
+/** serial input (receive) buffer head (sim only) */
+extern uint8_t g_w2_serial_buffer_head;
+
 /**
  * serial pc-robot communication module
  *
@@ -7,3 +25,6 @@
  * - sends all data in the message buffer
  */
 void w2_sercomm_main();
+
+/** append binary message to send buffer */
+void w2_sercomm_append_msg(w2_s_bin *data);
diff --git a/robot/setup.c b/robot/setup.c
index c74cca9..06c8fa4 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -1,13 +1,20 @@
 #include <stdlib.h>
 
-#include "consts.h"
-#include "halt.h"
+#include "../shared/bin.h"
+#include "../shared/consts.h"
 #include "modes.h"
 #include "orangutan_shim.h"
+#include "sercomm.h"
 #include "setup.h"
 
+// pointers for endianness check
+static const uint16_t _test	 = 1;
+static const uint8_t *_ptest = (uint8_t *)&_test;
+uint8_t g_w2_endianness;
+
 void w2_setup_main() {
-	serial_set_baud_rate(W2_SERIAL_BAUD);
+	// check endianness
+	g_w2_endianness = *_ptest;
 
 	// reset underside leds
 	red_led(0);
@@ -16,6 +23,17 @@ void w2_setup_main() {
 	// clear lcd
 	clear();
 
+	// start serial i/o
+	w2_cmd_setup_handlers();
+	serial_set_baud_rate(W2_SERIAL_BAUD);
+	serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE);
+
+	// reset timer
+	time_reset();
+
+	// set default mode
+	w2_modes_swap(W2_M_HALT);
+
 	// indicate startup done
 	play("L50 c>c");
 }
diff --git a/robot/sim.c b/robot/sim.c
index 6bd5838..ddc208a 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -1,15 +1,44 @@
 #include <stdio.h>
 #include <time.h>
 #include <string.h>
+#include <stdint.h>
+#include <termios.h>
+#include <unistd.h>
 
 #include "sim.h"
+#include "../shared/consts.h"
+#include "../shared/protocol.h"
+#include "sercomm.h"
+#include "errcatch.h"
 
 struct timespec reference_time; // NOLINT
+bool g_w2_sim_headless = false;
+
+static const char* const W2_CMD_NAMES[] = {
+	"PING",
+	"EXPT",
+	"MODE",
+	"SPED",
+	"DIRC",
+	"CORD",
+	"BOMD",
+	"SRES",
+	"MCFG",
+	"SENS",
+	"INFO",
+	"DISP",
+	"PLAY",
+	"CLED",
+};
+
+static const char* const W2_CMD_DIRECTIONS[] = {
+	"RX",
+	"TX",
+};
 
 void time_reset() {
 	simprintfunc("time_reset", "");
 	clock_gettime(CLOCK_MONOTONIC, &reference_time);
-	return;
 }
 
 unsigned long get_ms() {
@@ -22,43 +51,78 @@ unsigned long get_ms() {
 
 void red_led(unsigned char on) {
 	simprintfunc("red_led", "%i", on);
-	return;
 }
 
 void green_led(unsigned char on) {
 	simprintfunc("green_led", "%i", on);
-	return;
 }
 
 void clear() {
 	simprintfunc("clear", "");
-	return;
 }
 
 void play(const char* melody) {
 	simprintfunc("play", "\"%s\"", melody);
-	return;
 }
 
 void serial_set_baud_rate(unsigned int rate) {
 	simprintfunc("serial_set_baud_rate", "%u", rate);
-	return;
 }
 
 void serial_send(char* message, unsigned int length) {
-	simprintfunc("serial_send", "<see below>, %u", length);
-	unsigned int bytes = 0;
-	simprintf("");
-	for (unsigned int byte = 0; byte < length; byte++) {
-		if (bytes > DBG_BYTES_PER_LINE) {
-			bytes = 0;
-			printf("\n");
-			simprintf("");
-		}
-		printf("%02x ", message[byte]);
-		bytes++;
+	if (g_w2_sim_headless) {
+		for (unsigned int byte = 0; byte < length; byte++)
+			putc(message[byte] & 0xff, stdout);
+		return;
 	}
+
+	if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<see below>, %u", length);
+
+	if (!DBG_ENABLE_SERIAL) return;
+	w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message);
+	w2_sim_print_serial(bin);
+	free(bin);
+}
+
+void serial_receive_ring(char* buffer, unsigned char size) {
+	simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size);
+}
+
+unsigned char serial_get_received_bytes() {
+	simprintfunc("serial_get_received_bytes", "");
+	return g_w2_serial_buffer_head;
+}
+
+void w2_sim_setup(int argc, char **argv) {
+	if (argc > 1 && strcmp(argv[1], "headless") == 0)
+		g_w2_sim_headless = true;
+
+	// disable echo and enable raw mode
+	struct termios term;
+	tcgetattr(STDIN_FILENO, &term);
+	term.c_lflag &= ~(ECHO | ICANON);
+	term.c_cc[VTIME] = 0;
+	term.c_cc[VMIN] = 0;
+	tcsetattr(STDIN_FILENO, 0, &term);
+
+	// debug error
+	// w2_errcatch_throw(W2_E_WARN_BATTERY_LOW);
+}
+
+void w2_sim_cycle_begin() {
+	// read bytes from stdin
+	while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0)
+		g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE;
+}
+
+void w2_sim_print_serial(w2_s_bin *data) {
+	if (g_w2_sim_headless) return;
+	simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]);
+	for (int i = 0; i < data->bytes; i++)
+		printf(" %02x", data->data[i]);
 	printf("\n");
-	return;
 }
 
+void set_motors(int left, int right) {
+	simprintfunc("set_motors", "%i, %i", left, right);
+}
diff --git a/robot/sim.h b/robot/sim.h
index 15a1b4b..a595042 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -3,6 +3,20 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <stdbool.h>
+
+#include "../shared/bin.h"
+
+extern bool g_w2_sim_headless;
+
+// debug fine-tuning
+#define DBG_ENABLE_PRINTFUNC (1)
+#define DBG_ENABLE_SIMWARN (1)
+#define DBG_ENABLE_SIMINFO (1)
+#define DBG_ENABLE_CYCLEINFO (0)
+#define DBG_ENABLE_SERIAL (1)
+
+#define DBG_MAX_CYCLES (10)
 
 // debug print options
 #define DBG_BYTES_PER_LINE 16
@@ -19,12 +33,12 @@
 #define COL_RST "\e[0m"
 
 // debug stdout print macros
-#define simprintf(message, ...) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__)
+#define simprintf(message, ...) if (!g_w2_sim_headless) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__)
 #define simprint(message) simprintf(message "\n")
-#define simprintfunc(name, fmt, ...) simprintf(COL_BLU "[FUNC] " \
-		COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__)
-#define simwarn(message, ...) simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__)
-#define siminfo(message, ...) simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__)
+#define simprintfunc(name, fmt, ...) if (DBG_ENABLE_PRINTFUNC) { simprintf(COL_BLU "[FUNC] " \
+		COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); }
+#define simwarn(message, ...) if (DBG_ENABLE_SIMWARN) { simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__); }
+#define siminfo(message, ...) if (DBG_ENABLE_SIMINFO) { simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__); }
 
 /**
  * simulates pololu library functions for local testing
@@ -35,6 +49,14 @@ unsigned long get_ms(); // NOLINT
 void red_led(unsigned char on); // NOLINT
 void green_led(unsigned char on); // NOLINT
 void clear(); // NOLINT
-void play(const char* melody); // NOLINT
+void play(const char *melody); // NOLINT
 void serial_set_baud_rate(unsigned int rate); // NOLINT
-void serial_send(char* message, unsigned int length); // NOLINT
+void serial_send(char *message, unsigned int length); // NOLINT
+void serial_receive_ring(char *buffer, unsigned char size); // NOLINT
+unsigned char serial_get_received_bytes(); // NOLINT
+void set_motors(int left, int right); // NOLINT
+
+void w2_sim_setup(int argc, char **argv);
+void w2_sim_cycle_begin();
+void w2_sim_print_serial(w2_s_bin *data);
+
diff --git a/robot/tests/dirc.bin b/robot/tests/dirc.bin
new file mode 100644
index 0000000..1aea35c
--- /dev/null
+++ b/robot/tests/dirc.bin
diff --git a/robot/tests/info.bin b/robot/tests/info.bin
new file mode 100644
index 0000000..e1ce36c
--- /dev/null
+++ b/robot/tests/info.bin
@@ -0,0 +1 @@
+ÿ
\ No newline at end of file
diff --git a/robot/tests/mcfg.bin b/robot/tests/mcfg.bin
new file mode 100644
index 0000000..a43ecf8
--- /dev/null
+++ b/robot/tests/mcfg.bin
diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin
new file mode 100644
index 0000000..9cd9175
--- /dev/null
+++ b/robot/tests/mode.bin
@@ -0,0 +1 @@
+ÿ
\ No newline at end of file
diff --git a/robot/tests/padded_info.bin b/robot/tests/padded_info.bin
new file mode 100644
index 0000000..2ff1d8f
--- /dev/null
+++ b/robot/tests/padded_info.bin
@@ -0,0 +1 @@
+=ãÿ=íÿÿI89ÿ
\ No newline at end of file
diff --git a/robot/tests/ping.bin b/robot/tests/ping.bin
new file mode 100644
index 0000000..456804e
--- /dev/null
+++ b/robot/tests/ping.bin
diff --git a/robot/tests/readme.md b/robot/tests/readme.md
new file mode 100644
index 0000000..7f4cc9a
--- /dev/null
+++ b/robot/tests/readme.md
@@ -0,0 +1,11 @@
+this is a subdirectory that contains binary example commands to send to the
+simultation robot code
+
+to use:
+
+```
+./a.out < file
+```
+
+where `file` is one of the .bin files in this folder
+
diff --git a/robot/tests/undefined.bin b/robot/tests/undefined.bin
new file mode 100644
index 0000000..d44f518
--- /dev/null
+++ b/robot/tests/undefined.bin
@@ -0,0 +1 @@
+ÿ€9 4­
\ No newline at end of file