Diff: main.cpp

Revision:

0:9d3b35ce3c45

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Aug 03 01:48:34 2013 +0000
@@ -0,0 +1,127 @@
+#include "mbed.h"
+#include "math.h"
+#include "motordriver.h"
+#include "ReceiverIR.h"
+
+#include "CodecSwift.h"
+#include "DecodeSwift.h"
+
+
+Serial pc(USBTX,USBRX);
+
+ReceiverIR ir_rx(p30);
+RemoteIR::Format format;
+uint8_t buf[32];
+int bitcount;
+
+DecodeSwift swift;
+CodecSwift::swiftPropo_t swiftData;
+CodecSwift::normalizePropo_t normalize;
+
+
+Motor Right(p24, p27, p26, 1);
+Motor Left(p22, p14, p13, 1);
+
+Timeout timeout;
+
+void isr_timeout(void)
+{
+    Right.speed(0);
+    Left.speed(0);
+}
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+int main()
+{
+    pc.baud(38400);
+
+    timeout.detach();
+    timeout.attach_us(&isr_timeout, 100 * 1000);
+
+    uint8_t count = 0;
+    float theta;
+    float x;
+    float y;
+    float r;
+    float l;
+
+
+    while(1) {
+
+        // IR data Monitor
+        led1 = 0;
+        led2 = 0;
+        led3 = 0;
+        led4 = 0;
+
+        switch(ir_rx.getState()) {
+            case ReceiverIR::Idle:
+                led1 = 1;
+                break;
+            case ReceiverIR::Receiving:
+                led2 = 1;
+                break;
+            case ReceiverIR::Received:
+                led3 = 1;
+                break;
+            default:
+                led4 = 1;
+                break;
+        }
+
+
+        // IR data to Motor data
+        if (ir_rx.getState() == ReceiverIR::Received) {
+            bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+
+            bool ans = swift.normalize(buf, &normalize);
+
+
+            if(ans == true) {
+                timeout.detach();
+                timeout.attach_us(&isr_timeout, 100 * 1000);
+
+                theta = atan2f(normalize.elevator, normalize.ladder);
+                x = -normalize.slottle * cosf(theta);
+                y = -normalize.slottle * sinf(theta);
+
+                if((fabsf(normalize.elevator) < 0.01) && (fabsf(normalize.ladder) < 0.01)) {
+                    r = 0;
+                    l = 0;
+                } else if(x > 0.0) {
+                    r = (y - x / 2.0);
+                    l = (y + x / 2.0);
+                } else {
+                    r = (y - x / 2.0);
+                    l = (y + x / 2.0);
+                }
+
+#if 0
+                printf("x = %f y = %f r = %f l = %f\n",x, y, r, l);
+#endif
+
+                Right.speed(r);
+                Left.speed(-l);
+            } else if( ++count > 10) {
+                count = 0;
+                Right.speed(0);
+                Left.speed(0);
+            }
+
+#if 0
+            if(ans == true) {
+                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);
+            } else {
+                printf("NG\n");
+            }
+#endif
+        }
+
+
+
+    }
+}