main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "motordriver.h"
00004 #include "ReceiverIR.h"
00005 
00006 #include "CodecSwift.h"
00007 #include "DecodeSwift.h"
00008 
00009 
00010 Serial pc(USBTX,USBRX);
00011 
00012 ReceiverIR ir_rx(p30);
00013 RemoteIR::Format format;
00014 uint8_t buf[32];
00015 int bitcount;
00016 
00017 DecodeSwift swift;
00018 CodecSwift::swiftPropo_t swiftData;
00019 CodecSwift::normalizePropo_t normalize;
00020 
00021 
00022 Motor Right(p24, p27, p26, 1);
00023 Motor Left(p22, p14, p13, 1);
00024 
00025 Timeout timeout;
00026 
00027 void isr_timeout(void)
00028 {
00029     Right.speed(0);
00030     Left.speed(0);
00031 }
00032 
00033 DigitalOut led1(LED1);
00034 DigitalOut led2(LED2);
00035 DigitalOut led3(LED3);
00036 DigitalOut led4(LED4);
00037 
00038 int main()
00039 {
00040     pc.baud(38400);
00041 
00042     timeout.detach();
00043     timeout.attach_us(&isr_timeout, 100 * 1000);
00044 
00045     uint8_t count = 0;
00046     float theta;
00047     float x;
00048     float y;
00049     float r;
00050     float l;
00051 
00052 
00053     while(1) {
00054 
00055         // IR data Monitor
00056         led1 = 0;
00057         led2 = 0;
00058         led3 = 0;
00059         led4 = 0;
00060 
00061         switch(ir_rx.getState()) {
00062             case ReceiverIR::Idle:
00063                 led1 = 1;
00064                 break;
00065             case ReceiverIR::Receiving:
00066                 led2 = 1;
00067                 break;
00068             case ReceiverIR::Received:
00069                 led3 = 1;
00070                 break;
00071             default:
00072                 led4 = 1;
00073                 break;
00074         }
00075 
00076 
00077         // IR data to Motor data
00078         if (ir_rx.getState() == ReceiverIR::Received) {
00079             bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
00080 
00081             bool ans = swift.normalize(buf, &normalize);
00082 
00083 
00084             if(ans == true) {
00085                 timeout.detach();
00086                 timeout.attach_us(&isr_timeout, 100 * 1000);
00087 
00088                 theta = atan2f(normalize.elevator, normalize.ladder);
00089                 x = -normalize.slottle * cosf(theta);
00090                 y = -normalize.slottle * sinf(theta);
00091 
00092                 if((fabsf(normalize.elevator) < 0.01) && (fabsf(normalize.ladder) < 0.01)) {
00093                     r = 0;
00094                     l = 0;
00095                 } else if(x > 0.0) {
00096                     r = (y - x / 2.0);
00097                     l = (y + x / 2.0);
00098                 } else {
00099                     r = (y - x / 2.0);
00100                     l = (y + x / 2.0);
00101                 }
00102 
00103 #if 0
00104                 printf("x = %f y = %f r = %f l = %f\n",x, y, r, l);
00105 #endif
00106 
00107                 Right.speed(r);
00108                 Left.speed(-l);
00109             } else if( ++count > 10) {
00110                 count = 0;
00111                 Right.speed(0);
00112                 Left.speed(0);
00113             }
00114 
00115 #if 0
00116             if(ans == true) {
00117                 printf("count = %02x band = %1d  slottle = %f trim = %f ladder = %f elevator = %f\n",normalize.count,normalize.band,normalize.slottle,normalize.trim,normalize.ladder, normalize.elevator);
00118             } else {
00119                 printf("NG\n");
00120             }
00121 #endif
00122         }
00123 
00124 
00125 
00126     }
00127 }