File content as of revision 0:f3297abb5739:

#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);
    }
}