main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "WakeUp.h"
00004 #include "FastPWM.h"
00005 #ifndef M_PI
00006 #define M_PI           3.14159265358979323846
00007 #endif
00008 
00009 #include "WiiChuk_compat.hpp"
00010 #include "lib_crc.h"
00011 
00012 #include "tlc59108.h"
00013 
00014 //#include "USBSerial.h"
00015 
00016 #include "RFM69.h"
00017 #define GATEWAY_ID    2
00018 #define NODE_ID       1
00019 #define NETWORKID     100
00020 
00021 // Uncomment only one of the following three to match radio frequency
00022 //#define FREQUENCY     RF69_433MHZ    
00023 #define FREQUENCY     RF69_868MHZ
00024 //#define FREQUENCY     RF69_915MHZ
00025 
00026 const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" };
00027 
00028 #define DEBUG 0
00029 #ifdef DEBUG
00030 #ifdef USBSerial
00031     USBSerial pc;
00032 #else
00033     Serial pc(USBTX, USBRX);
00034 #endif
00035 #else
00036     class Null : public Stream {
00037         public:
00038         Null(): Stream("null") {}
00039         void baud(int) {}
00040 
00041         protected:
00042         virtual int _getc() { return 0; }
00043         virtual int _putc(int c) { return 0; }
00044     };
00045     Null pc;
00046 #endif
00047 #if TARGET_KL25Z
00048 DigitalOut gnd(PTC6);
00049 PwmOut ir(PTA5);
00050 #endif
00051 #define pulse(x, y) ir = 0.5; wait_us(x); ir = 0.0; wait_us(y); 
00052 
00053 /* generated in bash from lirc raw codes */
00054 /*
00055 f=duplo.conf
00056 sed -n '/begin raw_codes/,/end raw_codes/s/name //p' $f|
00057     while read n; do
00058         echo -e "void $n() {$(
00059             grep $n -A 1 $f|tail -n1|
00060                 sed 's/$/ 1000/;s@\([0-9]\+\)[[:space:]]\+\([0-9]\+\)[[:space:]]*@\n\tpulse(\1, \2);@g'
00061         )\n}"
00062     done
00063 */
00064 
00065 void FX() {
00066     pulse(1042, 208);
00067     pulse(208, 208);
00068     pulse(208, 562);
00069     pulse(625, 208);
00070     pulse(208, 625);
00071     pulse(208, 1);
00072 }
00073 void XF() {
00074     pulse(229, 604);
00075     pulse(229, 188);
00076     pulse(229, 188);
00077     pulse(438, 333);
00078     pulse(625, 208);
00079     pulse(1250, 1);
00080 }
00081 void FF() {
00082     pulse(208, 625);
00083     pulse(208, 208);
00084     pulse(208, 208);
00085     pulse(417, 354);
00086     pulse(208, 208);
00087     pulse(208, 1042);
00088     pulse(208, 1);
00089 }
00090 void RX() {
00091     pulse(1042, 208);
00092     pulse(208, 188);
00093     pulse(229, 542);
00094     pulse(646, 1);
00095 }
00096 void XR() {
00097     pulse(208, 1042);
00098     pulse(208, 188);
00099     pulse(229, 542);
00100     pulse(625, 417);
00101     pulse(854, 1);
00102 }
00103 void RR() {
00104     pulse(229, 1021);
00105     pulse(229, 208);
00106     pulse(208, 542);
00107     pulse(229, 188);
00108     pulse(229, 1229);
00109     pulse(229, 1);
00110 }
00111 void RF() {
00112     pulse(208, 625);
00113     pulse(208, 208);
00114     pulse(208, 208);
00115     pulse(417, 333);
00116     pulse(208, 208);
00117     pulse(208, 208);
00118     pulse(208, 1);
00119 }
00120 void FR() {
00121     pulse(229, 1021);
00122     pulse(229, 188);
00123     pulse(229, 542);
00124     pulse(208, 208);
00125     pulse(208, 208);
00126     pulse(208, 625);
00127     pulse(417, 1);
00128 }
00129 void BB() {
00130     pulse(1042, 208);
00131     pulse(208, 208);
00132     pulse(208, 542);
00133     pulse(208, 417);
00134     pulse(208, 208);
00135     pulse(1042, 1);
00136 }
00137 
00138 int mag;
00139 bool central = true;
00140 Timer central_time;
00141 bool central_time_trip = 0;
00142 int stops_sent = 0;
00143 int direction = -1;
00144 Thread *thread;
00145 void ir_thread(void const *args) {
00146     while(1) {
00147         //if (!central)
00148             for(int x = 0; x < 40; x++) {
00149                 #if DEBUG
00150                 pc.printf(central? "stop %s %d\r\n" : "direction %s %d\r\n", directions[direction], stops_sent);
00151                 #endif
00152                 if (central || (x % 4) > mag) {
00153                     if (stops_sent < 50) {
00154                         stops_sent++;
00155                         BB();
00156                     }
00157                 } else {
00158                     stops_sent = 0;
00159                     switch(direction) {
00160                         case 0: XR(); break;
00161                         case 1: RR(); break;
00162                         case 2: RX(); break;
00163                         case 3: FR(); break;
00164                         case 4: FX(); break;
00165                         case 5: FF(); break;
00166                         case 6: XF(); break;
00167                         case 7: RF(); break;
00168                     }
00169                 }
00170                 Thread::wait(5);
00171             }
00172             Thread::wait(30);
00173     }
00174 }
00175 
00176 FastPWM speaker(PTA12);
00177 DigitalOut speaker_gnd(PTC4);
00178 //float speaker = 0;
00179 //float speaker_gnd = 0;
00180 
00181 //global variables used by interrupt routine
00182 volatile int i=512;
00183 float *wave;//[512];
00184  
00185 // Interrupt routine
00186 // used to output next analog sample whenever a timer interrupt occurs
00187 void Sample_timer_interrupt(void)
00188 {
00189     // send next analog sample out to D to A
00190     speaker = wave[i];
00191     // increment pointer and wrap around back to 0 at 128
00192     i = (i+1) & 0x07F;
00193 }
00194 
00195 int Siren_pitch = 1;
00196 void Siren_pitch_flip() {
00197     speaker.period(wave[--i]);
00198     if (i == 0)
00199         i = 128;
00200 }
00201 
00202 Ticker siren_pitch;
00203 Timeout siren_pitch_switch;
00204 bool Siren_last = 0;
00205 void Siren_faster() {
00206     siren_pitch.detach();
00207     siren_pitch.attach(&Siren_pitch_flip, 0.6/128);
00208 }
00209 void Siren_state(bool on) {
00210     if (Siren_last != on) {
00211         pc.printf("siren %d\r\n", on);
00212         siren_pitch.detach();
00213         Siren_pitch = 1;
00214         Siren_pitch_flip();
00215         siren_pitch.detach();
00216         if (on)
00217             siren_pitch.attach(&Siren_pitch_flip, 0.6/128);
00218         //siren_pitch_switch.attach(&Siren_faster, 2.0);
00219         speaker = on ? 0.5 : 0;
00220     }
00221     Siren_last = on;
00222 }
00223 
00224 DigitalOut Headlight_l(PTD7);
00225 DigitalOut Headlight_r(PTB8);
00226 
00227 #define Headlight_swap 3
00228 #define Headlight_flicker 15
00229 uint8_t Headlight_mode = 0;
00230 uint8_t Headlight_phase = 0;
00231 Ticker Headlight_pattern;
00232 void Headlight_interrupt() {
00233     Headlight_phase = (Headlight_phase + 1) % Headlight_flicker;
00234     Headlight_l = Headlight_mode == 1 || (Headlight_mode == 2) && ((Headlight_phase % 2) == 0) && (Headlight_phase * 2 > Headlight_flicker);
00235     Headlight_r = Headlight_mode == 1 || (Headlight_mode == 2) && ((Headlight_phase % 2) == 0) && (Headlight_phase * 2 < Headlight_flicker);
00236 }
00237 
00238 void Headlight_state(bool a, bool b) {
00239     uint8_t mode = b ? 1 : a ? 2 : 0;
00240     if (Headlight_mode != mode) {
00241         if (mode == 1)
00242             i = 512;
00243         Headlight_mode = mode;
00244         Headlight_pattern.detach();
00245         Headlight_interrupt();
00246         if (mode > 0)
00247             Headlight_pattern.attach_us(&Headlight_interrupt, (timestamp_t)(1000000/Headlight_swap/Headlight_flicker));
00248     }
00249 }
00250 
00251 uint8_t indicators = 0;
00252 uint8_t indicator_phase = 0;
00253 Ticker indicator_pattern;
00254 void indicator_interrupt() {
00255     indicator_phase++;
00256 }
00257 void indicator_init() {
00258     indicator_pattern.attach(&indicator_interrupt, 0.5);
00259 }
00260 
00261 TLC59108 *light_bar;
00262 #define light_bar_phases 10
00263 uint8_t light_bar_pattern[light_bar_phases+1][8] = {
00264     { 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
00265     { 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
00266     { 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
00267     { 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
00268     { 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 },
00269     { 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00 },
00270     { 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 },
00271     { 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00 },
00272     { 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00 },
00273     { 0x00, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00 },
00274     { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } };
00275 uint8_t light_bar_phase = 0;
00276 
00277 void light_bar_thread(const void*) {
00278     while(true) {
00279         Thread::wait(20);
00280         light_bar_phase = Siren_last ? (light_bar_phase + 1) % light_bar_phases : light_bar_phases;
00281         uint8_t* pattern = light_bar_pattern[light_bar_phase];
00282         pattern[4] = Headlight_l ? 0xFF : 0x00;
00283         pattern[5] = Headlight_r ? 0xFF : 0x00;
00284         pattern[6] = (indicator_phase % 2) == 0 && indicators == 1 ? 0xFF : 0x00;
00285         pattern[7] = (indicator_phase % 2) == 0 && indicators == 2 ? 0xFF : 0x00;
00286         pc.printf("light bar %d\r\n", light_bar->setBrightness(pattern));
00287     }
00288 }
00289 Thread *t_light_bar_thread;
00290 
00291 void light_bar_init() {
00292     light_bar = new TLC59108(PTE0, PTE1);
00293     pc.printf("light bar init %d\r\n", light_bar->setLedOutputMode(TLC59108::LED_MODE::PWM_INDGRP));
00294     light_bar->setGroupBrightness(100);
00295     t_light_bar_thread = new Thread(&light_bar_thread);
00296 }
00297 
00298 void sleep_loop(void const *argument) {
00299     while (true) {
00300         sleep();
00301         Thread::wait(100);
00302     }
00303 }
00304 
00305 #ifdef TARGET_KL25Z
00306     PwmOut r(LED_RED);
00307     //float r = 1.0f;
00308     PwmOut g(LED_GREEN);
00309     //float g = 1.0f;
00310     PwmOut b(LED_BLUE);
00311     //float b = 1.0f;
00312     DigitalOut nun_vdd(PTE31);
00313     WiiChuck *nun;
00314     uint8_t nun_settled = 0;
00315     bool nun_init(nunchuk *next) {
00316         //return false;
00317         for (int i = 0; i < 5; i++) {
00318             if (nun)
00319                 delete nun;
00320             nun_vdd = 1;
00321             Thread::wait(50);
00322             nun = new WiiChuck(PTE0, PTE1, pc);
00323             if (nun->Read(&next->X, &next->Y, &next->aX, &next->aY, &next->aZ, &next->C, &next->Z)) {
00324                 nun_settled = 0;
00325                 Thread::wait(10);
00326                 return true;
00327             }
00328             pc.printf("nun_init error\r\n");
00329             nun_vdd = 0;
00330             Thread::wait(50);
00331         }
00332         return false;
00333     }
00334     void nun_sleep() {
00335         nun_vdd = 0;
00336     }
00337     RFM69 radio(PTD2, PTD3, PTC5, PTD0, PTA13);
00338 #else
00339     WiiChuck *nun = new WiiChuck(p9, p10, pc);
00340     bool nun_init(nunchuk *) {}
00341     void nun_sleep() {}
00342 #endif
00343 
00344 Timer rx_last_contact;
00345 bool rx_last_contact_trip = 0;
00346 bool rx_to_snooze = true;
00347 bool rx_snoozing = false;
00348 bool rx_snoozed() {
00349     if (rx_snoozing) {
00350         pc.printf("still snoozing\r\n");
00351         Thread::signal_wait(0x1);
00352         pc.printf("signalled?\r\n");
00353     }
00354     return true;
00355 }
00356 
00357 void rx_snoozer(void const *mainThreadID) {
00358     pc.printf("snooze rx %f\r\n", rx_last_contact.read());
00359     
00360     rx_snoozing = true;
00361     radio.sleep();
00362     WakeUp::set((rx_last_contact_trip |= (rx_last_contact.read() < 60))? 2 : 10);
00363     deepsleep();
00364     stops_sent = 0;
00365     rx_snoozing = false;
00366     rx_to_snooze = true;
00367     pc.printf("unsnooze rx\r\n");
00368     osSignalSet((osThreadId)mainThreadID, 0x1);
00369 }
00370 
00371 int main()
00372 {
00373     RtosTimer rx_snooze(&rx_snoozer, osTimerOnce, (void*)osThreadGetId());
00374 
00375 #ifndef USBSerial
00376     pc.baud(115200);
00377 #endif
00378 
00379     r = g = b = 1;
00380     gnd = 0;
00381     ir.period_us(1000/38);
00382 
00383     speaker_gnd = 0;
00384     speaker = 0;
00385     //speaker.period(1.0/200000.0);
00386 
00387     /*
00388     speaker = 0.2;
00389     while(1) {
00390     speaker.period(.8/554.365);
00391     wait(.8);
00392     speaker.period(.8/523.251);
00393     wait(.8);
00394     }
00395     //speaker = 0.2;
00396     // set up a timer to be used for sample rate interrupts
00397     Ticker Sample_Period;
00398     // precompute 128 sample points on one sine wave cycle 
00399     // used for continuous sine wave output later
00400     for(int k=0; k<128; k++) {
00401         wave[k]=((1.0 + sin((float(k)/128.0*6.28318530717959)))/2.0);
00402         // scale the sine wave from 0.0 to 1.0 - as needed for AnalogOut arg 
00403     }
00404     // turn on timer interrupts to start sine wave output
00405     // sample rate is 500Hz with 128 samples per cycle on sine wave
00406     while(1) {
00407         Sample_Period.detach();
00408         Sample_Period.attach(&Sample_timer_interrupt, 1.0/(554.365*128));
00409         wait(.8);
00410         Sample_Period.detach();
00411         Sample_Period.attach(&Sample_timer_interrupt, 1.0/(523.251*128));
00412         wait(.8);
00413     }
00414     // everything else needed is already being done by the interrupt routine
00415     */
00416 
00417     nunchuk n1, n2;
00418     nunchuk *n = &n1, *next = &n2;
00419     bool sender = nun_init(n);
00420     if (sender) {
00421         pc.printf("chuck attached\r\n");
00422         radio.initialize(FREQUENCY, NODE_ID, NETWORKID);
00423         central_time.start();
00424     } else {
00425         pc.printf("chuck unavailable\r\n");
00426         radio.initialize(FREQUENCY, GATEWAY_ID, NETWORKID);
00427         // only relevant to the receiver
00428         thread = new Thread(ir_thread);
00429         rx_last_contact.start();
00430         // these break the nunchuk
00431         wave = new float[i];
00432         int m = i-128;
00433         for(int k = 0; k < m; k++) {
00434             // ramp up
00435             wave[i-k-1] = 1.0/(1000+k*400.0/m);
00436         }
00437         for(int k = 0; k < 128; k++) {
00438             // LFO
00439             wave[127-k] = 1.0/(1400+200*sin(k/128.0*6.28318530717959));
00440         }
00441         WakeUp::calibrate();
00442         light_bar_init();
00443         indicator_init();
00444    }
00445     radio.encrypt("0123456789054321");
00446     //radio.promiscuous(false);
00447     radio.setHighPower(true);
00448     radio.setPowerLevel(20);
00449     radio.rcCalibration();
00450     //radio.readAllRegs();
00451     pc.printf("temp %d\r\n", radio.readTemperature(-1));
00452 
00453     //Thread t_sleep_loop(&sleep_loop);
00454     bool read = false;
00455     while(1) {
00456         if (sender) {
00457             if (/*nun_settled++ > 2 && */central_time_trip |= (central_time.read() > 5)) {
00458                 #if DEBUG
00459                 pc.printf("snooze tx %f\r\n", central_time.read());
00460                 g = 0; Thread::wait(10); g = 1; Thread::wait(10);
00461                 #endif
00462                 nun_sleep();
00463                 radio.sleep();
00464                 Thread::wait(10);
00465                 WakeUp::set(2);
00466                 deepsleep();
00467                 nun_init(next);
00468                 #if DEBUG
00469                 r = 0; Thread::wait(10); r = 1; Thread::wait(10);
00470                 pc.printf("unsnooze tx\r\n");
00471                 #endif
00472             }
00473             nunchuk *last = n;
00474             n = next;
00475             next = last;
00476             read = nun->Read(&n->X, &n->Y, &n->aX, &n->aY, &n->aZ, &n->C, &n->Z);
00477             n->sum = 0;
00478             n->sum = calculate_crc8((char*)n, sizeof(struct nunchuk));
00479         }
00480         else if (rx_snoozed() && radio.receiveDone()) {
00481             rx_last_contact.reset();
00482             rx_last_contact_trip = 0;
00483             rx_snooze.stop();
00484             rx_to_snooze = true;
00485             //pc.printf("rssi %d\r\n", radio.RSSI);
00486             read = (radio.DATALEN == sizeof(struct nunchuk));
00487             if (read) {
00488                 memcpy((void*)next, (const void*)radio.DATA, radio.DATALEN);
00489                 uint8_t sum = next->sum;
00490                 next->sum = 0;
00491                 read = sum == calculate_crc8((char*)next, sizeof(struct nunchuk));
00492                 if (read) {
00493                     nunchuk *last = n;
00494                     n = next;
00495                     next = last;
00496                 }
00497             }
00498             if (radio.DATALEN > 30)
00499                 pc.printf((const char*)radio.DATA);
00500             if (!read)
00501                 pc.printf("len %d\r\n", radio.DATALEN);
00502         } else if (rx_to_snooze) {
00503             rx_snooze.start(100);
00504             rx_to_snooze = false;
00505         }
00506         if(read)
00507         {
00508             Siren_state(n->C);
00509             Headlight_state(n->C, n->Z);
00510             float x = n->X - 128, y = n->Y - 128;
00511             float R = x*x + y*y, p = atan2(y, x) * 4 / M_PI - 0.5;
00512             mag = sqrt(R)/42; if (mag > 2) mag = 10;
00513             //const char *directions[8] = { "XR", "RR", "RX", "FR", "FX", "FF", "XF", "RF" };
00514 
00515             int c = 7;
00516             if (p > -4) c = 0;
00517             if (p > -3) c = 1;
00518             if (p > -2) c = 2;
00519             if (p > -1) c = 3;
00520             if (p >  0) c = 4;
00521             if (p >  1) c = 5;
00522             if (p >  2) c = 6;
00523             if (p >  3) c = 7;
00524             direction = c;
00525 
00526 //#if DEBUG > 1
00527             pc.printf("%d: ", sizeof(struct nunchuk));
00528             pc.printf("x%3d y%3d c%1d z%1d -- ", n->X, n->Y, n->C, n->Z);
00529             pc.printf("x%d y%d z%d -- %8.2f %8.4f %s %d \r\n", n->aX, n->aY, n->aZ, R, p, directions[direction], mag);
00530 
00531             //radio.send(GATEWAY_ID, (const void*)"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA", 50, false);
00532 //#endif
00533             if (sender)
00534                 if (central_time.read() < 1)
00535                     radio.send(GATEWAY_ID, (const void*)n, sizeof(struct nunchuk), false);
00536                 else
00537                     radio.sleep();
00538 
00539 #if DEBUG
00540             char wake[100];
00541 #endif
00542 #ifdef TARGET_KL25Z
00543             indicators = 0;
00544             if (R <= 0.1) {
00545                 double twitch = pow(n->aX - next->aX, 2.0) + pow(n->aY - next->aY, 2.0) + pow(n->aZ - next->aZ, 2.0);
00546 #if DEBUG > 1
00547                 pc.printf("twitch %f\r\n", twitch);
00548 #endif
00549                 r = 1.0f;
00550                 g = 1.0f;
00551                 b = 1.0f;
00552                 if (n->C || n->Z || /*nun_settled > 2 &&*/ twitch > 1000) {
00553                     Thread::wait(10);
00554 #if DEBUG
00555                     pc.printf("c %d z %d twitch %f\r\n", n->C, n->Z, twitch);
00556                     snprintf(wake, 100, "Wake for twitch %f\r\n", twitch);
00557                     pc.printf(wake);
00558                     radio.send(GATEWAY_ID, (const void*)wake, 100, false);
00559 #endif
00560                     central_time.reset();
00561                     central_time_trip = 0;
00562                 }
00563             } else {
00564                 switch(direction) {
00565                     // left
00566                     case 0: //XR(); break;
00567                     case 6: //XF(); break;
00568                     case 7: //RF(); break;
00569                     indicators = 2; break;
00570                     // right
00571                     case 2: //RX(); break;
00572                     case 3: //FR(); break;
00573                     case 4: //FX(); break;
00574                     indicators = 1; break;
00575                     // neither
00576                     case 1: //RR(); break;
00577                     case 5: //FF(); break;
00578                     break;
00579                 }
00580 #if DEBUG
00581                 snprintf(wake, 100, "Wake for R %f\r\n", R);
00582                 pc.printf(wake);
00583                 radio.send(GATEWAY_ID, (const void*)wake, 100, false);
00584 #endif
00585                 if (R < 40) {
00586                     if (!central) {
00587                         pc.printf("central\r\n");
00588                         central = true;
00589                     }
00590                 } else {
00591                     central_time.reset();
00592                     central_time_trip = 0;
00593                     if (central) {
00594                         pc.printf("go %s\r\n", directions[direction]);
00595                         central = false;
00596                     }
00597                 }
00598                 R = R/20000;
00599                 float pal[8][3] = {
00600                     {   0,   0,   1 },
00601                     {   0,   1,   1 },
00602                     {   0,   1,   0 },
00603                     {   1,   1,   0 },
00604                     {   1, 0.5,   0 },
00605                     {   1,   0,   0 },
00606                     {   1,   0,   1 },
00607                     { 0.5,   0,   1 },
00608                 };
00609                 c = (c + 4) % 8;
00610                 r = 1.0f - pal[c][0] * R;
00611                 g = 1.0f - pal[c][1] * R;
00612                 b = 1.0f - pal[c][2] * R;
00613             }
00614 #endif
00615         }
00616         else if (sender)
00617         {
00618             pc.printf("Error\r\n");
00619         }
00620         if (sender)
00621             wait(0.01);
00622         else
00623             Thread::wait(10);
00624     }
00625 }