From ea9922ac767c43818c123965d40f766a52a744c9 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Thu, 1 Dec 2016 10:13:44 -0500
Subject: competition control is good

---
 main.cpp | 87 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 76 insertions(+), 11 deletions(-)

(limited to 'main.cpp')

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);
 	}
 }
-- 
cgit v1.2.3