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@24:46777890b7d9, 2016-06-28 (annotated)

Committer:
eisd
Date:
Tue Jun 28 19:15:52 2016 +0000
Revision:
24:46777890b7d9
Parent:
23:e08b10694786 
Report RSSI with ACK

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