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

Committer:
eisd
Date:
Mon Jun 06 13:14:07 2016 +0000
Revision:
5:7af5760d7e8f
Parent:
4:c9711f0cd097
Child:
6:3482f1e19d71
Threaded rfm listener

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"
gbrush 0:7c98bcd8a245 8
eisd 2:04fdd571a385 9 //#include "USBSerial.h"
eisd 2:04fdd571a385 10
eisd 2:04fdd571a385 11 #include "RFM69.h"
eisd 2:04fdd571a385 12 #define GATEWAY_ID 2
eisd 2:04fdd571a385 13 #define NODE_ID 1
eisd 2:04fdd571a385 14 #define NETWORKID 100
gbrush 0:7c98bcd8a245 15
eisd 2:04fdd571a385 16 // Uncomment only one of the following three to match radio frequency
eisd 2:04fdd571a385 17 //#define FREQUENCY RF69_433MHZ
eisd 2:04fdd571a385 18 #define FREQUENCY RF69_868MHZ
eisd 2:04fdd571a385 19 //#define FREQUENCY RF69_915MHZ
eisd 2:04fdd571a385 20
eisd 3:9091adbed369 21 const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" };
eisd 5:7af5760d7e8f 22
eisd 5:7af5760d7e8f 23 #ifdef USBSerial
eisd 5:7af5760d7e8f 24 USBSerial pc;
eisd 5:7af5760d7e8f 25 #else
eisd 2:04fdd571a385 26 Serial pc(USBTX, USBRX);
eisd 1:de8c34c9ccdf 27 #endif
eisd 5:7af5760d7e8f 28
eisd 5:7af5760d7e8f 29 #if TARGET_KL25Z
eisd 5:7af5760d7e8f 30 DigitalOut gnd(PTC0);
eisd 5:7af5760d7e8f 31 PwmOut ir(PTD4);
eisd 5:7af5760d7e8f 32 #endif
eisd 5:7af5760d7e8f 33 #define pulse(x, y) ir = 0.5; wait_us(x); ir = 0.0; wait_us(y);
eisd 5:7af5760d7e8f 34
eisd 5:7af5760d7e8f 35 /* generated in bash from lirc raw codes */
eisd 5:7af5760d7e8f 36 /*
eisd 5:7af5760d7e8f 37 f=duplo.conf
eisd 5:7af5760d7e8f 38 sed -n '/begin raw_codes/,/end raw_codes/s/name //p' $f|
eisd 5:7af5760d7e8f 39 while read n; do
eisd 5:7af5760d7e8f 40 echo -e "void $n() {$(
eisd 5:7af5760d7e8f 41 grep $n -A 1 $f|tail -n1|
eisd 5:7af5760d7e8f 42 sed 's/$/ 1000/;s@\([0-9]\+\)[[:space:]]\+\([0-9]\+\)[[:space:]]*@\n\tpulse(\1, \2);@g'
eisd 5:7af5760d7e8f 43 )\n}"
eisd 5:7af5760d7e8f 44 done
eisd 5:7af5760d7e8f 45 */
eisd 5:7af5760d7e8f 46
eisd 5:7af5760d7e8f 47 void FX() {
eisd 5:7af5760d7e8f 48 pulse(1042, 208);
eisd 5:7af5760d7e8f 49 pulse(208, 208);
eisd 5:7af5760d7e8f 50 pulse(208, 562);
eisd 5:7af5760d7e8f 51 pulse(625, 208);
eisd 5:7af5760d7e8f 52 pulse(208, 625);
eisd 5:7af5760d7e8f 53 pulse(208, 1);
eisd 5:7af5760d7e8f 54 }
eisd 5:7af5760d7e8f 55 void XF() {
eisd 5:7af5760d7e8f 56 pulse(229, 604);
eisd 5:7af5760d7e8f 57 pulse(229, 188);
eisd 5:7af5760d7e8f 58 pulse(229, 188);
eisd 5:7af5760d7e8f 59 pulse(438, 333);
eisd 5:7af5760d7e8f 60 pulse(625, 208);
eisd 5:7af5760d7e8f 61 pulse(1250, 1);
eisd 5:7af5760d7e8f 62 }
eisd 5:7af5760d7e8f 63 void FF() {
eisd 5:7af5760d7e8f 64 pc.printf("FF\r\n");
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 5:7af5760d7e8f 122 int direction = -1;
eisd 5:7af5760d7e8f 123 Thread *thread;
eisd 5:7af5760d7e8f 124 void ir_thread(void const *args) {
eisd 5:7af5760d7e8f 125 while(1) {
eisd 5:7af5760d7e8f 126 //if (!central)
eisd 5:7af5760d7e8f 127 for(int x = 0; x < 5; x++) {
eisd 5:7af5760d7e8f 128 pc.printf("direction %d\r\n", direction);//directions[direction]);
eisd 5:7af5760d7e8f 129 if (central)
eisd 5:7af5760d7e8f 130 BB();
eisd 5:7af5760d7e8f 131 else
eisd 5:7af5760d7e8f 132 switch(direction) {
eisd 5:7af5760d7e8f 133 case 0: XR(); break;
eisd 5:7af5760d7e8f 134 case 1: RR(); break;
eisd 5:7af5760d7e8f 135 case 2: RX(); break;
eisd 5:7af5760d7e8f 136 case 3: FR(); break;
eisd 5:7af5760d7e8f 137 case 4: FX(); break;
eisd 5:7af5760d7e8f 138 case 5: FF(); break;
eisd 5:7af5760d7e8f 139 case 6: XF(); break;
eisd 5:7af5760d7e8f 140 case 7: RF(); break;
eisd 5:7af5760d7e8f 141 }
eisd 5:7af5760d7e8f 142 Thread::wait(20);
eisd 5:7af5760d7e8f 143 }
eisd 5:7af5760d7e8f 144 Thread::wait(50);
eisd 5:7af5760d7e8f 145 }
eisd 5:7af5760d7e8f 146 }
eisd 5:7af5760d7e8f 147
gbrush 0:7c98bcd8a245 148 int main() {
eisd 5:7af5760d7e8f 149 gnd = 0;
eisd 5:7af5760d7e8f 150 ir.period(1.0/38000.0);
eisd 1:de8c34c9ccdf 151 #ifndef USBSerial
eisd 1:de8c34c9ccdf 152 pc.baud(115200);
eisd 1:de8c34c9ccdf 153 #endif
gbrush 0:7c98bcd8a245 154
eisd 2:04fdd571a385 155 #ifdef TARGET_KL25Z
eisd 5:7af5760d7e8f 156 //PwmOut r(LED_RED);
eisd 5:7af5760d7e8f 157 float r = 1.0f;
eisd 5:7af5760d7e8f 158 //PwmOut g(LED_GREEN);
eisd 5:7af5760d7e8f 159 float g = 1.0f;
eisd 5:7af5760d7e8f 160 //while(1){FF();wait(0.01);}
eisd 5:7af5760d7e8f 161 //PwmOut b(LED_BLUE);
eisd 5:7af5760d7e8f 162 float b = 1.0f;
eisd 3:9091adbed369 163 WiiChuck nun(PTE0, PTE1, pc);
eisd 2:04fdd571a385 164 RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13);
eisd 2:04fdd571a385 165 #else
eisd 2:04fdd571a385 166 WiiChuck nun(p9, p10, pc);
eisd 2:04fdd571a385 167 #endif
eisd 2:04fdd571a385 168
eisd 5:7af5760d7e8f 169 nunchuk n1, n2;
eisd 5:7af5760d7e8f 170 nunchuk *n = &n1, *next = &n2;
eisd 5:7af5760d7e8f 171 bool sender = nun.Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
eisd 4:c9711f0cd097 172 if (sender) {
eisd 4:c9711f0cd097 173 pc.printf("chuck attached\r\n");
eisd 4:c9711f0cd097 174 radio.initialize(FREQUENCY, NODE_ID, NETWORKID);
eisd 4:c9711f0cd097 175 } else {
eisd 4:c9711f0cd097 176 pc.printf("chuck unavailable\r\n");
eisd 4:c9711f0cd097 177 radio.initialize(FREQUENCY, GATEWAY_ID, NETWORKID);
eisd 5:7af5760d7e8f 178 thread = new Thread(ir_thread);
eisd 5:7af5760d7e8f 179 }
eisd 3:9091adbed369 180 radio.encrypt("0123456789054321");
eisd 2:04fdd571a385 181 //radio.promiscuous(false);
eisd 3:9091adbed369 182 radio.setHighPower(true);
eisd 3:9091adbed369 183 radio.setPowerLevel(20);
eisd 3:9091adbed369 184 radio.rcCalibration();
eisd 2:04fdd571a385 185 //radio.readAllRegs();
eisd 2:04fdd571a385 186 pc.printf("temp %d\r\n", radio.readTemperature(-1));
eisd 1:de8c34c9ccdf 187
eisd 5:7af5760d7e8f 188 bool read;
eisd 1:de8c34c9ccdf 189 while(1) {
eisd 4:c9711f0cd097 190 if (sender)
eisd 5:7af5760d7e8f 191 read = nun.Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
eisd 4:c9711f0cd097 192 else {
eisd 4:c9711f0cd097 193 radio.receive();
eisd 5:7af5760d7e8f 194 printf("rssi %d\r\n", radio.RSSI);
eisd 4:c9711f0cd097 195 read = (radio.DATALEN == sizeof(struct nunchuk));
eisd 5:7af5760d7e8f 196 if (read) {
eisd 5:7af5760d7e8f 197 memcpy((void*)&next, (const void*)radio.DATA, radio.DATALEN);
eisd 5:7af5760d7e8f 198 nunchuk *last = n;
eisd 5:7af5760d7e8f 199 n = next;
eisd 5:7af5760d7e8f 200 next = last;
eisd 5:7af5760d7e8f 201 } else
eisd 5:7af5760d7e8f 202 printf("len %d\r\n", radio.DATALEN);
eisd 4:c9711f0cd097 203 }
eisd 1:de8c34c9ccdf 204 if(read)
eisd 1:de8c34c9ccdf 205 {
eisd 5:7af5760d7e8f 206 float x = n->X - 128, y = n->Y - 128;
eisd 3:9091adbed369 207 float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5;
eisd 1:de8c34c9ccdf 208 int c = 0;
eisd 3:9091adbed369 209 if (p > -4) c = 0;
eisd 3:9091adbed369 210 if (p > -3) c = 1;
eisd 3:9091adbed369 211 if (p > -2) c = 2;
eisd 3:9091adbed369 212 if (p > -1) c = 3;
eisd 3:9091adbed369 213 if (p > 0) c = 4;
eisd 3:9091adbed369 214 if (p > 1) c = 5;
eisd 3:9091adbed369 215 if (p > 2) c = 6;
eisd 3:9091adbed369 216 if (p > 3) c = 7;
eisd 5:7af5760d7e8f 217 direction = c;
eisd 1:de8c34c9ccdf 218
eisd 3:9091adbed369 219 #ifdef DEBUG
eisd 3:9091adbed369 220 pc.printf("x%3d y%3d c%1d z%1d --", n.X, n.Y, n.C, n.Z);
eisd 3:9091adbed369 221 pc.printf("x%d y%d z%d -- %.3f %s \r\n", n.aX, n.aY, n.aZ, R, directions[c]);
eisd 3:9091adbed369 222
eisd 3:9091adbed369 223 //radio.send(GATEWAY_ID, (const void*)"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA", 50, false);
eisd 3:9091adbed369 224 #endif
eisd 4:c9711f0cd097 225 if (sender)
eisd 4:c9711f0cd097 226 radio.send(GATEWAY_ID, (const void*)&n, sizeof(struct nunchuk), false);
eisd 3:9091adbed369 227
eisd 1:de8c34c9ccdf 228 #ifdef TARGET_KL25Z
eisd 3:9091adbed369 229 if (R < 20) {
eisd 3:9091adbed369 230 if (!central) {
eisd 3:9091adbed369 231 pc.printf("central\r\n");
eisd 3:9091adbed369 232 central = true;
eisd 3:9091adbed369 233 }
eisd 1:de8c34c9ccdf 234 r = 1.0f;
eisd 1:de8c34c9ccdf 235 g = 1.0f;
eisd 1:de8c34c9ccdf 236 b = 1.0f;
eisd 1:de8c34c9ccdf 237 } else {
eisd 3:9091adbed369 238 if (central) {
eisd 3:9091adbed369 239 pc.printf("go\r\n");
eisd 3:9091adbed369 240 central = false;
eisd 3:9091adbed369 241 }
eisd 3:9091adbed369 242 R = R/20000;
eisd 1:de8c34c9ccdf 243 float pal[8][3] = {
eisd 1:de8c34c9ccdf 244 { 0, 0, 1 },
eisd 1:de8c34c9ccdf 245 { 0, 1, 1 },
eisd 1:de8c34c9ccdf 246 { 0, 1, 0 },
eisd 1:de8c34c9ccdf 247 { 1, 1, 0 },
eisd 1:de8c34c9ccdf 248 { 1, 0.5, 0 },
eisd 1:de8c34c9ccdf 249 { 1, 0, 0 },
eisd 1:de8c34c9ccdf 250 { 1, 0, 1 },
eisd 1:de8c34c9ccdf 251 { 0.5, 0, 1 },
eisd 1:de8c34c9ccdf 252 };
eisd 1:de8c34c9ccdf 253 r = 1.0f - pal[c][0] * R;
eisd 1:de8c34c9ccdf 254 g = 1.0f - pal[c][1] * R;
eisd 1:de8c34c9ccdf 255 b = 1.0f - pal[c][2] * R;
eisd 1:de8c34c9ccdf 256 }
eisd 1:de8c34c9ccdf 257 #endif
eisd 1:de8c34c9ccdf 258 }
eisd 4:c9711f0cd097 259 else if (sender)
eisd 1:de8c34c9ccdf 260 {
eisd 1:de8c34c9ccdf 261 pc.printf("Error\r\n");
eisd 3:9091adbed369 262 return 0;
eisd 1:de8c34c9ccdf 263 }
eisd 4:c9711f0cd097 264 if (sender)
eisd 4:c9711f0cd097 265 wait(0.01);
eisd 5:7af5760d7e8f 266 else
eisd 5:7af5760d7e8f 267 Thread::wait(10);
eisd 1:de8c34c9ccdf 268 }
eisd 1:de8c34c9ccdf 269 }