Diff: main.cpp

Revision:

0:7092b32c717c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jan 10 04:25:57 2015 +0000
@@ -0,0 +1,113 @@
+#include "mbed.h"
+
+PwmOut moter[] = {p21,p22,p23,p24};
+BusOut selecter(p5,p6,p7,p8,p9,p10,p11,p12);
+Serial pc(USBTX,USBRX);
+Serial mc(p13,p14);
+#define pi 3.141592
+
+int main() {
+    uint8_t data_r1,data_r2,data_l1,data_l2,sw_data5,sw_data6,start;
+    double signal_r,signal_l,signal_a1,signal_a2,angle1,angle2;
+    uint8_t select_r,select_l,select_all,check,checker;
+    int n,m,x,z;
+    
+    while(1){
+        select_r = 0;
+        select_l = 0;
+        signal_r = 0;
+        signal_l = 0;
+        signal_a1 = 0;
+        signal_a2 = 0;
+        if(mc.readable() == 1){
+            start = mc.getc();
+            data_r1 = mc.getc();
+            data_r2 = mc.getc();
+            data_l1 = mc.getc();
+            data_l2 = mc.getc();
+            sw_data5 = mc.getc();
+            sw_data6 = mc.getc();
+            check = mc.getc();
+            
+            if(start == 255){
+                if(data_r1 != 0){
+                    signal_r = data_r1/200.0000;
+                    select_r = 16;
+                }else if(data_r2 != 0){
+                    signal_r = data_r2/200.0000;
+                    select_r = 32;
+                }
+                if(data_l1 != 0){
+                    signal_l = data_l1/200.0000;
+                    select_l = 64;
+                }else if(data_l2 != 0){
+                    signal_l = data_l2/200.0000;
+                    select_l = 128;
+                }
+                checker = data_r1^data_r2^data_l1^data_l2^sw_data5^sw_data6;
+                select_all = sw_data5+sw_data6+select_r+select_l;
+                
+                if(sw_data5 == 1){
+                    n += 8;
+                    z = -90;
+                }else if(sw_data5/2 == 1){
+                    z += 8;
+                    n = -90;
+                }else if(sw_data5 == 0){
+                    n = -90;
+                    z = -90;
+                }
+                if((n+z+90) > -90 && (n+z+90) < 0){
+                    angle1 = (n+z+90)*pi/180;
+                    signal_a1 = (1.000+sin(angle1))/2;
+                }else if((n+z+90) == 0){
+                    signal_a1 = 0.500;
+                }else if((n+z+90) > 0 && (n+z+90) < 90){
+                    angle1 = (n+z+90)*pi/180.0;
+                    signal_a1 = sin(angle1)/2+0.5000;
+                }else if((n+z+90) > 89){
+                    signal_a1 = 1.000;
+                }else if(n == -90 && z == -90){
+                    signal_a1 = 0;
+                    angle1 = 0;
+                }
+                if(sw_data6/4 == 1){
+                    m += 8;
+                    x = -90;
+                }else if(sw_data6/8 == 1){
+                    x += 8;
+                    m = -90;
+                }else if(sw_data6 == 0){
+                    m = -90;
+                    x = -90;
+                }
+                if(m > -90 && m < 0 || x > -90 && x < 0){
+                    angle2 = (m+x+90)*pi/180;
+                    signal_a2 = (1.000+sin(angle2))/2;
+                }else if((m+x+90) == 0){
+                    signal_a2 = 0.500;
+                }else if(m > 0 && m < 90 || x > 0 && x < 90){
+                    angle2 = (m+x+90)*pi/180.0;
+                    signal_a2 = sin(angle2)/2+0.5000;
+                }else if((m+x+90) > 89){
+                    signal_a2 = 1.000;
+                }else if(m == -90 && x == -90){
+                    signal_a2 = 0;
+                    angle2 = 0;
+                }
+                
+                pc.printf("%f_%f_%d_%d_%d_%d\n",signal_a1,signal_a2,n,z,m,x);
+                //pc.printf("%f_%f_%u_%u_%u_%u\n",signal_r,signal_l,select_r,select_l,select_all,check);
+                //pc.printf("%u_%u\n",checker,check);
+                
+                if(check == checker){
+                    moter[2] = signal_r;
+                    moter[3] = signal_l;
+                    moter[0] = signal_a1;
+                    moter[1] = signal_a2;
+                    selecter = select_all;
+                }
+            }
+        }
+    }
+}
\ No newline at end of file