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@13:ab524cb4f768, 2016-06-28 (annotated)

Committer:
eisd
Date:
Tue Jun 28 10:17:09 2016 +0000
Revision:
13:ab524cb4f768
Parent:
12:eec17d54a646
Child:
14:579137c8ceac
Longer, deeper sleep on receiver

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