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@7:f720032c2fb9, 2016-06-25 (annotated)

Committer:
eisd
Date:
Sat Jun 25 07:20:25 2016 +0000
Revision:
7:f720032c2fb9
Parent:
6:3482f1e19d71
Child:
8:5f933580b560
Chuck checksum; stop sending stop; basic siren

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 1:de8c34c9ccdf 3 #ifndef M_PI
eisd 1:de8c34c9ccdf 4 #define M_PI 3.14159265358979323846
eisd 1:de8c34c9ccdf 5 #endif
eisd 1:de8c34c9ccdf 6
eisd 1:de8c34c9ccdf 7 #include "WiiChuk_compat.hpp"
eisd 7:f720032c2fb9 8 #include "lib_crc.h"
gbrush 0:7c98bcd8a245 9
eisd 2:04fdd571a385 10 //#include "USBSerial.h"
eisd 2:04fdd571a385 11
eisd 2:04fdd571a385 12 #include "RFM69.h"
eisd 2:04fdd571a385 13 #define GATEWAY_ID 2
eisd 2:04fdd571a385 14 #define NODE_ID 1
eisd 2:04fdd571a385 15 #define NETWORKID 100
gbrush 0:7c98bcd8a245 16
eisd 2:04fdd571a385 17 // Uncomment only one of the following three to match radio frequency
eisd 2:04fdd571a385 18 //#define FREQUENCY RF69_433MHZ
eisd 2:04fdd571a385 19 #define FREQUENCY RF69_868MHZ
eisd 2:04fdd571a385 20 //#define FREQUENCY RF69_915MHZ
eisd 2:04fdd571a385 21
eisd 3:9091adbed369 22 const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" };
eisd 5:7af5760d7e8f 23
eisd 5:7af5760d7e8f 24 #ifdef USBSerial
eisd 5:7af5760d7e8f 25 USBSerial pc;
eisd 5:7af5760d7e8f 26 #else
eisd 2:04fdd571a385 27 Serial pc(USBTX, USBRX);
eisd 1:de8c34c9ccdf 28 #endif
eisd 5:7af5760d7e8f 29
eisd 5:7af5760d7e8f 30 #if TARGET_KL25Z
eisd 5:7af5760d7e8f 31 DigitalOut gnd(PTC0);
eisd 5:7af5760d7e8f 32 PwmOut ir(PTD4);
eisd 5:7af5760d7e8f 33 #endif
eisd 5:7af5760d7e8f 34 #define pulse(x, y) ir = 0.5; wait_us(x); ir = 0.0; wait_us(y);
eisd 5:7af5760d7e8f 35
eisd 5:7af5760d7e8f 36 /* generated in bash from lirc raw codes */
eisd 5:7af5760d7e8f 37 /*
eisd 5:7af5760d7e8f 38 f=duplo.conf
eisd 5:7af5760d7e8f 39 sed -n '/begin raw_codes/,/end raw_codes/s/name //p' $f|
eisd 5:7af5760d7e8f 40 while read n; do
eisd 5:7af5760d7e8f 41 echo -e "void $n() {$(
eisd 5:7af5760d7e8f 42 grep $n -A 1 $f|tail -n1|
eisd 5:7af5760d7e8f 43 sed 's/$/ 1000/;s@\([0-9]\+\)[[:space:]]\+\([0-9]\+\)[[:space:]]*@\n\tpulse(\1, \2);@g'
eisd 5:7af5760d7e8f 44 )\n}"
eisd 5:7af5760d7e8f 45 done
eisd 5:7af5760d7e8f 46 */
eisd 5:7af5760d7e8f 47
eisd 5:7af5760d7e8f 48 void FX() {
eisd 5:7af5760d7e8f 49 pulse(1042, 208);
eisd 5:7af5760d7e8f 50 pulse(208, 208);
eisd 5:7af5760d7e8f 51 pulse(208, 562);
eisd 5:7af5760d7e8f 52 pulse(625, 208);
eisd 5:7af5760d7e8f 53 pulse(208, 625);
eisd 5:7af5760d7e8f 54 pulse(208, 1);
eisd 5:7af5760d7e8f 55 }
eisd 5:7af5760d7e8f 56 void XF() {
eisd 5:7af5760d7e8f 57 pulse(229, 604);
eisd 5:7af5760d7e8f 58 pulse(229, 188);
eisd 5:7af5760d7e8f 59 pulse(229, 188);
eisd 5:7af5760d7e8f 60 pulse(438, 333);
eisd 5:7af5760d7e8f 61 pulse(625, 208);
eisd 5:7af5760d7e8f 62 pulse(1250, 1);
eisd 5:7af5760d7e8f 63 }
eisd 5:7af5760d7e8f 64 void FF() {
eisd 5:7af5760d7e8f 65 pulse(208, 625);
eisd 5:7af5760d7e8f 66 pulse(208, 208);
eisd 5:7af5760d7e8f 67 pulse(208, 208);
eisd 5:7af5760d7e8f 68 pulse(417, 354);
eisd 5:7af5760d7e8f 69 pulse(208, 208);
eisd 5:7af5760d7e8f 70 pulse(208, 1042);
eisd 5:7af5760d7e8f 71 pulse(208, 1);
eisd 5:7af5760d7e8f 72 }
eisd 5:7af5760d7e8f 73 void RX() {
eisd 5:7af5760d7e8f 74 pulse(1042, 208);
eisd 5:7af5760d7e8f 75 pulse(208, 188);
eisd 5:7af5760d7e8f 76 pulse(229, 542);
eisd 5:7af5760d7e8f 77 pulse(646, 1);
eisd 5:7af5760d7e8f 78 }
eisd 5:7af5760d7e8f 79 void XR() {
eisd 5:7af5760d7e8f 80 pulse(208, 1042);
eisd 5:7af5760d7e8f 81 pulse(208, 188);
eisd 5:7af5760d7e8f 82 pulse(229, 542);
eisd 5:7af5760d7e8f 83 pulse(625, 417);
eisd 5:7af5760d7e8f 84 pulse(854, 1);
eisd 5:7af5760d7e8f 85 }
eisd 5:7af5760d7e8f 86 void RR() {
eisd 5:7af5760d7e8f 87 pulse(229, 1021);
eisd 5:7af5760d7e8f 88 pulse(229, 208);
eisd 5:7af5760d7e8f 89 pulse(208, 542);
eisd 5:7af5760d7e8f 90 pulse(229, 188);
eisd 5:7af5760d7e8f 91 pulse(229, 1229);
eisd 5:7af5760d7e8f 92 pulse(229, 1);
eisd 5:7af5760d7e8f 93 }
eisd 5:7af5760d7e8f 94 void RF() {
eisd 5:7af5760d7e8f 95 pulse(208, 625);
eisd 5:7af5760d7e8f 96 pulse(208, 208);
eisd 5:7af5760d7e8f 97 pulse(208, 208);
eisd 5:7af5760d7e8f 98 pulse(417, 333);
eisd 5:7af5760d7e8f 99 pulse(208, 208);
eisd 5:7af5760d7e8f 100 pulse(208, 208);
eisd 5:7af5760d7e8f 101 pulse(208, 1);
eisd 5:7af5760d7e8f 102 }
eisd 5:7af5760d7e8f 103 void FR() {
eisd 5:7af5760d7e8f 104 pulse(229, 1021);
eisd 5:7af5760d7e8f 105 pulse(229, 188);
eisd 5:7af5760d7e8f 106 pulse(229, 542);
eisd 5:7af5760d7e8f 107 pulse(208, 208);
eisd 5:7af5760d7e8f 108 pulse(208, 208);
eisd 5:7af5760d7e8f 109 pulse(208, 625);
eisd 5:7af5760d7e8f 110 pulse(417, 1);
eisd 5:7af5760d7e8f 111 }
eisd 5:7af5760d7e8f 112 void BB() {
eisd 5:7af5760d7e8f 113 pulse(1042, 208);
eisd 5:7af5760d7e8f 114 pulse(208, 208);
eisd 5:7af5760d7e8f 115 pulse(208, 542);
eisd 5:7af5760d7e8f 116 pulse(208, 417);
eisd 5:7af5760d7e8f 117 pulse(208, 208);
eisd 5:7af5760d7e8f 118 pulse(1042, 1);
eisd 5:7af5760d7e8f 119 }
eisd 5:7af5760d7e8f 120
eisd 5:7af5760d7e8f 121 bool central = true;
eisd 7:f720032c2fb9 122 int stops_sent = 0;
eisd 5:7af5760d7e8f 123 int direction = -1;
eisd 5:7af5760d7e8f 124 Thread *thread;
eisd 5:7af5760d7e8f 125 void ir_thread(void const *args) {
eisd 5:7af5760d7e8f 126 while(1) {
eisd 5:7af5760d7e8f 127 //if (!central)
eisd 5:7af5760d7e8f 128 for(int x = 0; x < 5; x++) {
eisd 7:f720032c2fb9 129 //pc.printf(central? "stop\r\n" : "direction %s\r\n", directions[direction]);
eisd 7:f720032c2fb9 130 if (central && stops_sent++ < 50)
eisd 5:7af5760d7e8f 131 BB();
eisd 7:f720032c2fb9 132 else {
eisd 7:f720032c2fb9 133 stops_sent = 0;
eisd 7:f720032c2fb9 134 switch(direction) {
eisd 7:f720032c2fb9 135 case 0: XR(); break;
eisd 7:f720032c2fb9 136 case 1: RR(); break;
eisd 7:f720032c2fb9 137 case 2: RX(); break;
eisd 7:f720032c2fb9 138 case 3: FR(); break;
eisd 7:f720032c2fb9 139 case 4: FX(); break;
eisd 7:f720032c2fb9 140 case 5: FF(); break;
eisd 7:f720032c2fb9 141 case 6: XF(); break;
eisd 7:f720032c2fb9 142 case 7: RF(); break;
eisd 7:f720032c2fb9 143 }
eisd 5:7af5760d7e8f 144 }
eisd 6:3482f1e19d71 145 Thread::wait(10);
eisd 5:7af5760d7e8f 146 }
eisd 5:7af5760d7e8f 147 Thread::wait(50);
eisd 5:7af5760d7e8f 148 }
eisd 5:7af5760d7e8f 149 }
eisd 5:7af5760d7e8f 150
eisd 7:f720032c2fb9 151 PwmOut speaker(PTA12);
eisd 7:f720032c2fb9 152 DigitalOut speaker_gnd(PTC4);
eisd 7:f720032c2fb9 153 //float speaker = 0;
eisd 7:f720032c2fb9 154 //float speaker_gnd = 0;
eisd 7:f720032c2fb9 155
eisd 7:f720032c2fb9 156 //global variables used by interrupt routine
eisd 7:f720032c2fb9 157 volatile int i=0;
eisd 7:f720032c2fb9 158 float wave[128];
eisd 7:f720032c2fb9 159
eisd 7:f720032c2fb9 160 // Interrupt routine
eisd 7:f720032c2fb9 161 // used to output next analog sample whenever a timer interrupt occurs
eisd 7:f720032c2fb9 162 void Sample_timer_interrupt(void)
eisd 7:f720032c2fb9 163 {
eisd 7:f720032c2fb9 164 // send next analog sample out to D to A
eisd 7:f720032c2fb9 165 speaker = wave[i];
eisd 7:f720032c2fb9 166 // increment pointer and wrap around back to 0 at 128
eisd 7:f720032c2fb9 167 i = (i+1) & 0x07F;
eisd 7:f720032c2fb9 168 }
eisd 7:f720032c2fb9 169
eisd 7:f720032c2fb9 170 int Siren_pitch = 1;
eisd 7:f720032c2fb9 171 void Siren_pitch_flip() {
eisd 7:f720032c2fb9 172 speaker.period(Siren_pitch ? 0.4/554.365 : 0.4/523.251);
eisd 7:f720032c2fb9 173 Siren_pitch ^= 1;
eisd 7:f720032c2fb9 174 }
eisd 7:f720032c2fb9 175
eisd 7:f720032c2fb9 176 Ticker siren_pitch;
eisd 7:f720032c2fb9 177 bool Siren_last = 0;
eisd 7:f720032c2fb9 178 void Siren_state(bool on) {
eisd 7:f720032c2fb9 179 if (Siren_last != on) {
eisd 7:f720032c2fb9 180 pc.printf("siren %d\r\n", on);
eisd 7:f720032c2fb9 181 siren_pitch.detach();
eisd 7:f720032c2fb9 182 Siren_pitch = 1;
eisd 7:f720032c2fb9 183 Siren_pitch_flip();
eisd 7:f720032c2fb9 184 siren_pitch.attach(&Siren_pitch_flip, 0.8);
eisd 7:f720032c2fb9 185 speaker = on ? 0.3 : 0;
eisd 7:f720032c2fb9 186 }
eisd 7:f720032c2fb9 187 Siren_last = on;
eisd 7:f720032c2fb9 188 }
eisd 7:f720032c2fb9 189
eisd 7:f720032c2fb9 190 int main()
eisd 7:f720032c2fb9 191 {
eisd 7:f720032c2fb9 192
eisd 1:de8c34c9ccdf 193 #ifndef USBSerial
eisd 1:de8c34c9ccdf 194 pc.baud(115200);
eisd 1:de8c34c9ccdf 195 #endif
gbrush 0:7c98bcd8a245 196
eisd 2:04fdd571a385 197 #ifdef TARGET_KL25Z
eisd 6:3482f1e19d71 198 PwmOut r(LED_RED);
eisd 6:3482f1e19d71 199 //float r = 1.0f;
eisd 6:3482f1e19d71 200 PwmOut g(LED_GREEN);
eisd 6:3482f1e19d71 201 //float g = 1.0f;
eisd 6:3482f1e19d71 202 PwmOut b(LED_BLUE);
eisd 5:7af5760d7e8f 203 //while(1){FF();wait(0.01);}
eisd 6:3482f1e19d71 204 //float b = 1.0f;
eisd 3:9091adbed369 205 WiiChuck nun(PTE0, PTE1, pc);
eisd 2:04fdd571a385 206 RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13);
eisd 2:04fdd571a385 207 #else
eisd 2:04fdd571a385 208 WiiChuck nun(p9, p10, pc);
eisd 2:04fdd571a385 209 #endif
eisd 2:04fdd571a385 210
eisd 6:3482f1e19d71 211 gnd = 0;
eisd 6:3482f1e19d71 212 ir.period_us(1000/38);
eisd 6:3482f1e19d71 213
eisd 7:f720032c2fb9 214 speaker_gnd = 0;
eisd 7:f720032c2fb9 215 speaker = 0;
eisd 7:f720032c2fb9 216 //speaker.period(1.0/200000.0);
eisd 7:f720032c2fb9 217
eisd 7:f720032c2fb9 218 /*
eisd 7:f720032c2fb9 219 speaker = 0.2;
eisd 7:f720032c2fb9 220 while(1) {
eisd 7:f720032c2fb9 221 speaker.period(.8/554.365);
eisd 7:f720032c2fb9 222 wait(.8);
eisd 7:f720032c2fb9 223 speaker.period(.8/523.251);
eisd 7:f720032c2fb9 224 wait(.8);
eisd 7:f720032c2fb9 225 }
eisd 7:f720032c2fb9 226 //speaker = 0.2;
eisd 7:f720032c2fb9 227 // set up a timer to be used for sample rate interrupts
eisd 7:f720032c2fb9 228 Ticker Sample_Period;
eisd 7:f720032c2fb9 229 // precompute 128 sample points on one sine wave cycle
eisd 7:f720032c2fb9 230 // used for continuous sine wave output later
eisd 7:f720032c2fb9 231 for(int k=0; k<128; k++) {
eisd 7:f720032c2fb9 232 wave[k]=((1.0 + sin((float(k)/128.0*6.28318530717959)))/2.0);
eisd 7:f720032c2fb9 233 // scale the sine wave from 0.0 to 1.0 - as needed for AnalogOut arg
eisd 7:f720032c2fb9 234 }
eisd 7:f720032c2fb9 235 // turn on timer interrupts to start sine wave output
eisd 7:f720032c2fb9 236 // sample rate is 500Hz with 128 samples per cycle on sine wave
eisd 7:f720032c2fb9 237 while(1) {
eisd 7:f720032c2fb9 238 Sample_Period.detach();
eisd 7:f720032c2fb9 239 Sample_Period.attach(&Sample_timer_interrupt, 1.0/(554.365*128));
eisd 7:f720032c2fb9 240 wait(.8);
eisd 7:f720032c2fb9 241 Sample_Period.detach();
eisd 7:f720032c2fb9 242 Sample_Period.attach(&Sample_timer_interrupt, 1.0/(523.251*128));
eisd 7:f720032c2fb9 243 wait(.8);
eisd 7:f720032c2fb9 244 }
eisd 7:f720032c2fb9 245 // everything else needed is already being done by the interrupt routine
eisd 7:f720032c2fb9 246 */
eisd 7:f720032c2fb9 247
eisd 5:7af5760d7e8f 248 nunchuk n1, n2;
eisd 5:7af5760d7e8f 249 nunchuk *n = &n1, *next = &n2;
eisd 5:7af5760d7e8f 250 bool sender = nun.Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
eisd 4:c9711f0cd097 251 if (sender) {
eisd 4:c9711f0cd097 252 pc.printf("chuck attached\r\n");
eisd 4:c9711f0cd097 253 radio.initialize(FREQUENCY, NODE_ID, NETWORKID);
eisd 4:c9711f0cd097 254 } else {
eisd 4:c9711f0cd097 255 pc.printf("chuck unavailable\r\n");
eisd 4:c9711f0cd097 256 radio.initialize(FREQUENCY, GATEWAY_ID, NETWORKID);
eisd 5:7af5760d7e8f 257 thread = new Thread(ir_thread);
eisd 5:7af5760d7e8f 258 }
eisd 3:9091adbed369 259 radio.encrypt("0123456789054321");
eisd 2:04fdd571a385 260 //radio.promiscuous(false);
eisd 3:9091adbed369 261 radio.setHighPower(true);
eisd 3:9091adbed369 262 radio.setPowerLevel(20);
eisd 3:9091adbed369 263 radio.rcCalibration();
eisd 2:04fdd571a385 264 //radio.readAllRegs();
eisd 2:04fdd571a385 265 pc.printf("temp %d\r\n", radio.readTemperature(-1));
eisd 1:de8c34c9ccdf 266
eisd 6:3482f1e19d71 267 bool read = false;
eisd 1:de8c34c9ccdf 268 while(1) {
eisd 7:f720032c2fb9 269 if (sender) {
eisd 5:7af5760d7e8f 270 read = nun.Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
eisd 7:f720032c2fb9 271 n->sum = 0;
eisd 7:f720032c2fb9 272 n->sum = calculate_crc8((char*)n, sizeof(struct nunchuk));
eisd 7:f720032c2fb9 273 }
eisd 6:3482f1e19d71 274 else if (radio.receiveDone()) {
eisd 7:f720032c2fb9 275 //pc.printf("rssi %d\r\n", radio.RSSI);
eisd 4:c9711f0cd097 276 read = (radio.DATALEN == sizeof(struct nunchuk));
eisd 5:7af5760d7e8f 277 if (read) {
eisd 6:3482f1e19d71 278 memcpy((void*)next, (const void*)radio.DATA, radio.DATALEN);
eisd 7:f720032c2fb9 279 uint8_t sum = next->sum;
eisd 7:f720032c2fb9 280 next->sum = 0;
eisd 7:f720032c2fb9 281 read = sum == calculate_crc8((char*)next, sizeof(struct nunchuk));
eisd 7:f720032c2fb9 282 if (read) {
eisd 7:f720032c2fb9 283 nunchuk *last = n;
eisd 7:f720032c2fb9 284 n = next;
eisd 7:f720032c2fb9 285 next = last;
eisd 7:f720032c2fb9 286 }
eisd 7:f720032c2fb9 287 }
eisd 7:f720032c2fb9 288 if (!read)
eisd 7:f720032c2fb9 289 pc.printf("len %d\r\n", radio.DATALEN);
eisd 4:c9711f0cd097 290 }
eisd 1:de8c34c9ccdf 291 if(read)
eisd 1:de8c34c9ccdf 292 {
eisd 7:f720032c2fb9 293 Siren_state(n->C);
eisd 5:7af5760d7e8f 294 float x = n->X - 128, y = n->Y - 128;
eisd 3:9091adbed369 295 float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5;
eisd 1:de8c34c9ccdf 296 int c = 0;
eisd 3:9091adbed369 297 if (p > -4) c = 0;
eisd 3:9091adbed369 298 if (p > -3) c = 1;
eisd 3:9091adbed369 299 if (p > -2) c = 2;
eisd 3:9091adbed369 300 if (p > -1) c = 3;
eisd 3:9091adbed369 301 if (p > 0) c = 4;
eisd 3:9091adbed369 302 if (p > 1) c = 5;
eisd 3:9091adbed369 303 if (p > 2) c = 6;
eisd 3:9091adbed369 304 if (p > 3) c = 7;
eisd 5:7af5760d7e8f 305 direction = c;
eisd 1:de8c34c9ccdf 306
eisd 6:3482f1e19d71 307 #if DEBUG
eisd 6:3482f1e19d71 308 pc.printf("%d: ", sizeof(struct nunchuk));
eisd 6:3482f1e19d71 309 pc.printf("x%3d y%3d c%1d z%1d --", n->X, n->Y, n->C, n->Z);
eisd 6:3482f1e19d71 310 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 311
eisd 3:9091adbed369 312 //radio.send(GATEWAY_ID, (const void*)"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA", 50, false);
eisd 3:9091adbed369 313 #endif
eisd 4:c9711f0cd097 314 if (sender)
eisd 6:3482f1e19d71 315 radio.send(GATEWAY_ID, (const void*)n, sizeof(struct nunchuk), false);
eisd 3:9091adbed369 316
eisd 1:de8c34c9ccdf 317 #ifdef TARGET_KL25Z
eisd 3:9091adbed369 318 if (R < 20) {
eisd 3:9091adbed369 319 if (!central) {
eisd 3:9091adbed369 320 pc.printf("central\r\n");
eisd 3:9091adbed369 321 central = true;
eisd 3:9091adbed369 322 }
eisd 1:de8c34c9ccdf 323 r = 1.0f;
eisd 1:de8c34c9ccdf 324 g = 1.0f;
eisd 1:de8c34c9ccdf 325 b = 1.0f;
eisd 1:de8c34c9ccdf 326 } else {
eisd 3:9091adbed369 327 if (central) {
eisd 6:3482f1e19d71 328 pc.printf("go %s\r\n", directions[direction]);
eisd 3:9091adbed369 329 central = false;
eisd 3:9091adbed369 330 }
eisd 3:9091adbed369 331 R = R/20000;
eisd 1:de8c34c9ccdf 332 float pal[8][3] = {
eisd 1:de8c34c9ccdf 333 { 0, 0, 1 },
eisd 1:de8c34c9ccdf 334 { 0, 1, 1 },
eisd 1:de8c34c9ccdf 335 { 0, 1, 0 },
eisd 1:de8c34c9ccdf 336 { 1, 1, 0 },
eisd 1:de8c34c9ccdf 337 { 1, 0.5, 0 },
eisd 1:de8c34c9ccdf 338 { 1, 0, 0 },
eisd 1:de8c34c9ccdf 339 { 1, 0, 1 },
eisd 1:de8c34c9ccdf 340 { 0.5, 0, 1 },
eisd 1:de8c34c9ccdf 341 };
eisd 6:3482f1e19d71 342 c = (c + 4) % 8;
eisd 1:de8c34c9ccdf 343 r = 1.0f - pal[c][0] * R;
eisd 1:de8c34c9ccdf 344 g = 1.0f - pal[c][1] * R;
eisd 1:de8c34c9ccdf 345 b = 1.0f - pal[c][2] * R;
eisd 1:de8c34c9ccdf 346 }
eisd 1:de8c34c9ccdf 347 #endif
eisd 1:de8c34c9ccdf 348 }
eisd 4:c9711f0cd097 349 else if (sender)
eisd 1:de8c34c9ccdf 350 {
eisd 1:de8c34c9ccdf 351 pc.printf("Error\r\n");
eisd 3:9091adbed369 352 return 0;
eisd 1:de8c34c9ccdf 353 }
eisd 4:c9711f0cd097 354 if (sender)
eisd 4:c9711f0cd097 355 wait(0.01);
eisd 5:7af5760d7e8f 356 else
eisd 5:7af5760d7e8f 357 Thread::wait(10);
eisd 1:de8c34c9ccdf 358 }
eisd 1:de8c34c9ccdf 359 }