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
main.cpp@6:3482f1e19d71, 2016-06-10 (annotated)
- Committer:
- eisd
- Date:
- Fri Jun 10 06:17:55 2016 +0000
- Revision:
- 6:3482f1e19d71
- Parent:
- 5:7af5760d7e8f
- Child:
- 7:f720032c2fb9
Working rfm receiver and tweaks
Who changed what in which revision?
User | Revision | Line number | New 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 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 65 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 66 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 67 | pulse(417, 354); |
eisd | 5:7af5760d7e8f | 68 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 69 | pulse(208, 1042); |
eisd | 5:7af5760d7e8f | 70 | pulse(208, 1); |
eisd | 5:7af5760d7e8f | 71 | } |
eisd | 5:7af5760d7e8f | 72 | void RX() { |
eisd | 5:7af5760d7e8f | 73 | pulse(1042, 208); |
eisd | 5:7af5760d7e8f | 74 | pulse(208, 188); |
eisd | 5:7af5760d7e8f | 75 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 76 | pulse(646, 1); |
eisd | 5:7af5760d7e8f | 77 | } |
eisd | 5:7af5760d7e8f | 78 | void XR() { |
eisd | 5:7af5760d7e8f | 79 | pulse(208, 1042); |
eisd | 5:7af5760d7e8f | 80 | pulse(208, 188); |
eisd | 5:7af5760d7e8f | 81 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 82 | pulse(625, 417); |
eisd | 5:7af5760d7e8f | 83 | pulse(854, 1); |
eisd | 5:7af5760d7e8f | 84 | } |
eisd | 5:7af5760d7e8f | 85 | void RR() { |
eisd | 5:7af5760d7e8f | 86 | pulse(229, 1021); |
eisd | 5:7af5760d7e8f | 87 | pulse(229, 208); |
eisd | 5:7af5760d7e8f | 88 | pulse(208, 542); |
eisd | 5:7af5760d7e8f | 89 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 90 | pulse(229, 1229); |
eisd | 5:7af5760d7e8f | 91 | pulse(229, 1); |
eisd | 5:7af5760d7e8f | 92 | } |
eisd | 5:7af5760d7e8f | 93 | void RF() { |
eisd | 5:7af5760d7e8f | 94 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 95 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 96 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 97 | pulse(417, 333); |
eisd | 5:7af5760d7e8f | 98 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 99 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 100 | pulse(208, 1); |
eisd | 5:7af5760d7e8f | 101 | } |
eisd | 5:7af5760d7e8f | 102 | void FR() { |
eisd | 5:7af5760d7e8f | 103 | pulse(229, 1021); |
eisd | 5:7af5760d7e8f | 104 | pulse(229, 188); |
eisd | 5:7af5760d7e8f | 105 | pulse(229, 542); |
eisd | 5:7af5760d7e8f | 106 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 107 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 108 | pulse(208, 625); |
eisd | 5:7af5760d7e8f | 109 | pulse(417, 1); |
eisd | 5:7af5760d7e8f | 110 | } |
eisd | 5:7af5760d7e8f | 111 | void BB() { |
eisd | 5:7af5760d7e8f | 112 | pulse(1042, 208); |
eisd | 5:7af5760d7e8f | 113 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 114 | pulse(208, 542); |
eisd | 5:7af5760d7e8f | 115 | pulse(208, 417); |
eisd | 5:7af5760d7e8f | 116 | pulse(208, 208); |
eisd | 5:7af5760d7e8f | 117 | pulse(1042, 1); |
eisd | 5:7af5760d7e8f | 118 | } |
eisd | 5:7af5760d7e8f | 119 | |
eisd | 5:7af5760d7e8f | 120 | bool central = true; |
eisd | 5:7af5760d7e8f | 121 | int direction = -1; |
eisd | 5:7af5760d7e8f | 122 | Thread *thread; |
eisd | 5:7af5760d7e8f | 123 | void ir_thread(void const *args) { |
eisd | 5:7af5760d7e8f | 124 | while(1) { |
eisd | 5:7af5760d7e8f | 125 | //if (!central) |
eisd | 5:7af5760d7e8f | 126 | for(int x = 0; x < 5; x++) { |
eisd | 6:3482f1e19d71 | 127 | pc.printf(central? "stop\r\n" : "direction %s\r\n", directions[direction]); |
eisd | 5:7af5760d7e8f | 128 | if (central) |
eisd | 5:7af5760d7e8f | 129 | BB(); |
eisd | 5:7af5760d7e8f | 130 | else |
eisd | 5:7af5760d7e8f | 131 | switch(direction) { |
eisd | 5:7af5760d7e8f | 132 | case 0: XR(); break; |
eisd | 5:7af5760d7e8f | 133 | case 1: RR(); break; |
eisd | 5:7af5760d7e8f | 134 | case 2: RX(); break; |
eisd | 5:7af5760d7e8f | 135 | case 3: FR(); break; |
eisd | 5:7af5760d7e8f | 136 | case 4: FX(); break; |
eisd | 5:7af5760d7e8f | 137 | case 5: FF(); break; |
eisd | 5:7af5760d7e8f | 138 | case 6: XF(); break; |
eisd | 5:7af5760d7e8f | 139 | case 7: RF(); break; |
eisd | 5:7af5760d7e8f | 140 | } |
eisd | 6:3482f1e19d71 | 141 | Thread::wait(10); |
eisd | 5:7af5760d7e8f | 142 | } |
eisd | 5:7af5760d7e8f | 143 | Thread::wait(50); |
eisd | 5:7af5760d7e8f | 144 | } |
eisd | 5:7af5760d7e8f | 145 | } |
eisd | 5:7af5760d7e8f | 146 | |
gbrush | 0:7c98bcd8a245 | 147 | int main() { |
eisd | 1:de8c34c9ccdf | 148 | #ifndef USBSerial |
eisd | 1:de8c34c9ccdf | 149 | pc.baud(115200); |
eisd | 1:de8c34c9ccdf | 150 | #endif |
gbrush | 0:7c98bcd8a245 | 151 | |
eisd | 2:04fdd571a385 | 152 | #ifdef TARGET_KL25Z |
eisd | 6:3482f1e19d71 | 153 | PwmOut r(LED_RED); |
eisd | 6:3482f1e19d71 | 154 | //float r = 1.0f; |
eisd | 6:3482f1e19d71 | 155 | PwmOut g(LED_GREEN); |
eisd | 6:3482f1e19d71 | 156 | //float g = 1.0f; |
eisd | 6:3482f1e19d71 | 157 | PwmOut b(LED_BLUE); |
eisd | 5:7af5760d7e8f | 158 | //while(1){FF();wait(0.01);} |
eisd | 6:3482f1e19d71 | 159 | //float b = 1.0f; |
eisd | 3:9091adbed369 | 160 | WiiChuck nun(PTE0, PTE1, pc); |
eisd | 2:04fdd571a385 | 161 | RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13); |
eisd | 2:04fdd571a385 | 162 | #else |
eisd | 2:04fdd571a385 | 163 | WiiChuck nun(p9, p10, pc); |
eisd | 2:04fdd571a385 | 164 | #endif |
eisd | 2:04fdd571a385 | 165 | |
eisd | 6:3482f1e19d71 | 166 | gnd = 0; |
eisd | 6:3482f1e19d71 | 167 | ir.period_us(1000/38); |
eisd | 6:3482f1e19d71 | 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 | 6:3482f1e19d71 | 188 | bool read = false; |
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 | 6:3482f1e19d71 | 192 | else if (radio.receiveDone()) { |
eisd | 6:3482f1e19d71 | 193 | //printf("rssi %d\r\n", radio.RSSI); |
eisd | 4:c9711f0cd097 | 194 | read = (radio.DATALEN == sizeof(struct nunchuk)); |
eisd | 5:7af5760d7e8f | 195 | if (read) { |
eisd | 6:3482f1e19d71 | 196 | memcpy((void*)next, (const void*)radio.DATA, radio.DATALEN); |
eisd | 5:7af5760d7e8f | 197 | nunchuk *last = n; |
eisd | 5:7af5760d7e8f | 198 | n = next; |
eisd | 5:7af5760d7e8f | 199 | next = last; |
eisd | 5:7af5760d7e8f | 200 | } else |
eisd | 5:7af5760d7e8f | 201 | printf("len %d\r\n", radio.DATALEN); |
eisd | 4:c9711f0cd097 | 202 | } |
eisd | 1:de8c34c9ccdf | 203 | if(read) |
eisd | 1:de8c34c9ccdf | 204 | { |
eisd | 5:7af5760d7e8f | 205 | float x = n->X - 128, y = n->Y - 128; |
eisd | 3:9091adbed369 | 206 | float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5; |
eisd | 1:de8c34c9ccdf | 207 | int c = 0; |
eisd | 3:9091adbed369 | 208 | if (p > -4) c = 0; |
eisd | 3:9091adbed369 | 209 | if (p > -3) c = 1; |
eisd | 3:9091adbed369 | 210 | if (p > -2) c = 2; |
eisd | 3:9091adbed369 | 211 | if (p > -1) c = 3; |
eisd | 3:9091adbed369 | 212 | if (p > 0) c = 4; |
eisd | 3:9091adbed369 | 213 | if (p > 1) c = 5; |
eisd | 3:9091adbed369 | 214 | if (p > 2) c = 6; |
eisd | 3:9091adbed369 | 215 | if (p > 3) c = 7; |
eisd | 5:7af5760d7e8f | 216 | direction = c; |
eisd | 1:de8c34c9ccdf | 217 | |
eisd | 6:3482f1e19d71 | 218 | #if DEBUG |
eisd | 6:3482f1e19d71 | 219 | pc.printf("%d: ", sizeof(struct nunchuk)); |
eisd | 6:3482f1e19d71 | 220 | pc.printf("x%3d y%3d c%1d z%1d --", n->X, n->Y, n->C, n->Z); |
eisd | 6:3482f1e19d71 | 221 | 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 | 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 | 6:3482f1e19d71 | 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 | 6:3482f1e19d71 | 239 | pc.printf("go %s\r\n", directions[direction]); |
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 | 6:3482f1e19d71 | 253 | c = (c + 4) % 8; |
eisd | 1:de8c34c9ccdf | 254 | r = 1.0f - pal[c][0] * R; |
eisd | 1:de8c34c9ccdf | 255 | g = 1.0f - pal[c][1] * R; |
eisd | 1:de8c34c9ccdf | 256 | b = 1.0f - pal[c][2] * R; |
eisd | 1:de8c34c9ccdf | 257 | } |
eisd | 1:de8c34c9ccdf | 258 | #endif |
eisd | 1:de8c34c9ccdf | 259 | } |
eisd | 4:c9711f0cd097 | 260 | else if (sender) |
eisd | 1:de8c34c9ccdf | 261 | { |
eisd | 1:de8c34c9ccdf | 262 | pc.printf("Error\r\n"); |
eisd | 3:9091adbed369 | 263 | return 0; |
eisd | 1:de8c34c9ccdf | 264 | } |
eisd | 4:c9711f0cd097 | 265 | if (sender) |
eisd | 4:c9711f0cd097 | 266 | wait(0.01); |
eisd | 5:7af5760d7e8f | 267 | else |
eisd | 5:7af5760d7e8f | 268 | Thread::wait(10); |
eisd | 1:de8c34c9ccdf | 269 | } |
eisd | 1:de8c34c9ccdf | 270 | } |