Wii Nunchuk via RFM69HW to Duplo 9203 Remote Control Car Kit using ARM mbed on a FRDM-KL25Z
Dependencies: CRC FastPWM RFM69 USBDevice WakeUp WiiChuk_compat mbed-rtos mbed tlc59108
Fork of wiiNunchuk_compat by
main.cpp@27:c559bfe67109, 2017-08-02 (annotated)
- Committer:
- eisd
- Date:
- Wed Aug 02 15:17:17 2017 +0000
- Revision:
- 27:c559bfe67109
- Parent:
- 26:ddfb293f4606
Transmit stick position for indicators to act before movement
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gbrush | 0:7c98bcd8a245 | 1 | #include "mbed.h" |
eisd | 5:7af5760d7e8f | 2 | #include "rtos.h" |
eisd | 13:ab524cb4f768 | 3 | #include "WakeUp.h" |
eisd | 11:7c3fb795af73 | 4 | #include "FastPWM.h" |
eisd | 1:de8c34c9ccdf | 5 | #ifndef M_PI |
eisd | 1:de8c34c9ccdf | 6 | #define M_PI 3.14159265358979323846 |
eisd | 1:de8c34c9ccdf | 7 | #endif |
eisd | 1:de8c34c9ccdf | 8 | |
eisd | 1:de8c34c9ccdf | 9 | #include "WiiChuk_compat.hpp" |
eisd | 7:f720032c2fb9 | 10 | #include "lib_crc.h" |
gbrush | 0:7c98bcd8a245 | 11 | |
eisd | 25:74edb9fc46c6 | 12 | #include "tlc59108.h" |
eisd | 25:74edb9fc46c6 | 13 | |
eisd | 2:04fdd571a385 | 14 | //#include "USBSerial.h" |
eisd | 2:04fdd571a385 | 15 | |
eisd | 2:04fdd571a385 | 16 | #include "RFM69.h" |
eisd | 2:04fdd571a385 | 17 | #define GATEWAY_ID 2 |
eisd | 2:04fdd571a385 | 18 | #define NODE_ID 1 |
eisd | 2:04fdd571a385 | 19 | #define NETWORKID 100 |
gbrush | 0:7c98bcd8a245 | 20 | |
eisd | 2:04fdd571a385 | 21 | // Uncomment only one of the following three to match radio frequency |
eisd | 2:04fdd571a385 | 22 | //#define FREQUENCY RF69_433MHZ |
eisd | 2:04fdd571a385 | 23 | #define FREQUENCY RF69_868MHZ |
eisd | 2:04fdd571a385 | 24 | //#define FREQUENCY RF69_915MHZ |
eisd | 2:04fdd571a385 | 25 | |
eisd | 3:9091adbed369 | 26 | const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" }; |
eisd | 5:7af5760d7e8f | 27 | |
eisd | 14:579137c8ceac | 28 | #define DEBUG 0 |
eisd | 14:579137c8ceac | 29 | #ifdef DEBUG |
eisd | 5:7af5760d7e8f | 30 | #ifdef USBSerial |
eisd | 5:7af5760d7e8f | 31 | USBSerial pc; |
eisd | 5:7af5760d7e8f | 32 | #else |
eisd | 2:04fdd571a385 | 33 | Serial pc(USBTX, USBRX); |
eisd | 1:de8c34c9ccdf | 34 | #endif |
eisd | 14:579137c8ceac | 35 | #else |
eisd | 14:579137c8ceac | 36 | class Null : public Stream { |
eisd | 14:579137c8ceac | 37 | public: |
eisd | 14:579137c8ceac | 38 | Null(): Stream("null") {} |
eisd | 14:579137c8ceac | 39 | void baud(int) {} |
eisd | 5:7af5760d7e8f | 40 | |
eisd | 14:579137c8ceac | 41 | protected: |
eisd | 14:579137c8ceac | 42 | virtual int _getc() { return 0; } |
eisd | 14:579137c8ceac | 43 | virtual int _putc(int c) { return 0; } |
eisd | 14:579137c8ceac | 44 | }; |
eisd | 14:579137c8ceac | 45 | Null pc; |
eisd | 14:579137c8ceac | 46 | #endif |
eisd | 5:7af5760d7e8f | 47 | #if TARGET_KL25Z |
eisd | 25:74edb9fc46c6 | 48 | DigitalOut gnd(PTC6); |
eisd | 25:74edb9fc46c6 | 49 | PwmOut ir(PTA5); |
eisd | 5:7af5760d7e8f | 50 | #endif |
eisd | 5:7af5760d7e8f | 51 | #define pulse(x, y) ir = 0.5; wait_us(x); ir = 0.0; wait_us(y); |
eisd | 5:7af5760d7e8f | 52 | |
eisd | 5:7af5760d7e8f | 53 | /* generated in bash from lirc raw codes */ |
eisd | 5:7af5760d7e8f | 54 | /* |
eisd | 5:7af5760d7e8f | 55 | f=duplo.conf |
eisd | 5:7af5760d7e8f | 56 | sed -n '/begin raw_codes/,/end raw_codes/s/name //p' $f| |
eisd | 5:7af5760d7e8f | 57 | while read n; do |
eisd | 5:7af5760d7e8f | 58 | echo -e "void $n() {$( |
eisd | 5:7af5760d7e8f | 59 | grep $n -A 1 $f|tail -n1| |
eisd | 5:7af5760d7e8f | 60 | sed 's/$/ 1000/;s@\([0-9]\+\)[[:space:]]\+\([0-9]\+\)[[:space:]]*@\n\tpulse(\1, \2);@g' |
eisd | 5:7af5760d7e8f | 61 | )\n}" |
eisd | 5:7af5760d7e8f | 62 | done |
eisd | 5:7af5760d7e8f | 63 | */ |
eisd | 5:7af5760d7e8f | 64 | |
eisd | 5:7af5760d7e8f | 65 | void FX() { |
eisd | 5:7af5760d7e8f | 66 | pulse(1042, 208); |
eisd | 5:7af5760d7e8f | 67 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 68 | pulse(208, 562); |
eisd | 5:7af5760d7e8f | 69 | pulse(625, 208); |
eisd | 5:7af5760d7e8f | 70 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 71 | pulse(208, 1); |
eisd | 5:7af5760d7e8f | 72 | } |
eisd | 5:7af5760d7e8f | 73 | void XF() { |
eisd | 5:7af5760d7e8f | 74 | pulse(229, 604); |
eisd | 5:7af5760d7e8f | 75 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 76 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 77 | pulse(438, 333); |
eisd | 5:7af5760d7e8f | 78 | pulse(625, 208); |
eisd | 5:7af5760d7e8f | 79 | pulse(1250, 1); |
eisd | 5:7af5760d7e8f | 80 | } |
eisd | 5:7af5760d7e8f | 81 | void FF() { |
eisd | 5:7af5760d7e8f | 82 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 83 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 84 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 85 | pulse(417, 354); |
eisd | 5:7af5760d7e8f | 86 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 87 | pulse(208, 1042); |
eisd | 5:7af5760d7e8f | 88 | pulse(208, 1); |
eisd | 5:7af5760d7e8f | 89 | } |
eisd | 5:7af5760d7e8f | 90 | void RX() { |
eisd | 5:7af5760d7e8f | 91 | pulse(1042, 208); |
eisd | 5:7af5760d7e8f | 92 | pulse(208, 188); |
eisd | 5:7af5760d7e8f | 93 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 94 | pulse(646, 1); |
eisd | 5:7af5760d7e8f | 95 | } |
eisd | 5:7af5760d7e8f | 96 | void XR() { |
eisd | 5:7af5760d7e8f | 97 | pulse(208, 1042); |
eisd | 5:7af5760d7e8f | 98 | pulse(208, 188); |
eisd | 5:7af5760d7e8f | 99 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 100 | pulse(625, 417); |
eisd | 5:7af5760d7e8f | 101 | pulse(854, 1); |
eisd | 5:7af5760d7e8f | 102 | } |
eisd | 5:7af5760d7e8f | 103 | void RR() { |
eisd | 5:7af5760d7e8f | 104 | pulse(229, 1021); |
eisd | 5:7af5760d7e8f | 105 | pulse(229, 208); |
eisd | 5:7af5760d7e8f | 106 | pulse(208, 542); |
eisd | 5:7af5760d7e8f | 107 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 108 | pulse(229, 1229); |
eisd | 5:7af5760d7e8f | 109 | pulse(229, 1); |
eisd | 5:7af5760d7e8f | 110 | } |
eisd | 5:7af5760d7e8f | 111 | void RF() { |
eisd | 5:7af5760d7e8f | 112 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 113 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 114 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 115 | pulse(417, 333); |
eisd | 5:7af5760d7e8f | 116 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 117 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 118 | pulse(208, 1); |
eisd | 5:7af5760d7e8f | 119 | } |
eisd | 5:7af5760d7e8f | 120 | void FR() { |
eisd | 5:7af5760d7e8f | 121 | pulse(229, 1021); |
eisd | 5:7af5760d7e8f | 122 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 123 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 124 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 125 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 126 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 127 | pulse(417, 1); |
eisd | 5:7af5760d7e8f | 128 | } |
eisd | 5:7af5760d7e8f | 129 | void BB() { |
eisd | 5:7af5760d7e8f | 130 | pulse(1042, 208); |
eisd | 5:7af5760d7e8f | 131 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 132 | pulse(208, 542); |
eisd | 5:7af5760d7e8f | 133 | pulse(208, 417); |
eisd | 5:7af5760d7e8f | 134 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 135 | pulse(1042, 1); |
eisd | 5:7af5760d7e8f | 136 | } |
eisd | 5:7af5760d7e8f | 137 | |
eisd | 25:74edb9fc46c6 | 138 | int mag; |
eisd | 5:7af5760d7e8f | 139 | bool central = true; |
eisd | 12:eec17d54a646 | 140 | Timer central_time; |
eisd | 25:74edb9fc46c6 | 141 | bool central_time_trip = 0; |
eisd | 7:f720032c2fb9 | 142 | int stops_sent = 0; |
eisd | 5:7af5760d7e8f | 143 | int direction = -1; |
eisd | 5:7af5760d7e8f | 144 | Thread *thread; |
eisd | 5:7af5760d7e8f | 145 | void ir_thread(void const *args) { |
eisd | 5:7af5760d7e8f | 146 | while(1) { |
eisd | 5:7af5760d7e8f | 147 | //if (!central) |
eisd | 25:74edb9fc46c6 | 148 | for(int x = 0; x < 40; x++) { |
eisd | 14:579137c8ceac | 149 | #if DEBUG |
eisd | 14:579137c8ceac | 150 | pc.printf(central? "stop %s %d\r\n" : "direction %s %d\r\n", directions[direction], stops_sent); |
eisd | 14:579137c8ceac | 151 | #endif |
eisd | 25:74edb9fc46c6 | 152 | if (central || (x % 4) > mag) { |
eisd | 10:ba778fc959b8 | 153 | if (stops_sent < 50) { |
eisd | 10:ba778fc959b8 | 154 | stops_sent++; |
eisd | 9:dc6ffa2ef179 | 155 | BB(); |
eisd | 10:ba778fc959b8 | 156 | } |
eisd | 9:dc6ffa2ef179 | 157 | } else { |
eisd | 7:f720032c2fb9 | 158 | stops_sent = 0; |
eisd | 7:f720032c2fb9 | 159 | switch(direction) { |
eisd | 7:f720032c2fb9 | 160 | case 0: XR(); break; |
eisd | 7:f720032c2fb9 | 161 | case 1: RR(); break; |
eisd | 7:f720032c2fb9 | 162 | case 2: RX(); break; |
eisd | 7:f720032c2fb9 | 163 | case 3: FR(); break; |
eisd | 7:f720032c2fb9 | 164 | case 4: FX(); break; |
eisd | 7:f720032c2fb9 | 165 | case 5: FF(); break; |
eisd | 7:f720032c2fb9 | 166 | case 6: XF(); break; |
eisd | 7:f720032c2fb9 | 167 | case 7: RF(); break; |
eisd | 7:f720032c2fb9 | 168 | } |
eisd | 5:7af5760d7e8f | 169 | } |
eisd | 25:74edb9fc46c6 | 170 | Thread::wait(5); |
eisd | 5:7af5760d7e8f | 171 | } |
eisd | 25:74edb9fc46c6 | 172 | Thread::wait(30); |
eisd | 5:7af5760d7e8f | 173 | } |
eisd | 5:7af5760d7e8f | 174 | } |
eisd | 5:7af5760d7e8f | 175 | |
eisd | 11:7c3fb795af73 | 176 | FastPWM speaker(PTA12); |
eisd | 7:f720032c2fb9 | 177 | DigitalOut speaker_gnd(PTC4); |
eisd | 7:f720032c2fb9 | 178 | //float speaker = 0; |
eisd | 7:f720032c2fb9 | 179 | //float speaker_gnd = 0; |
eisd | 7:f720032c2fb9 | 180 | |
eisd | 7:f720032c2fb9 | 181 | //global variables used by interrupt routine |
eisd | 11:7c3fb795af73 | 182 | volatile int i=512; |
eisd | 12:eec17d54a646 | 183 | float *wave;//[512]; |
eisd | 7:f720032c2fb9 | 184 | |
eisd | 7:f720032c2fb9 | 185 | // Interrupt routine |
eisd | 7:f720032c2fb9 | 186 | // used to output next analog sample whenever a timer interrupt occurs |
eisd | 7:f720032c2fb9 | 187 | void Sample_timer_interrupt(void) |
eisd | 7:f720032c2fb9 | 188 | { |
eisd | 7:f720032c2fb9 | 189 | // send next analog sample out to D to A |
eisd | 7:f720032c2fb9 | 190 | speaker = wave[i]; |
eisd | 7:f720032c2fb9 | 191 | // increment pointer and wrap around back to 0 at 128 |
eisd | 7:f720032c2fb9 | 192 | i = (i+1) & 0x07F; |
eisd | 7:f720032c2fb9 | 193 | } |
eisd | 7:f720032c2fb9 | 194 | |
eisd | 7:f720032c2fb9 | 195 | int Siren_pitch = 1; |
eisd | 7:f720032c2fb9 | 196 | void Siren_pitch_flip() { |
eisd | 11:7c3fb795af73 | 197 | speaker.period(wave[--i]); |
eisd | 11:7c3fb795af73 | 198 | if (i == 0) |
eisd | 11:7c3fb795af73 | 199 | i = 128; |
eisd | 7:f720032c2fb9 | 200 | } |
eisd | 7:f720032c2fb9 | 201 | |
eisd | 7:f720032c2fb9 | 202 | Ticker siren_pitch; |
eisd | 11:7c3fb795af73 | 203 | Timeout siren_pitch_switch; |
eisd | 7:f720032c2fb9 | 204 | bool Siren_last = 0; |
eisd | 11:7c3fb795af73 | 205 | void Siren_faster() { |
eisd | 11:7c3fb795af73 | 206 | siren_pitch.detach(); |
eisd | 11:7c3fb795af73 | 207 | siren_pitch.attach(&Siren_pitch_flip, 0.6/128); |
eisd | 11:7c3fb795af73 | 208 | } |
eisd | 7:f720032c2fb9 | 209 | void Siren_state(bool on) { |
eisd | 7:f720032c2fb9 | 210 | if (Siren_last != on) { |
eisd | 7:f720032c2fb9 | 211 | pc.printf("siren %d\r\n", on); |
eisd | 7:f720032c2fb9 | 212 | siren_pitch.detach(); |
eisd | 7:f720032c2fb9 | 213 | Siren_pitch = 1; |
eisd | 7:f720032c2fb9 | 214 | Siren_pitch_flip(); |
eisd | 11:7c3fb795af73 | 215 | siren_pitch.detach(); |
eisd | 11:7c3fb795af73 | 216 | if (on) |
eisd | 11:7c3fb795af73 | 217 | siren_pitch.attach(&Siren_pitch_flip, 0.6/128); |
eisd | 11:7c3fb795af73 | 218 | //siren_pitch_switch.attach(&Siren_faster, 2.0); |
eisd | 25:74edb9fc46c6 | 219 | speaker = on ? 0.5 : 0; |
eisd | 7:f720032c2fb9 | 220 | } |
eisd | 7:f720032c2fb9 | 221 | Siren_last = on; |
eisd | 7:f720032c2fb9 | 222 | } |
eisd | 7:f720032c2fb9 | 223 | |
eisd | 8:5f933580b560 | 224 | DigitalOut Headlight_l(PTD7); |
eisd | 8:5f933580b560 | 225 | DigitalOut Headlight_r(PTB8); |
eisd | 8:5f933580b560 | 226 | |
eisd | 8:5f933580b560 | 227 | #define Headlight_swap 3 |
eisd | 8:5f933580b560 | 228 | #define Headlight_flicker 15 |
eisd | 8:5f933580b560 | 229 | uint8_t Headlight_mode = 0; |
eisd | 8:5f933580b560 | 230 | uint8_t Headlight_phase = 0; |
eisd | 8:5f933580b560 | 231 | Ticker Headlight_pattern; |
eisd | 8:5f933580b560 | 232 | void Headlight_interrupt() { |
eisd | 8:5f933580b560 | 233 | Headlight_phase = (Headlight_phase + 1) % Headlight_flicker; |
eisd | 8:5f933580b560 | 234 | Headlight_l = Headlight_mode == 1 || (Headlight_mode == 2) && ((Headlight_phase % 2) == 0) && (Headlight_phase * 2 > Headlight_flicker); |
eisd | 8:5f933580b560 | 235 | Headlight_r = Headlight_mode == 1 || (Headlight_mode == 2) && ((Headlight_phase % 2) == 0) && (Headlight_phase * 2 < Headlight_flicker); |
eisd | 8:5f933580b560 | 236 | } |
eisd | 25:74edb9fc46c6 | 237 | |
eisd | 8:5f933580b560 | 238 | void Headlight_state(bool a, bool b) { |
eisd | 8:5f933580b560 | 239 | uint8_t mode = b ? 1 : a ? 2 : 0; |
eisd | 8:5f933580b560 | 240 | if (Headlight_mode != mode) { |
eisd | 11:7c3fb795af73 | 241 | if (mode == 1) |
eisd | 11:7c3fb795af73 | 242 | i = 512; |
eisd | 8:5f933580b560 | 243 | Headlight_mode = mode; |
eisd | 12:eec17d54a646 | 244 | Headlight_pattern.detach(); |
eisd | 8:5f933580b560 | 245 | Headlight_interrupt(); |
eisd | 12:eec17d54a646 | 246 | if (mode > 0) |
eisd | 12:eec17d54a646 | 247 | Headlight_pattern.attach_us(&Headlight_interrupt, (timestamp_t)(1000000/Headlight_swap/Headlight_flicker)); |
eisd | 8:5f933580b560 | 248 | } |
eisd | 8:5f933580b560 | 249 | } |
eisd | 8:5f933580b560 | 250 | |
eisd | 26:ddfb293f4606 | 251 | uint8_t indicators = 0; |
eisd | 26:ddfb293f4606 | 252 | uint8_t indicator_phase = 0; |
eisd | 26:ddfb293f4606 | 253 | Ticker indicator_pattern; |
eisd | 26:ddfb293f4606 | 254 | void indicator_interrupt() { |
eisd | 26:ddfb293f4606 | 255 | indicator_phase++; |
eisd | 26:ddfb293f4606 | 256 | } |
eisd | 26:ddfb293f4606 | 257 | void indicator_init() { |
eisd | 26:ddfb293f4606 | 258 | indicator_pattern.attach(&indicator_interrupt, 0.5); |
eisd | 26:ddfb293f4606 | 259 | } |
eisd | 26:ddfb293f4606 | 260 | |
eisd | 25:74edb9fc46c6 | 261 | TLC59108 *light_bar; |
eisd | 25:74edb9fc46c6 | 262 | #define light_bar_phases 10 |
eisd | 26:ddfb293f4606 | 263 | uint8_t light_bar_pattern[light_bar_phases+1][8] = { |
eisd | 26:ddfb293f4606 | 264 | { 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 265 | { 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 266 | { 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 267 | { 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 268 | { 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 269 | { 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 270 | { 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 271 | { 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 272 | { 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 273 | { 0x00, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00 }, |
eisd | 26:ddfb293f4606 | 274 | { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } }; |
eisd | 25:74edb9fc46c6 | 275 | uint8_t light_bar_phase = 0; |
eisd | 25:74edb9fc46c6 | 276 | |
eisd | 25:74edb9fc46c6 | 277 | void light_bar_thread(const void*) { |
eisd | 25:74edb9fc46c6 | 278 | while(true) { |
eisd | 25:74edb9fc46c6 | 279 | Thread::wait(20); |
eisd | 26:ddfb293f4606 | 280 | light_bar_phase = Siren_last ? (light_bar_phase + 1) % light_bar_phases : light_bar_phases; |
eisd | 26:ddfb293f4606 | 281 | uint8_t* pattern = light_bar_pattern[light_bar_phase]; |
eisd | 26:ddfb293f4606 | 282 | pattern[4] = Headlight_l ? 0xFF : 0x00; |
eisd | 26:ddfb293f4606 | 283 | pattern[5] = Headlight_r ? 0xFF : 0x00; |
eisd | 26:ddfb293f4606 | 284 | pattern[6] = (indicator_phase % 2) == 0 && indicators == 1 ? 0xFF : 0x00; |
eisd | 26:ddfb293f4606 | 285 | pattern[7] = (indicator_phase % 2) == 0 && indicators == 2 ? 0xFF : 0x00; |
eisd | 26:ddfb293f4606 | 286 | pc.printf("light bar %d\r\n", light_bar->setBrightness(pattern)); |
eisd | 25:74edb9fc46c6 | 287 | } |
eisd | 25:74edb9fc46c6 | 288 | } |
eisd | 25:74edb9fc46c6 | 289 | Thread *t_light_bar_thread; |
eisd | 25:74edb9fc46c6 | 290 | |
eisd | 25:74edb9fc46c6 | 291 | void light_bar_init() { |
eisd | 25:74edb9fc46c6 | 292 | light_bar = new TLC59108(PTE0, PTE1); |
eisd | 25:74edb9fc46c6 | 293 | pc.printf("light bar init %d\r\n", light_bar->setLedOutputMode(TLC59108::LED_MODE::PWM_INDGRP)); |
eisd | 25:74edb9fc46c6 | 294 | light_bar->setGroupBrightness(100); |
eisd | 25:74edb9fc46c6 | 295 | t_light_bar_thread = new Thread(&light_bar_thread); |
eisd | 25:74edb9fc46c6 | 296 | } |
eisd | 25:74edb9fc46c6 | 297 | |
eisd | 12:eec17d54a646 | 298 | void sleep_loop(void const *argument) { |
eisd | 12:eec17d54a646 | 299 | while (true) { |
eisd | 12:eec17d54a646 | 300 | sleep(); |
eisd | 12:eec17d54a646 | 301 | Thread::wait(100); |
eisd | 12:eec17d54a646 | 302 | } |
eisd | 12:eec17d54a646 | 303 | } |
gbrush | 0:7c98bcd8a245 | 304 | |
eisd | 2:04fdd571a385 | 305 | #ifdef TARGET_KL25Z |
eisd | 6:3482f1e19d71 | 306 | PwmOut r(LED_RED); |
eisd | 6:3482f1e19d71 | 307 | //float r = 1.0f; |
eisd | 6:3482f1e19d71 | 308 | PwmOut g(LED_GREEN); |
eisd | 6:3482f1e19d71 | 309 | //float g = 1.0f; |
eisd | 6:3482f1e19d71 | 310 | PwmOut b(LED_BLUE); |
eisd | 6:3482f1e19d71 | 311 | //float b = 1.0f; |
eisd | 17:b9030e94b622 | 312 | DigitalOut nun_vdd(PTE31); |
eisd | 23:e08b10694786 | 313 | WiiChuck *nun; |
eisd | 23:e08b10694786 | 314 | uint8_t nun_settled = 0; |
eisd | 22:d4d991c7a40b | 315 | bool nun_init(nunchuk *next) { |
eisd | 25:74edb9fc46c6 | 316 | //return false; |
eisd | 25:74edb9fc46c6 | 317 | for (int i = 0; i < 5; i++) { |
eisd | 22:d4d991c7a40b | 318 | if (nun) |
eisd | 22:d4d991c7a40b | 319 | delete nun; |
eisd | 23:e08b10694786 | 320 | nun_vdd = 1; |
eisd | 23:e08b10694786 | 321 | Thread::wait(50); |
eisd | 22:d4d991c7a40b | 322 | nun = new WiiChuck(PTE0, PTE1, pc); |
eisd | 22:d4d991c7a40b | 323 | if (nun->Read(&next->X, &next->Y, &next->aX, &next->aY, &next->aZ, &next->C, &next->Z)) { |
eisd | 23:e08b10694786 | 324 | nun_settled = 0; |
eisd | 22:d4d991c7a40b | 325 | Thread::wait(10); |
eisd | 22:d4d991c7a40b | 326 | return true; |
eisd | 22:d4d991c7a40b | 327 | } |
eisd | 22:d4d991c7a40b | 328 | pc.printf("nun_init error\r\n"); |
eisd | 22:d4d991c7a40b | 329 | nun_vdd = 0; |
eisd | 23:e08b10694786 | 330 | Thread::wait(50); |
eisd | 22:d4d991c7a40b | 331 | } |
eisd | 22:d4d991c7a40b | 332 | return false; |
eisd | 17:b9030e94b622 | 333 | } |
eisd | 17:b9030e94b622 | 334 | void nun_sleep() { |
eisd | 17:b9030e94b622 | 335 | nun_vdd = 0; |
eisd | 17:b9030e94b622 | 336 | } |
eisd | 2:04fdd571a385 | 337 | RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13); |
eisd | 2:04fdd571a385 | 338 | #else |
eisd | 17:b9030e94b622 | 339 | WiiChuck *nun = new WiiChuck(p9, p10, pc); |
eisd | 22:d4d991c7a40b | 340 | bool nun_init(nunchuk *) {} |
eisd | 17:b9030e94b622 | 341 | void nun_sleep() {} |
eisd | 2:04fdd571a385 | 342 | #endif |
eisd | 2:04fdd571a385 | 343 | |
eisd | 13:ab524cb4f768 | 344 | Timer rx_last_contact; |
eisd | 25:74edb9fc46c6 | 345 | bool rx_last_contact_trip = 0; |
eisd | 12:eec17d54a646 | 346 | bool rx_to_snooze = true; |
eisd | 12:eec17d54a646 | 347 | bool rx_snoozing = false; |
eisd | 12:eec17d54a646 | 348 | bool rx_snoozed() { |
eisd | 12:eec17d54a646 | 349 | if (rx_snoozing) { |
eisd | 12:eec17d54a646 | 350 | pc.printf("still snoozing\r\n"); |
eisd | 12:eec17d54a646 | 351 | Thread::signal_wait(0x1); |
eisd | 12:eec17d54a646 | 352 | pc.printf("signalled?\r\n"); |
eisd | 12:eec17d54a646 | 353 | } |
eisd | 12:eec17d54a646 | 354 | return true; |
eisd | 12:eec17d54a646 | 355 | } |
eisd | 12:eec17d54a646 | 356 | |
eisd | 12:eec17d54a646 | 357 | void rx_snoozer(void const *mainThreadID) { |
eisd | 13:ab524cb4f768 | 358 | pc.printf("snooze rx %f\r\n", rx_last_contact.read()); |
eisd | 12:eec17d54a646 | 359 | |
eisd | 12:eec17d54a646 | 360 | rx_snoozing = true; |
eisd | 13:ab524cb4f768 | 361 | radio.sleep(); |
eisd | 25:74edb9fc46c6 | 362 | WakeUp::set((rx_last_contact_trip |= (rx_last_contact.read() < 60))? 2 : 10); |
eisd | 13:ab524cb4f768 | 363 | deepsleep(); |
eisd | 23:e08b10694786 | 364 | stops_sent = 0; |
eisd | 12:eec17d54a646 | 365 | rx_snoozing = false; |
eisd | 12:eec17d54a646 | 366 | rx_to_snooze = true; |
eisd | 12:eec17d54a646 | 367 | pc.printf("unsnooze rx\r\n"); |
eisd | 12:eec17d54a646 | 368 | osSignalSet((osThreadId)mainThreadID, 0x1); |
eisd | 12:eec17d54a646 | 369 | } |
eisd | 12:eec17d54a646 | 370 | |
eisd | 12:eec17d54a646 | 371 | int main() |
eisd | 12:eec17d54a646 | 372 | { |
eisd | 13:ab524cb4f768 | 373 | RtosTimer rx_snooze(&rx_snoozer, osTimerOnce, (void*)osThreadGetId()); |
eisd | 12:eec17d54a646 | 374 | |
eisd | 12:eec17d54a646 | 375 | #ifndef USBSerial |
eisd | 12:eec17d54a646 | 376 | pc.baud(115200); |
eisd | 12:eec17d54a646 | 377 | #endif |
eisd | 12:eec17d54a646 | 378 | |
eisd | 13:ab524cb4f768 | 379 | r = g = b = 1; |
eisd | 6:3482f1e19d71 | 380 | gnd = 0; |
eisd | 6:3482f1e19d71 | 381 | ir.period_us(1000/38); |
eisd | 6:3482f1e19d71 | 382 | |
eisd | 7:f720032c2fb9 | 383 | speaker_gnd = 0; |
eisd | 7:f720032c2fb9 | 384 | speaker = 0; |
eisd | 7:f720032c2fb9 | 385 | //speaker.period(1.0/200000.0); |
eisd | 7:f720032c2fb9 | 386 | |
eisd | 7:f720032c2fb9 | 387 | /* |
eisd | 7:f720032c2fb9 | 388 | speaker = 0.2; |
eisd | 7:f720032c2fb9 | 389 | while(1) { |
eisd | 7:f720032c2fb9 | 390 | speaker.period(.8/554.365); |
eisd | 7:f720032c2fb9 | 391 | wait(.8); |
eisd | 7:f720032c2fb9 | 392 | speaker.period(.8/523.251); |
eisd | 7:f720032c2fb9 | 393 | wait(.8); |
eisd | 7:f720032c2fb9 | 394 | } |
eisd | 7:f720032c2fb9 | 395 | //speaker = 0.2; |
eisd | 7:f720032c2fb9 | 396 | // set up a timer to be used for sample rate interrupts |
eisd | 7:f720032c2fb9 | 397 | Ticker Sample_Period; |
eisd | 7:f720032c2fb9 | 398 | // precompute 128 sample points on one sine wave cycle |
eisd | 7:f720032c2fb9 | 399 | // used for continuous sine wave output later |
eisd | 7:f720032c2fb9 | 400 | for(int k=0; k<128; k++) { |
eisd | 7:f720032c2fb9 | 401 | wave[k]=((1.0 + sin((float(k)/128.0*6.28318530717959)))/2.0); |
eisd | 7:f720032c2fb9 | 402 | // scale the sine wave from 0.0 to 1.0 - as needed for AnalogOut arg |
eisd | 7:f720032c2fb9 | 403 | } |
eisd | 7:f720032c2fb9 | 404 | // turn on timer interrupts to start sine wave output |
eisd | 7:f720032c2fb9 | 405 | // sample rate is 500Hz with 128 samples per cycle on sine wave |
eisd | 7:f720032c2fb9 | 406 | while(1) { |
eisd | 7:f720032c2fb9 | 407 | Sample_Period.detach(); |
eisd | 7:f720032c2fb9 | 408 | Sample_Period.attach(&Sample_timer_interrupt, 1.0/(554.365*128)); |
eisd | 7:f720032c2fb9 | 409 | wait(.8); |
eisd | 7:f720032c2fb9 | 410 | Sample_Period.detach(); |
eisd | 7:f720032c2fb9 | 411 | Sample_Period.attach(&Sample_timer_interrupt, 1.0/(523.251*128)); |
eisd | 7:f720032c2fb9 | 412 | wait(.8); |
eisd | 7:f720032c2fb9 | 413 | } |
eisd | 7:f720032c2fb9 | 414 | // everything else needed is already being done by the interrupt routine |
eisd | 7:f720032c2fb9 | 415 | */ |
eisd | 7:f720032c2fb9 | 416 | |
eisd | 5:7af5760d7e8f | 417 | nunchuk n1, n2; |
eisd | 5:7af5760d7e8f | 418 | nunchuk *n = &n1, *next = &n2; |
eisd | 22:d4d991c7a40b | 419 | bool sender = nun_init(n); |
eisd | 4:c9711f0cd097 | 420 | if (sender) { |
eisd | 4:c9711f0cd097 | 421 | pc.printf("chuck attached\r\n"); |
eisd | 4:c9711f0cd097 | 422 | radio.initialize(FREQUENCY, NODE_ID, NETWORKID); |
eisd | 15:8888e36afe43 | 423 | central_time.start(); |
eisd | 4:c9711f0cd097 | 424 | } else { |
eisd | 4:c9711f0cd097 | 425 | pc.printf("chuck unavailable\r\n"); |
eisd | 4:c9711f0cd097 | 426 | radio.initialize(FREQUENCY, GATEWAY_ID, NETWORKID); |
eisd | 15:8888e36afe43 | 427 | // only relevant to the receiver |
eisd | 5:7af5760d7e8f | 428 | thread = new Thread(ir_thread); |
eisd | 15:8888e36afe43 | 429 | rx_last_contact.start(); |
eisd | 15:8888e36afe43 | 430 | // these break the nunchuk |
eisd | 12:eec17d54a646 | 431 | wave = new float[i]; |
eisd | 12:eec17d54a646 | 432 | int m = i-128; |
eisd | 12:eec17d54a646 | 433 | for(int k = 0; k < m; k++) { |
eisd | 12:eec17d54a646 | 434 | // ramp up |
eisd | 12:eec17d54a646 | 435 | wave[i-k-1] = 1.0/(1000+k*400.0/m); |
eisd | 12:eec17d54a646 | 436 | } |
eisd | 12:eec17d54a646 | 437 | for(int k = 0; k < 128; k++) { |
eisd | 12:eec17d54a646 | 438 | // LFO |
eisd | 12:eec17d54a646 | 439 | wave[127-k] = 1.0/(1400+200*sin(k/128.0*6.28318530717959)); |
eisd | 12:eec17d54a646 | 440 | } |
eisd | 15:8888e36afe43 | 441 | WakeUp::calibrate(); |
eisd | 25:74edb9fc46c6 | 442 | light_bar_init(); |
eisd | 26:ddfb293f4606 | 443 | indicator_init(); |
eisd | 15:8888e36afe43 | 444 | } |
eisd | 3:9091adbed369 | 445 | radio.encrypt("0123456789054321"); |
eisd | 2:04fdd571a385 | 446 | //radio.promiscuous(false); |
eisd | 3:9091adbed369 | 447 | radio.setHighPower(true); |
eisd | 3:9091adbed369 | 448 | radio.setPowerLevel(20); |
eisd | 3:9091adbed369 | 449 | radio.rcCalibration(); |
eisd | 2:04fdd571a385 | 450 | //radio.readAllRegs(); |
eisd | 2:04fdd571a385 | 451 | pc.printf("temp %d\r\n", radio.readTemperature(-1)); |
eisd | 1:de8c34c9ccdf | 452 | |
eisd | 25:74edb9fc46c6 | 453 | //Thread t_sleep_loop(&sleep_loop); |
eisd | 6:3482f1e19d71 | 454 | bool read = false; |
eisd | 1:de8c34c9ccdf | 455 | while(1) { |
eisd | 7:f720032c2fb9 | 456 | if (sender) { |
eisd | 25:74edb9fc46c6 | 457 | if (/*nun_settled++ > 2 && */central_time_trip |= (central_time.read() > 5)) { |
eisd | 16:3490cdea4986 | 458 | #if DEBUG |
eisd | 16:3490cdea4986 | 459 | pc.printf("snooze tx %f\r\n", central_time.read()); |
eisd | 16:3490cdea4986 | 460 | g = 0; Thread::wait(10); g = 1; Thread::wait(10); |
eisd | 16:3490cdea4986 | 461 | #endif |
eisd | 17:b9030e94b622 | 462 | nun_sleep(); |
eisd | 25:74edb9fc46c6 | 463 | radio.sleep(); |
eisd | 16:3490cdea4986 | 464 | Thread::wait(10); |
eisd | 25:74edb9fc46c6 | 465 | WakeUp::set(2); |
eisd | 16:3490cdea4986 | 466 | deepsleep(); |
eisd | 22:d4d991c7a40b | 467 | nun_init(next); |
eisd | 16:3490cdea4986 | 468 | #if DEBUG |
eisd | 16:3490cdea4986 | 469 | r = 0; Thread::wait(10); r = 1; Thread::wait(10); |
eisd | 16:3490cdea4986 | 470 | pc.printf("unsnooze tx\r\n"); |
eisd | 16:3490cdea4986 | 471 | #endif |
eisd | 16:3490cdea4986 | 472 | } |
eisd | 18:18c91572768a | 473 | nunchuk *last = n; |
eisd | 18:18c91572768a | 474 | n = next; |
eisd | 18:18c91572768a | 475 | next = last; |
eisd | 17:b9030e94b622 | 476 | read = nun->Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z); |
eisd | 7:f720032c2fb9 | 477 | n->sum = 0; |
eisd | 7:f720032c2fb9 | 478 | n->sum = calculate_crc8((char*)n, sizeof(struct nunchuk)); |
eisd | 7:f720032c2fb9 | 479 | } |
eisd | 12:eec17d54a646 | 480 | else if (rx_snoozed() && radio.receiveDone()) { |
eisd | 13:ab524cb4f768 | 481 | rx_last_contact.reset(); |
eisd | 25:74edb9fc46c6 | 482 | rx_last_contact_trip = 0; |
eisd | 12:eec17d54a646 | 483 | rx_snooze.stop(); |
eisd | 12:eec17d54a646 | 484 | rx_to_snooze = true; |
eisd | 23:e08b10694786 | 485 | //pc.printf("rssi %d\r\n", radio.RSSI); |
eisd | 4:c9711f0cd097 | 486 | read = (radio.DATALEN == sizeof(struct nunchuk)); |
eisd | 5:7af5760d7e8f | 487 | if (read) { |
eisd | 6:3482f1e19d71 | 488 | memcpy((void*)next, (const void*)radio.DATA, radio.DATALEN); |
eisd | 7:f720032c2fb9 | 489 | uint8_t sum = next->sum; |
eisd | 7:f720032c2fb9 | 490 | next->sum = 0; |
eisd | 7:f720032c2fb9 | 491 | read = sum == calculate_crc8((char*)next, sizeof(struct nunchuk)); |
eisd | 7:f720032c2fb9 | 492 | if (read) { |
eisd | 7:f720032c2fb9 | 493 | nunchuk *last = n; |
eisd | 7:f720032c2fb9 | 494 | n = next; |
eisd | 7:f720032c2fb9 | 495 | next = last; |
eisd | 7:f720032c2fb9 | 496 | } |
eisd | 7:f720032c2fb9 | 497 | } |
eisd | 25:74edb9fc46c6 | 498 | if (radio.DATALEN > 30) |
eisd | 25:74edb9fc46c6 | 499 | pc.printf((const char*)radio.DATA); |
eisd | 7:f720032c2fb9 | 500 | if (!read) |
eisd | 7:f720032c2fb9 | 501 | pc.printf("len %d\r\n", radio.DATALEN); |
eisd | 12:eec17d54a646 | 502 | } else if (rx_to_snooze) { |
eisd | 25:74edb9fc46c6 | 503 | rx_snooze.start(100); |
eisd | 12:eec17d54a646 | 504 | rx_to_snooze = false; |
eisd | 4:c9711f0cd097 | 505 | } |
eisd | 1:de8c34c9ccdf | 506 | if(read) |
eisd | 1:de8c34c9ccdf | 507 | { |
eisd | 7:f720032c2fb9 | 508 | Siren_state(n->C); |
eisd | 8:5f933580b560 | 509 | Headlight_state(n->C, n->Z); |
eisd | 5:7af5760d7e8f | 510 | float x = n->X - 128, y = n->Y - 128; |
eisd | 3:9091adbed369 | 511 | float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5; |
eisd | 25:74edb9fc46c6 | 512 | mag = sqrt(R)/42; if (mag > 2) mag = 10; |
eisd | 25:74edb9fc46c6 | 513 | //const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" }; |
eisd | 25:74edb9fc46c6 | 514 | |
eisd | 25:74edb9fc46c6 | 515 | int c = 7; |
eisd | 3:9091adbed369 | 516 | if (p > -4) c = 0; |
eisd | 3:9091adbed369 | 517 | if (p > -3) c = 1; |
eisd | 3:9091adbed369 | 518 | if (p > -2) c = 2; |
eisd | 3:9091adbed369 | 519 | if (p > -1) c = 3; |
eisd | 3:9091adbed369 | 520 | if (p > 0) c = 4; |
eisd | 3:9091adbed369 | 521 | if (p > 1) c = 5; |
eisd | 3:9091adbed369 | 522 | if (p > 2) c = 6; |
eisd | 3:9091adbed369 | 523 | if (p > 3) c = 7; |
eisd | 5:7af5760d7e8f | 524 | direction = c; |
eisd | 1:de8c34c9ccdf | 525 | |
eisd | 25:74edb9fc46c6 | 526 | //#if DEBUG > 1 |
eisd | 6:3482f1e19d71 | 527 | pc.printf("%d: ", sizeof(struct nunchuk)); |
eisd | 25:74edb9fc46c6 | 528 | pc.printf("x%3d y%3d c%1d z%1d -- ", n->X, n->Y, n->C, n->Z); |
eisd | 25:74edb9fc46c6 | 529 | pc.printf("x%d y%d z%d -- %8.2f %8.4f %s %d \r\n", n->aX, n->aY, n->aZ, R, p, directions[direction], mag); |
eisd | 3:9091adbed369 | 530 | |
eisd | 3:9091adbed369 | 531 | //radio.send(GATEWAY_ID, (const void*)"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA", 50, false); |
eisd | 25:74edb9fc46c6 | 532 | //#endif |
eisd | 16:3490cdea4986 | 533 | if (sender) |
eisd | 25:74edb9fc46c6 | 534 | if (central_time.read() < 1) |
eisd | 16:3490cdea4986 | 535 | radio.send(GATEWAY_ID, (const void*)n, sizeof(struct nunchuk), false); |
eisd | 16:3490cdea4986 | 536 | else |
eisd | 16:3490cdea4986 | 537 | radio.sleep(); |
eisd | 3:9091adbed369 | 538 | |
eisd | 25:74edb9fc46c6 | 539 | #if DEBUG |
eisd | 25:74edb9fc46c6 | 540 | char wake[100]; |
eisd | 25:74edb9fc46c6 | 541 | #endif |
eisd | 1:de8c34c9ccdf | 542 | #ifdef TARGET_KL25Z |
eisd | 26:ddfb293f4606 | 543 | indicators = 0; |
eisd | 27:c559bfe67109 | 544 | if (R <= 0.1) { |
eisd | 18:18c91572768a | 545 | double twitch = pow(n->aX - next->aX, 2.0) + pow(n->aY - next->aY, 2.0) + pow(n->aZ - next->aZ, 2.0); |
eisd | 18:18c91572768a | 546 | #if DEBUG > 1 |
eisd | 18:18c91572768a | 547 | pc.printf("twitch %f\r\n", twitch); |
eisd | 18:18c91572768a | 548 | #endif |
eisd | 1:de8c34c9ccdf | 549 | r = 1.0f; |
eisd | 1:de8c34c9ccdf | 550 | g = 1.0f; |
eisd | 1:de8c34c9ccdf | 551 | b = 1.0f; |
eisd | 25:74edb9fc46c6 | 552 | if (n->C || n->Z || /*nun_settled > 2 &&*/ twitch > 1000) { |
eisd | 23:e08b10694786 | 553 | Thread::wait(10); |
eisd | 23:e08b10694786 | 554 | #if DEBUG |
eisd | 23:e08b10694786 | 555 | pc.printf("c %d z %d twitch %f\r\n", n->C, n->Z, twitch); |
eisd | 25:74edb9fc46c6 | 556 | snprintf(wake, 100, "Wake for twitch %f\r\n", twitch); |
eisd | 25:74edb9fc46c6 | 557 | pc.printf(wake); |
eisd | 25:74edb9fc46c6 | 558 | radio.send(GATEWAY_ID, (const void*)wake, 100, false); |
eisd | 23:e08b10694786 | 559 | #endif |
eisd | 23:e08b10694786 | 560 | central_time.reset(); |
eisd | 25:74edb9fc46c6 | 561 | central_time_trip = 0; |
eisd | 23:e08b10694786 | 562 | } |
eisd | 1:de8c34c9ccdf | 563 | } else { |
eisd | 27:c559bfe67109 | 564 | switch(direction) { |
eisd | 27:c559bfe67109 | 565 | // left |
eisd | 27:c559bfe67109 | 566 | case 0: //XR(); break; |
eisd | 27:c559bfe67109 | 567 | case 6: //XF(); break; |
eisd | 27:c559bfe67109 | 568 | case 7: //RF(); break; |
eisd | 27:c559bfe67109 | 569 | indicators = 2; break; |
eisd | 27:c559bfe67109 | 570 | // right |
eisd | 27:c559bfe67109 | 571 | case 2: //RX(); break; |
eisd | 27:c559bfe67109 | 572 | case 3: //FR(); break; |
eisd | 27:c559bfe67109 | 573 | case 4: //FX(); break; |
eisd | 27:c559bfe67109 | 574 | indicators = 1; break; |
eisd | 27:c559bfe67109 | 575 | // neither |
eisd | 27:c559bfe67109 | 576 | case 1: //RR(); break; |
eisd | 27:c559bfe67109 | 577 | case 5: //FF(); break; |
eisd | 27:c559bfe67109 | 578 | break; |
eisd | 27:c559bfe67109 | 579 | } |
eisd | 25:74edb9fc46c6 | 580 | #if DEBUG |
eisd | 25:74edb9fc46c6 | 581 | snprintf(wake, 100, "Wake for R %f\r\n", R); |
eisd | 25:74edb9fc46c6 | 582 | pc.printf(wake); |
eisd | 25:74edb9fc46c6 | 583 | radio.send(GATEWAY_ID, (const void*)wake, 100, false); |
eisd | 25:74edb9fc46c6 | 584 | #endif |
eisd | 27:c559bfe67109 | 585 | if (R < 40) { |
eisd | 27:c559bfe67109 | 586 | if (!central) { |
eisd | 27:c559bfe67109 | 587 | pc.printf("central\r\n"); |
eisd | 27:c559bfe67109 | 588 | central = true; |
eisd | 27:c559bfe67109 | 589 | } |
eisd | 27:c559bfe67109 | 590 | } else { |
eisd | 27:c559bfe67109 | 591 | central_time.reset(); |
eisd | 27:c559bfe67109 | 592 | central_time_trip = 0; |
eisd | 27:c559bfe67109 | 593 | if (central) { |
eisd | 27:c559bfe67109 | 594 | pc.printf("go %s\r\n", directions[direction]); |
eisd | 27:c559bfe67109 | 595 | central = false; |
eisd | 27:c559bfe67109 | 596 | } |
eisd | 3:9091adbed369 | 597 | } |
eisd | 3:9091adbed369 | 598 | R = R/20000; |
eisd | 1:de8c34c9ccdf | 599 | float pal[8][3] = { |
eisd | 1:de8c34c9ccdf | 600 | { 0, 0, 1 }, |
eisd | 1:de8c34c9ccdf | 601 | { 0, 1, 1 }, |
eisd | 1:de8c34c9ccdf | 602 | { 0, 1, 0 }, |
eisd | 1:de8c34c9ccdf | 603 | { 1, 1, 0 }, |
eisd | 1:de8c34c9ccdf | 604 | { 1, 0.5, 0 }, |
eisd | 1:de8c34c9ccdf | 605 | { 1, 0, 0 }, |
eisd | 1:de8c34c9ccdf | 606 | { 1, 0, 1 }, |
eisd | 1:de8c34c9ccdf | 607 | { 0.5, 0, 1 }, |
eisd | 1:de8c34c9ccdf | 608 | }; |
eisd | 6:3482f1e19d71 | 609 | c = (c + 4) % 8; |
eisd | 1:de8c34c9ccdf | 610 | r = 1.0f - pal[c][0] * R; |
eisd | 1:de8c34c9ccdf | 611 | g = 1.0f - pal[c][1] * R; |
eisd | 1:de8c34c9ccdf | 612 | b = 1.0f - pal[c][2] * R; |
eisd | 1:de8c34c9ccdf | 613 | } |
eisd | 1:de8c34c9ccdf | 614 | #endif |
eisd | 1:de8c34c9ccdf | 615 | } |
eisd | 4:c9711f0cd097 | 616 | else if (sender) |
eisd | 1:de8c34c9ccdf | 617 | { |
eisd | 1:de8c34c9ccdf | 618 | pc.printf("Error\r\n"); |
eisd | 1:de8c34c9ccdf | 619 | } |
eisd | 4:c9711f0cd097 | 620 | if (sender) |
eisd | 4:c9711f0cd097 | 621 | wait(0.01); |
eisd | 5:7af5760d7e8f | 622 | else |
eisd | 5:7af5760d7e8f | 623 | Thread::wait(10); |
eisd | 1:de8c34c9ccdf | 624 | } |
eisd | 1:de8c34c9ccdf | 625 | } |