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

Committer:
eisd
Date:
Wed Aug 02 15:30:30 2017 +0000
Revision:
28:e7b487a50937
Parent:
27:c559bfe67109
Publishing modules

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 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 }