1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
|
#include <ch.h>
#include <hal.h>
#include <vex/lcd.hpp>
#include <vex/digital.hpp>
#include <vex/vexspi.h>
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
lcd::init();
spi::init();
digital::setMode(1, 1);
digital::setMode(2, 1);
digital::setMode(3, 1);
chThdCreateStatic(modeMonitorWA, 512, NORMALPRIO - 1, modeMonitor, nullptr);
while (1) {
// update
spi::update();
chThdSleepMilliseconds(10);
}
}
|
