File content as of revision 0:7092b32c717c:

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