David Eisner / Mbed 2 deprecated chuk

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 Greg Brush

main.cpp@25:74edb9fc46c6, 2017-01-04 (annotated)

Committer:
eisd
Date:
Wed Jan 04 06:21:18 2017 +0000
Revision:
25:74edb9fc46c6
Parent:
23:e08b10694786
Child:
26:ddfb293f4606
Tweaks and TLC59108-driven light bar

Who changed what in which revision?

UserRevisionLine numberNew 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 25:74edb9fc46c6 251 TLC59108 *light_bar;
eisd 25:74edb9fc46c6 252 #define light_bar_phases 10
eisd 25:74edb9fc46c6 253 uint8_t light_bar_pattern[light_bar_phases][4] = {
eisd 25:74edb9fc46c6 254 { 0xFF, 0xFF, 0x00, 0x00 },
eisd 25:74edb9fc46c6 255 { 0x00, 0xFF, 0x00, 0x00 },
eisd 25:74edb9fc46c6 256 { 0xFF, 0xFF, 0x00, 0x00 },
eisd 25:74edb9fc46c6 257 { 0x00, 0xFF, 0x00, 0x00 },
eisd 25:74edb9fc46c6 258 { 0xFF, 0x00, 0xFF, 0x00 },
eisd 25:74edb9fc46c6 259 { 0x00, 0x00, 0xFF, 0xFF },
eisd 25:74edb9fc46c6 260 { 0x00, 0x00, 0xFF, 0x00 },
eisd 25:74edb9fc46c6 261 { 0x00, 0x00, 0xFF, 0xFF },
eisd 25:74edb9fc46c6 262 { 0x00, 0x00, 0xFF, 0x00 },
eisd 25:74edb9fc46c6 263 { 0x00, 0xFF, 0x00, 0xFF } };
eisd 25:74edb9fc46c6 264 uint8_t light_bar_phase = 0;
eisd 25:74edb9fc46c6 265
eisd 25:74edb9fc46c6 266 void light_bar_thread(const void*) {
eisd 25:74edb9fc46c6 267 while(true) {
eisd 25:74edb9fc46c6 268 Thread::wait(20);
eisd 25:74edb9fc46c6 269 light_bar_phase = (light_bar_phase + 1) % light_bar_phases;
eisd 25:74edb9fc46c6 270 if (Headlight_mode == 2)
eisd 25:74edb9fc46c6 271 pc.printf("light bar %d\r\n", light_bar->setBrightness(light_bar_pattern[light_bar_phase]));
eisd 25:74edb9fc46c6 272 else
eisd 25:74edb9fc46c6 273 light_bar->setBrightness((uint8_t)0);
eisd 25:74edb9fc46c6 274 }
eisd 25:74edb9fc46c6 275 }
eisd 25:74edb9fc46c6 276 Thread *t_light_bar_thread;
eisd 25:74edb9fc46c6 277
eisd 25:74edb9fc46c6 278 void light_bar_init() {
eisd 25:74edb9fc46c6 279 light_bar = new TLC59108(PTE0, PTE1);
eisd 25:74edb9fc46c6 280 pc.printf("light bar init %d\r\n", light_bar->setLedOutputMode(TLC59108::LED_MODE::PWM_INDGRP));
eisd 25:74edb9fc46c6 281 light_bar->setGroupBrightness(100);
eisd 25:74edb9fc46c6 282 t_light_bar_thread = new Thread(&light_bar_thread);
eisd 25:74edb9fc46c6 283 }
eisd 25:74edb9fc46c6 284
eisd 12:eec17d54a646 285 void sleep_loop(void const *argument) {
eisd 12:eec17d54a646 286 while (true) {
eisd 12:eec17d54a646 287 sleep();
eisd 12:eec17d54a646 288 Thread::wait(100);
eisd 12:eec17d54a646 289 }
eisd 12:eec17d54a646 290 }
gbrush 0:7c98bcd8a245 291
eisd 2:04fdd571a385 292 #ifdef TARGET_KL25Z
eisd 6:3482f1e19d71 293 PwmOut r(LED_RED);
eisd 6:3482f1e19d71 294 //float r = 1.0f;
eisd 6:3482f1e19d71 295 PwmOut g(LED_GREEN);
eisd 6:3482f1e19d71 296 //float g = 1.0f;
eisd 6:3482f1e19d71 297 PwmOut b(LED_BLUE);
eisd 6:3482f1e19d71 298 //float b = 1.0f;
eisd 17:b9030e94b622 299 DigitalOut nun_vdd(PTE31);
eisd 23:e08b10694786 300 WiiChuck *nun;
eisd 23:e08b10694786 301 uint8_t nun_settled = 0;
eisd 22:d4d991c7a40b 302 bool nun_init(nunchuk *next) {
eisd 25:74edb9fc46c6 303 //return false;
eisd 25:74edb9fc46c6 304 for (int i = 0; i < 5; i++) {
eisd 22:d4d991c7a40b 305 if (nun)
eisd 22:d4d991c7a40b 306 delete nun;
eisd 23:e08b10694786 307 nun_vdd = 1;
eisd 23:e08b10694786 308 Thread::wait(50);
eisd 22:d4d991c7a40b 309 nun = new WiiChuck(PTE0, PTE1, pc);
eisd 22:d4d991c7a40b 310 if (nun->Read(&next->X, &next->Y, &next->aX, &next->aY, &next->aZ, &next->C, &next->Z)) {
eisd 23:e08b10694786 311 nun_settled = 0;
eisd 22:d4d991c7a40b 312 Thread::wait(10);
eisd 22:d4d991c7a40b 313 return true;
eisd 22:d4d991c7a40b 314 }
eisd 22:d4d991c7a40b 315 pc.printf("nun_init error\r\n");
eisd 22:d4d991c7a40b 316 nun_vdd = 0;
eisd 23:e08b10694786 317 Thread::wait(50);
eisd 22:d4d991c7a40b 318 }
eisd 22:d4d991c7a40b 319 return false;
eisd 17:b9030e94b622 320 }
eisd 17:b9030e94b622 321 void nun_sleep() {
eisd 17:b9030e94b622 322 nun_vdd = 0;
eisd 17:b9030e94b622 323 }
eisd 2:04fdd571a385 324 RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13);
eisd 2:04fdd571a385 325 #else
eisd 17:b9030e94b622 326 WiiChuck *nun = new WiiChuck(p9, p10, pc);
eisd 22:d4d991c7a40b 327 bool nun_init(nunchuk *) {}
eisd 17:b9030e94b622 328 void nun_sleep() {}
eisd 2:04fdd571a385 329 #endif
eisd 2:04fdd571a385 330
eisd 13:ab524cb4f768 331 Timer rx_last_contact;
eisd 25:74edb9fc46c6 332 bool rx_last_contact_trip = 0;
eisd 12:eec17d54a646 333 bool rx_to_snooze = true;
eisd 12:eec17d54a646 334 bool rx_snoozing = false;
eisd 12:eec17d54a646 335 bool rx_snoozed() {
eisd 12:eec17d54a646 336 if (rx_snoozing) {
eisd 12:eec17d54a646 337 pc.printf("still snoozing\r\n");
eisd 12:eec17d54a646 338 Thread::signal_wait(0x1);
eisd 12:eec17d54a646 339 pc.printf("signalled?\r\n");
eisd 12:eec17d54a646 340 }
eisd 12:eec17d54a646 341 return true;
eisd 12:eec17d54a646 342 }
eisd 12:eec17d54a646 343
eisd 12:eec17d54a646 344 void rx_snoozer(void const *mainThreadID) {
eisd 13:ab524cb4f768 345 pc.printf("snooze rx %f\r\n", rx_last_contact.read());
eisd 12:eec17d54a646 346
eisd 12:eec17d54a646 347 rx_snoozing = true;
eisd 13:ab524cb4f768 348 radio.sleep();
eisd 25:74edb9fc46c6 349 WakeUp::set((rx_last_contact_trip |= (rx_last_contact.read() < 60))? 2 : 10);
eisd 13:ab524cb4f768 350 deepsleep();
eisd 23:e08b10694786 351 stops_sent = 0;
eisd 12:eec17d54a646 352 rx_snoozing = false;
eisd 12:eec17d54a646 353 rx_to_snooze = true;
eisd 12:eec17d54a646 354 pc.printf("unsnooze rx\r\n");
eisd 12:eec17d54a646 355 osSignalSet((osThreadId)mainThreadID, 0x1);
eisd 12:eec17d54a646 356 }
eisd 12:eec17d54a646 357
eisd 12:eec17d54a646 358 int main()
eisd 12:eec17d54a646 359 {
eisd 13:ab524cb4f768 360 RtosTimer rx_snooze(&rx_snoozer, osTimerOnce, (void*)osThreadGetId());
eisd 12:eec17d54a646 361
eisd 12:eec17d54a646 362 #ifndef USBSerial
eisd 12:eec17d54a646 363 pc.baud(115200);
eisd 12:eec17d54a646 364 #endif
eisd 12:eec17d54a646 365
eisd 13:ab524cb4f768 366 r = g = b = 1;
eisd 6:3482f1e19d71 367 gnd = 0;
eisd 6:3482f1e19d71 368 ir.period_us(1000/38);
eisd 6:3482f1e19d71 369
eisd 7:f720032c2fb9 370 speaker_gnd = 0;
eisd 7:f720032c2fb9 371 speaker = 0;
eisd 7:f720032c2fb9 372 //speaker.period(1.0/200000.0);
eisd 7:f720032c2fb9 373
eisd 7:f720032c2fb9 374 /*
eisd 7:f720032c2fb9 375 speaker = 0.2;
eisd 7:f720032c2fb9 376 while(1) {
eisd 7:f720032c2fb9 377 speaker.period(.8/554.365);
eisd 7:f720032c2fb9 378 wait(.8);
eisd 7:f720032c2fb9 379 speaker.period(.8/523.251);
eisd 7:f720032c2fb9 380 wait(.8);
eisd 7:f720032c2fb9 381 }
eisd 7:f720032c2fb9 382 //speaker = 0.2;
eisd 7:f720032c2fb9 383 // set up a timer to be used for sample rate interrupts
eisd 7:f720032c2fb9 384 Ticker Sample_Period;
eisd 7:f720032c2fb9 385 // precompute 128 sample points on one sine wave cycle
eisd 7:f720032c2fb9 386 // used for continuous sine wave output later
eisd 7:f720032c2fb9 387 for(int k=0; k<128; k++) {
eisd 7:f720032c2fb9 388 wave[k]=((1.0 + sin((float(k)/128.0*6.28318530717959)))/2.0);
eisd 7:f720032c2fb9 389 // scale the sine wave from 0.0 to 1.0 - as needed for AnalogOut arg
eisd 7:f720032c2fb9 390 }
eisd 7:f720032c2fb9 391 // turn on timer interrupts to start sine wave output
eisd 7:f720032c2fb9 392 // sample rate is 500Hz with 128 samples per cycle on sine wave
eisd 7:f720032c2fb9 393 while(1) {
eisd 7:f720032c2fb9 394 Sample_Period.detach();
eisd 7:f720032c2fb9 395 Sample_Period.attach(&Sample_timer_interrupt, 1.0/(554.365*128));
eisd 7:f720032c2fb9 396 wait(.8);
eisd 7:f720032c2fb9 397 Sample_Period.detach();
eisd 7:f720032c2fb9 398 Sample_Period.attach(&Sample_timer_interrupt, 1.0/(523.251*128));
eisd 7:f720032c2fb9 399 wait(.8);
eisd 7:f720032c2fb9 400 }
eisd 7:f720032c2fb9 401 // everything else needed is already being done by the interrupt routine
eisd 7:f720032c2fb9 402 */
eisd 7:f720032c2fb9 403
eisd 5:7af5760d7e8f 404 nunchuk n1, n2;
eisd 5:7af5760d7e8f 405 nunchuk *n = &n1, *next = &n2;
eisd 22:d4d991c7a40b 406 bool sender = nun_init(n);
eisd 4:c9711f0cd097 407 if (sender) {
eisd 4:c9711f0cd097 408 pc.printf("chuck attached\r\n");
eisd 4:c9711f0cd097 409 radio.initialize(FREQUENCY, NODE_ID, NETWORKID);
eisd 15:8888e36afe43 410 central_time.start();
eisd 4:c9711f0cd097 411 } else {
eisd 4:c9711f0cd097 412 pc.printf("chuck unavailable\r\n");
eisd 4:c9711f0cd097 413 radio.initialize(FREQUENCY, GATEWAY_ID, NETWORKID);
eisd 15:8888e36afe43 414 // only relevant to the receiver
eisd 5:7af5760d7e8f 415 thread = new Thread(ir_thread);
eisd 15:8888e36afe43 416 rx_last_contact.start();
eisd 15:8888e36afe43 417 // these break the nunchuk
eisd 12:eec17d54a646 418 wave = new float[i];
eisd 12:eec17d54a646 419 int m = i-128;
eisd 12:eec17d54a646 420 for(int k = 0; k < m; k++) {
eisd 12:eec17d54a646 421 // ramp up
eisd 12:eec17d54a646 422 wave[i-k-1] = 1.0/(1000+k*400.0/m);
eisd 12:eec17d54a646 423 }
eisd 12:eec17d54a646 424 for(int k = 0; k < 128; k++) {
eisd 12:eec17d54a646 425 // LFO
eisd 12:eec17d54a646 426 wave[127-k] = 1.0/(1400+200*sin(k/128.0*6.28318530717959));
eisd 12:eec17d54a646 427 }
eisd 15:8888e36afe43 428 WakeUp::calibrate();
eisd 25:74edb9fc46c6 429 light_bar_init();
eisd 15:8888e36afe43 430 }
eisd 3:9091adbed369 431 radio.encrypt("0123456789054321");
eisd 2:04fdd571a385 432 //radio.promiscuous(false);
eisd 3:9091adbed369 433 radio.setHighPower(true);
eisd 3:9091adbed369 434 radio.setPowerLevel(20);
eisd 3:9091adbed369 435 radio.rcCalibration();
eisd 2:04fdd571a385 436 //radio.readAllRegs();
eisd 2:04fdd571a385 437 pc.printf("temp %d\r\n", radio.readTemperature(-1));
eisd 1:de8c34c9ccdf 438
eisd 25:74edb9fc46c6 439 //Thread t_sleep_loop(&sleep_loop);
eisd 6:3482f1e19d71 440 bool read = false;
eisd 1:de8c34c9ccdf 441 while(1) {
eisd 7:f720032c2fb9 442 if (sender) {
eisd 25:74edb9fc46c6 443 if (/*nun_settled++ > 2 && */central_time_trip |= (central_time.read() > 5)) {
eisd 16:3490cdea4986 444 #if DEBUG
eisd 16:3490cdea4986 445 pc.printf("snooze tx %f\r\n", central_time.read());
eisd 16:3490cdea4986 446 g = 0; Thread::wait(10); g = 1; Thread::wait(10);
eisd 16:3490cdea4986 447 #endif
eisd 17:b9030e94b622 448 nun_sleep();
eisd 25:74edb9fc46c6 449 radio.sleep();
eisd 16:3490cdea4986 450 Thread::wait(10);
eisd 25:74edb9fc46c6 451 WakeUp::set(2);
eisd 16:3490cdea4986 452 deepsleep();
eisd 22:d4d991c7a40b 453 nun_init(next);
eisd 16:3490cdea4986 454 #if DEBUG
eisd 16:3490cdea4986 455 r = 0; Thread::wait(10); r = 1; Thread::wait(10);
eisd 16:3490cdea4986 456 pc.printf("unsnooze tx\r\n");
eisd 16:3490cdea4986 457 #endif
eisd 16:3490cdea4986 458 }
eisd 18:18c91572768a 459 nunchuk *last = n;
eisd 18:18c91572768a 460 n = next;
eisd 18:18c91572768a 461 next = last;
eisd 17:b9030e94b622 462 read = nun->Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
eisd 7:f720032c2fb9 463 n->sum = 0;
eisd 7:f720032c2fb9 464 n->sum = calculate_crc8((char*)n, sizeof(struct nunchuk));
eisd 7:f720032c2fb9 465 }
eisd 12:eec17d54a646 466 else if (rx_snoozed() && radio.receiveDone()) {
eisd 13:ab524cb4f768 467 rx_last_contact.reset();
eisd 25:74edb9fc46c6 468 rx_last_contact_trip = 0;
eisd 12:eec17d54a646 469 rx_snooze.stop();
eisd 12:eec17d54a646 470 rx_to_snooze = true;
eisd 23:e08b10694786 471 //pc.printf("rssi %d\r\n", radio.RSSI);
eisd 4:c9711f0cd097 472 read = (radio.DATALEN == sizeof(struct nunchuk));
eisd 5:7af5760d7e8f 473 if (read) {
eisd 6:3482f1e19d71 474 memcpy((void*)next, (const void*)radio.DATA, radio.DATALEN);
eisd 7:f720032c2fb9 475 uint8_t sum = next->sum;
eisd 7:f720032c2fb9 476 next->sum = 0;
eisd 7:f720032c2fb9 477 read = sum == calculate_crc8((char*)next, sizeof(struct nunchuk));
eisd 7:f720032c2fb9 478 if (read) {
eisd 7:f720032c2fb9 479 nunchuk *last = n;
eisd 7:f720032c2fb9 480 n = next;
eisd 7:f720032c2fb9 481 next = last;
eisd 7:f720032c2fb9 482 }
eisd 7:f720032c2fb9 483 }
eisd 25:74edb9fc46c6 484 if (radio.DATALEN > 30)
eisd 25:74edb9fc46c6 485 pc.printf((const char*)radio.DATA);
eisd 7:f720032c2fb9 486 if (!read)
eisd 7:f720032c2fb9 487 pc.printf("len %d\r\n", radio.DATALEN);
eisd 12:eec17d54a646 488 } else if (rx_to_snooze) {
eisd 25:74edb9fc46c6 489 rx_snooze.start(100);
eisd 12:eec17d54a646 490 rx_to_snooze = false;
eisd 4:c9711f0cd097 491 }
eisd 1:de8c34c9ccdf 492 if(read)
eisd 1:de8c34c9ccdf 493 {
eisd 7:f720032c2fb9 494 Siren_state(n->C);
eisd 8:5f933580b560 495 Headlight_state(n->C, n->Z);
eisd 5:7af5760d7e8f 496 float x = n->X - 128, y = n->Y - 128;
eisd 3:9091adbed369 497 float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5;
eisd 25:74edb9fc46c6 498 mag = sqrt(R)/42; if (mag > 2) mag = 10;
eisd 25:74edb9fc46c6 499 //const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" };
eisd 25:74edb9fc46c6 500
eisd 25:74edb9fc46c6 501 int c = 7;
eisd 3:9091adbed369 502 if (p > -4) c = 0;
eisd 3:9091adbed369 503 if (p > -3) c = 1;
eisd 3:9091adbed369 504 if (p > -2) c = 2;
eisd 3:9091adbed369 505 if (p > -1) c = 3;
eisd 3:9091adbed369 506 if (p > 0) c = 4;
eisd 3:9091adbed369 507 if (p > 1) c = 5;
eisd 3:9091adbed369 508 if (p > 2) c = 6;
eisd 3:9091adbed369 509 if (p > 3) c = 7;
eisd 5:7af5760d7e8f 510 direction = c;
eisd 1:de8c34c9ccdf 511
eisd 25:74edb9fc46c6 512 //#if DEBUG > 1
eisd 6:3482f1e19d71 513 pc.printf("%d: ", sizeof(struct nunchuk));
eisd 25:74edb9fc46c6 514 pc.printf("x%3d y%3d c%1d z%1d -- ", n->X, n->Y, n->C, n->Z);
eisd 25:74edb9fc46c6 515 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 516
eisd 3:9091adbed369 517 //radio.send(GATEWAY_ID, (const void*)"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA", 50, false);
eisd 25:74edb9fc46c6 518 //#endif
eisd 16:3490cdea4986 519 if (sender)
eisd 25:74edb9fc46c6 520 if (central_time.read() < 1)
eisd 16:3490cdea4986 521 radio.send(GATEWAY_ID, (const void*)n, sizeof(struct nunchuk), false);
eisd 16:3490cdea4986 522 else
eisd 16:3490cdea4986 523 radio.sleep();
eisd 3:9091adbed369 524
eisd 25:74edb9fc46c6 525 #if DEBUG
eisd 25:74edb9fc46c6 526 char wake[100];
eisd 25:74edb9fc46c6 527 #endif
eisd 1:de8c34c9ccdf 528 #ifdef TARGET_KL25Z
eisd 3:9091adbed369 529 if (R < 20) {
eisd 3:9091adbed369 530 if (!central) {
eisd 3:9091adbed369 531 pc.printf("central\r\n");
eisd 3:9091adbed369 532 central = true;
eisd 3:9091adbed369 533 }
eisd 18:18c91572768a 534 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 535 #if DEBUG > 1
eisd 18:18c91572768a 536 pc.printf("twitch %f\r\n", twitch);
eisd 18:18c91572768a 537 #endif
eisd 1:de8c34c9ccdf 538 r = 1.0f;
eisd 1:de8c34c9ccdf 539 g = 1.0f;
eisd 1:de8c34c9ccdf 540 b = 1.0f;
eisd 25:74edb9fc46c6 541 if (n->C || n->Z || /*nun_settled > 2 &&*/ twitch > 1000) {
eisd 23:e08b10694786 542 Thread::wait(10);
eisd 23:e08b10694786 543 #if DEBUG
eisd 23:e08b10694786 544 pc.printf("c %d z %d twitch %f\r\n", n->C, n->Z, twitch);
eisd 25:74edb9fc46c6 545 snprintf(wake, 100, "Wake for twitch %f\r\n", twitch);
eisd 25:74edb9fc46c6 546 pc.printf(wake);
eisd 25:74edb9fc46c6 547 radio.send(GATEWAY_ID, (const void*)wake, 100, false);
eisd 23:e08b10694786 548 #endif
eisd 23:e08b10694786 549 central_time.reset();
eisd 25:74edb9fc46c6 550 central_time_trip = 0;
eisd 23:e08b10694786 551 }
eisd 1:de8c34c9ccdf 552 } else {
eisd 25:74edb9fc46c6 553 #if DEBUG
eisd 25:74edb9fc46c6 554 snprintf(wake, 100, "Wake for R %f\r\n", R);
eisd 25:74edb9fc46c6 555 pc.printf(wake);
eisd 25:74edb9fc46c6 556 radio.send(GATEWAY_ID, (const void*)wake, 100, false);
eisd 25:74edb9fc46c6 557 #endif
eisd 15:8888e36afe43 558 central_time.reset();
eisd 25:74edb9fc46c6 559 central_time_trip = 0;
eisd 3:9091adbed369 560 if (central) {
eisd 6:3482f1e19d71 561 pc.printf("go %s\r\n", directions[direction]);
eisd 3:9091adbed369 562 central = false;
eisd 3:9091adbed369 563 }
eisd 3:9091adbed369 564 R = R/20000;
eisd 1:de8c34c9ccdf 565 float pal[8][3] = {
eisd 1:de8c34c9ccdf 566 { 0, 0, 1 },
eisd 1:de8c34c9ccdf 567 { 0, 1, 1 },
eisd 1:de8c34c9ccdf 568 { 0, 1, 0 },
eisd 1:de8c34c9ccdf 569 { 1, 1, 0 },
eisd 1:de8c34c9ccdf 570 { 1, 0.5, 0 },
eisd 1:de8c34c9ccdf 571 { 1, 0, 0 },
eisd 1:de8c34c9ccdf 572 { 1, 0, 1 },
eisd 1:de8c34c9ccdf 573 { 0.5, 0, 1 },
eisd 1:de8c34c9ccdf 574 };
eisd 6:3482f1e19d71 575 c = (c + 4) % 8;
eisd 1:de8c34c9ccdf 576 r = 1.0f - pal[c][0] * R;
eisd 1:de8c34c9ccdf 577 g = 1.0f - pal[c][1] * R;
eisd 1:de8c34c9ccdf 578 b = 1.0f - pal[c][2] * R;
eisd 1:de8c34c9ccdf 579 }
eisd 1:de8c34c9ccdf 580 #endif
eisd 1:de8c34c9ccdf 581 }
eisd 4:c9711f0cd097 582 else if (sender)
eisd 1:de8c34c9ccdf 583 {
eisd 1:de8c34c9ccdf 584 pc.printf("Error\r\n");
eisd 1:de8c34c9ccdf 585 }
eisd 4:c9711f0cd097 586 if (sender)
eisd 4:c9711f0cd097 587 wait(0.01);
eisd 5:7af5760d7e8f 588 else
eisd 5:7af5760d7e8f 589 Thread::wait(10);
eisd 1:de8c34c9ccdf 590 }
eisd 1:de8c34c9ccdf 591 }