4 files changed, 83 insertions, 12 deletions
diff --git a/Makefile b/Makefile
index 31a3370..a519b6d 100644
--- a/Makefile
+++ b/Makefile
@@ -220,6 +220,6 @@ include $(RULESPATH)/rules.mk
 # upload target
 upload:
 	@echo "Uploading..."
-	@sudo cortexflash -w build/ch.hex /dev/ttyUSB0
+	@sudo cortexflash -w build/zephyr.hex /dev/ttyUSB0
 	@echo
 	@echo "Done."
diff --git a/main.cpp b/main.cpp
index f2a55af..17d35e9 100644
--- a/main.cpp
+++ b/main.cpp
@@ -5,28 +5,93 @@
 #include <vex/digital.hpp>
 #include <vex/vexspi.h>
 
-int main(void) {
+static bool shouldRun = true;
+
+static char modeMonitorWA[512];
+static char autonWA[512];
+static char opconWA[512];
+
+void auton(void *);
+void opcon(void *);
+
+void modeMonitor(void *unused)
+{
+	(void)unused;
+
+	static int ut = 1;
+	while (1) {
+		chThdSleepMilliseconds(16);
+		digital::setLed(2, (ut ^= 1));
+
+		auto state = spi::getState();
+		// if not disabled
+		if (!(state & 0x80)) {
+			shouldRun = true;
+
+			// if autonomous
+			if (state & 0x40) {
+				digital::setLed(3, 1);
+				chThdCreateStatic(autonWA, 512, NORMALPRIO - 1, auton, nullptr);
+				state = 0x40;
+			} else {
+				chThdCreateStatic(opconWA, 512, NORMALPRIO - 1, opcon, nullptr);
+				state = 0;
+			}
+
+			while ((spi::getState() & (0xC0)) == state)
+				chThdSleepMilliseconds(16);
+
+			// TODO stop all
+			shouldRun = false;
+		} else digital::setLed(3, 0);
+	}
+}
+
+void auton(void *unused)
+{
+	(void)unused;
+
+	static int led = 1;
+
+	while (shouldRun) {
+		digital::setLed(1, led);
+		led ^= 1;
+
+		chThdSleepMilliseconds(100);
+	}
+}
+
+void opcon(void *unused)
+{
+	(void)unused;
+
+	while (shouldRun) {
+		lcd::flush();
+		lcd::printn(0, 0, spi::getJoystick(1).Ch1);
+
+		chThdSleepMilliseconds(100);
+	}
+}
+
+int main(void)
+{
 	// init chibios
 	halInit();
 	chSysInit();
 
 	// init robot
-	digital::setMode(1, 1);
 	lcd::init();
 	spi::init();
+	
+	digital::setMode(1, 1);
+	digital::setMode(2, 1);
+	digital::setMode(3, 1);
 
-	int led = 1; // for led toggle
+	chThdCreateStatic(modeMonitorWA, 512, NORMALPRIO - 1, modeMonitor, nullptr);
 
 	while (1) {
 		// update
-		lcd::flush();
 		spi::update();
-
-		lcd::printn(0, 0, spi::getJoystick(1).Ch1);
-
-		digital::setLed(1, led);
-		led ^= 1;
-
-		chThdSleepMilliseconds(100);
+		chThdSleepMilliseconds(10);
 	}
 }
diff --git a/vex/vexspi.cpp b/vex/vexspi.cpp
index 4d05b9e..dd059e8 100644
--- a/vex/vexspi.cpp
+++ b/vex/vexspi.cpp
@@ -198,4 +198,8 @@ namespace spi {
 		return vexSpiData.rxdata.pak.batt2 * 59; 
 	}
 
+	int getState(void) {
+		return vexSpiData.rxdata.pak.ctl;
+	}
+
 }
diff --git a/vex/vexspi.h b/vex/vexspi.h
index aa7eeb9..c4fc6b1 100644
--- a/vex/vexspi.h
+++ b/vex/vexspi.h
@@ -166,6 +166,8 @@ namespace spi {
 
 	int getBatteryMain(void);
 	int getBatteryBackup(void);
+
+	int getState(void);
 }
 
 #endif  // __VEXSPI__