diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2016-12-01 10:13:44 -0500 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2016-12-01 10:13:44 -0500 |
| commit | ea9922ac767c43818c123965d40f766a52a744c9 (patch) | |
| tree | 4745c3724d559484142c1f6598ddf6f73f499de2 | |
| parent | 217e48a59d9d45eb87ea46e7fc32ce8d612ad462 (diff) | |
| -rw-r--r-- | Makefile | 2 | ||||
| -rw-r--r-- | main.cpp | 87 | ||||
| -rw-r--r-- | vex/vexspi.cpp | 4 | ||||
| -rw-r--r-- | vex/vexspi.h | 2 |
4 files changed, 83 insertions, 12 deletions
@@ -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."
@@ -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__
|