competition control is good

diff --git a/Makefile b/Makefile
index 31a33707caf1120e8facc31b2a9df755f5aae86d..a519b6d9713572dee0260057fa4fce110f5d8242 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -220,6 +220,6 @@ include $(RULESPATH)/rules.mk
 # upload target\r
 upload:\r
        @echo "Uploading..."\r
-       @sudo cortexflash -w build/ch.hex /dev/ttyUSB0\r
+       @sudo cortexflash -w build/zephyr.hex /dev/ttyUSB0\r
        @echo\r
        @echo "Done."\r

diff --git a/main.cpp b/main.cpp
index f2a55afbc34cf924e987f67dd163bfbe3fd6e600..17d35e939de8540c10598084193b7eb7c5aee5da 100644 (file)
--- a/main.cpp
+++ b/main.cpp
@@ -5,28 +5,93 @@
 #include <vex/digital.hpp>\r
 #include <vex/vexspi.h>\r
 \r
-int main(void) {\r
+static bool shouldRun = true;\r
+\r
+static char modeMonitorWA[512];\r
+static char autonWA[512];\r
+static char opconWA[512];\r
+\r
+void auton(void *);\r
+void opcon(void *);\r
+\r
+void modeMonitor(void *unused)\r
+{\r
+       (void)unused;\r
+\r
+       static int ut = 1;\r
+       while (1) {\r
+               chThdSleepMilliseconds(16);\r
+               digital::setLed(2, (ut ^= 1));\r
+\r
+               auto state = spi::getState();\r
+               // if not disabled\r
+               if (!(state & 0x80)) {\r
+                       shouldRun = true;\r
+\r
+                       // if autonomous\r
+                       if (state & 0x40) {\r
+                               digital::setLed(3, 1);\r
+                               chThdCreateStatic(autonWA, 512, NORMALPRIO - 1, auton, nullptr);\r
+                               state = 0x40;\r
+                       } else {\r
+                               chThdCreateStatic(opconWA, 512, NORMALPRIO - 1, opcon, nullptr);\r
+                               state = 0;\r
+                       }\r
+\r
+                       while ((spi::getState() & (0xC0)) == state)\r
+                               chThdSleepMilliseconds(16);\r
+\r
+                       // TODO stop all\r
+                       shouldRun = false;\r
+               } else digital::setLed(3, 0);\r
+       }\r
+}\r
+\r
+void auton(void *unused)\r
+{\r
+       (void)unused;\r
+\r
+       static int led = 1;\r
+\r
+       while (shouldRun) {\r
+               digital::setLed(1, led);\r
+               led ^= 1;\r
+\r
+               chThdSleepMilliseconds(100);\r
+       }\r
+}\r
+\r
+void opcon(void *unused)\r
+{\r
+       (void)unused;\r
+\r
+       while (shouldRun) {\r
+               lcd::flush();\r
+               lcd::printn(0, 0, spi::getJoystick(1).Ch1);\r
+\r
+               chThdSleepMilliseconds(100);\r
+       }\r
+}\r
+\r
+int main(void)\r
+{\r
        // init chibios\r
        halInit();\r
        chSysInit();\r
 \r
        // init robot\r
-       digital::setMode(1, 1);\r
        lcd::init();\r
        spi::init();\r
+       \r
+       digital::setMode(1, 1);\r
+       digital::setMode(2, 1);\r
+       digital::setMode(3, 1);\r
 \r
-       int led = 1; // for led toggle\r
+       chThdCreateStatic(modeMonitorWA, 512, NORMALPRIO - 1, modeMonitor, nullptr);\r
 \r
        while (1) {\r
                // update\r
-               lcd::flush();\r
                spi::update();\r
-\r
-               lcd::printn(0, 0, spi::getJoystick(1).Ch1);\r
-\r
-               digital::setLed(1, led);\r
-               led ^= 1;\r
-\r
-               chThdSleepMilliseconds(100);\r
+               chThdSleepMilliseconds(10);\r
        }\r
 }\r

diff --git a/vex/vexspi.cpp b/vex/vexspi.cpp
index 4d05b9e34db90020fb437d305b5fc23f72cfd53f..dd059e8b94969d0071438ab416c399345f8f89b5 100644 (file)
--- a/vex/vexspi.cpp
+++ b/vex/vexspi.cpp
@@ -198,4 +198,8 @@ namespace spi {
                return vexSpiData.rxdata.pak.batt2 * 59; \r
        }\r
 \r
+       int getState(void) {\r
+               return vexSpiData.rxdata.pak.ctl;\r
+       }\r
+\r
 }\r

diff --git a/vex/vexspi.h b/vex/vexspi.h
index aa7eeb94cd5ce64c44e8a7ab913143772d29ae7f..c4fc6b16528b1ed755287d34a0050ad23e4fc5c3 100644 (file)
--- a/vex/vexspi.h
+++ b/vex/vexspi.h
@@ -166,6 +166,8 @@ namespace spi {
 \r
        int getBatteryMain(void);\r
        int getBatteryBackup(void);\r
+\r
+       int getState(void);\r
 }\r
 \r
 #endif  // __VEXSPI__\r