Diff: main.cpp

Revision:

0:f3297abb5739

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jan 10 04:30:31 2015 +0000
@@ -0,0 +1,110 @@
+#include "mbed.h"
+
+PwmOut mytest[] = {LED1,LED2,LED3,LED4};
+AnalogIn stick[] = {p20,p19};
+Serial pc(USBTX,USBRX);
+Serial mc(p13,p14);
+DigitalIn swith[] = {p5,p6,p7,p8};
+
+int main() {
+    double analog_r,analog_l,analog_ra[10],analog_la[10];
+    int i;
+    uint8_t data_r1,data_r2,data_l1,data_l2,start = 255,check;
+    uint8_t sw_data1,sw_data2,sw_data3,sw_data4,sw_data5,sw_data6;
+    double signal_r1,signal_r2,signal_l1,signal_l2;
+    double ave_r,ave_l,sum_r,sum_l;
+    swith[0].mode(PullUp);
+    swith[1].mode(PullUp);
+    swith[2].mode(PullUp);
+    swith[3].mode(PullUp);
+    wait(0.25);
+    for(i=0;i<10;i++){
+        analog_ra[i] = stick[0];
+        analog_la[i] = stick[1];
+        sum_r += analog_ra[i];
+        sum_l += analog_la[i];
+        pc.printf("%f_%f\n",analog_ra[i],analog_la[i]);
+        wait(0.1);
+    }
+    ave_r = sum_r/10;
+    ave_l = sum_l/10;
+    mytest[0] = 1;
+    wait(0.2);
+    
+    while(1) {
+        data_r1 = 0;
+        data_r2 = 0;
+        data_l1 = 0;
+        data_l2 = 0;
+        analog_r = stick[0];
+        analog_l = stick[1];
+        
+        if(analog_r > ave_r-0.050 && analog_r < ave_r+0.050){
+            signal_r1 = 0;
+            signal_r2 = 0;
+            mytest[0] = signal_r1;
+            mytest[1] = signal_r2;
+        }else if(analog_r < ave_r-0.050){
+            signal_r1 = 0;
+            signal_r2 = (analog_r-ave_r)/ave_r*(-1);
+            data_r1 = (signal_r2+0.0005)*200;
+            mytest[0] = signal_r1;
+            mytest[1] = signal_r2;
+        }else if(analog_r > ave_r+0.050){
+            signal_r2 = 0;
+            signal_r1 = (analog_r-ave_r)/ave_r;
+            data_r2 = (signal_r1+0.0005)*200;
+            mytest[0] = signal_r1;
+            mytest[1] = signal_r2;
+        }
+        if(analog_l > ave_l-0.050 && analog_l < ave_l+0.050){
+            signal_l1 = 0;
+            signal_l2 = 0;
+            mytest[2] = signal_l1;
+            mytest[3] = signal_l2;
+        }else if(analog_l < ave_l-0.050){
+            signal_l1 = 0;
+            signal_l2 = (analog_l-ave_l)/ave_l*(-1);
+            data_l1 = (signal_l2+0.0005)*200;
+            mytest[2] = signal_l1;
+            mytest[3] = signal_l2;
+        }else if(analog_l > ave_l+0.050){
+            signal_l2 = 0;
+            signal_l1 = (analog_l-ave_l)/ave_l;
+            data_l2 = (signal_l1+0.0005)*200;
+            mytest[2] = signal_l1;
+            mytest[3] = signal_l2;
+        }
+        
+        sw_data1 = !swith[0]*1;
+        sw_data2 = !swith[1]*2;
+        sw_data3 = !swith[2]*4;
+        sw_data4 = !swith[3]*8;
+        if(sw_data1 != 0 && sw_data2 == 0){
+            sw_data5 = sw_data1;
+        }else if(sw_data1 == 0 && sw_data2 != 0){
+            sw_data5 = sw_data2;
+        }else if(sw_data1 == 0 && sw_data2 == 0){
+            sw_data5 = 0;
+        }
+        if(sw_data3 != 0 && sw_data4 == 0){
+            sw_data6 = sw_data3;
+        }else if(sw_data3 == 0 && sw_data4 != 0){
+            sw_data6 = sw_data4;
+        }else if(sw_data3 == 0 && sw_data4 == 0){
+            sw_data6 = 0;
+        }
+        check = data_r1^data_r2^data_l1^data_l2^sw_data5^sw_data6;
+        
+        mc.putc(start);
+        mc.putc(data_r1);
+        mc.putc(data_r2);
+        mc.putc(data_l1);
+        mc.putc(data_l2);
+        mc.putc(sw_data5);
+        mc.putc(sw_data6);
+        mc.putc(check);
+        
+        pc.printf("%u_%u_%u_%u_%u_%u_%f_%f_%f_%f_%u\n",data_r1,data_r2,data_l1,data_l2,sw_data5,sw_data6,ave_r,ave_l,analog_r,analog_l,check);
+    }
+}