File content as of revision 9:687dddda7432:

#include "mbed.h"
#define pi 3.141592653589793
Serial sousin(p28,p27);
Serial hunter(p9,p10);
Serial pc(USBTX,USBRX);
Timer em;
DigitalOut  check (LED1);
BusOut check2 (LED2,LED3);

BusOut mo1 (p13,p14);
BusOut mo2 (p15,p16);
BusOut mo3 (p17,p18);
PwmOut mop1 (p21);
PwmOut mop2 (p22);
PwmOut mop3 (p23);
double get_theta(double stickx,double sticky)
{
    double x,y,theta;
    if (stickx>0.5){
        x = stickx - 0.5;
    }else if(stickx <= 0.5){
        x = -(0.5 - stickx);
    }else{
        x = 0;
    }
    if (sticky>0.5){
        y = sticky - 0.5;
    }else if(sticky <= 0.5){
        y = -(0.5 - sticky);
    }else{
        y = 0;
    }
    if(!(x==0 && y ==0)){
        theta =atan(y/x);
    }else {
            
       theta = 0;
    }
    if(x>0 && y > 0){
        theta = pi+theta;
    }else
    if(x>0 && y < 0){
       theta = pi+theta;
    }else
    if(x<0 && y < 0){
        theta = theta;
    }else{
     theta = 2*pi+theta;
    }
    return theta;
}
int get_serial(double *stx,double *sty,int8_t *sw)
{   
    int error,x,y;
    if (sousin.getc() == 255)
    {
        x = sousin.getc();
        y = sousin.getc();
        *sw = sousin.getc();
        error = sousin.getc();
        *stx = x/100.0;
        *sty = y/100.0; 
        if (error == x^y^(*sw)){
            em.reset();
            pc.printf("sw-->%d\n\r",x);
            return 0;
        }else{
            return 1;
        }
    }else{
        return 1;
    }
}
void allstop()
{
    mo1 = 0;
    mo2 = 0;
    mo3 = 0;
    check = 1;
    while(1){}
}
void omni_cont(int8_t hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
{   
    
    double md1,md2,md3,t1;
    if (hun == 2)
    {
        t1 = 0.1;
    }else if(hun == 1)
    {
        t1 = -0.1;
    }else{
        t1 = 0;
    }
    md1 = -(sin(theta)*0.9+t1);
    md2 = (sin(theta-pi*2/3)*0.9+t1);
    md3 = (sin(theta-pi*4/3)*0.9+t1);
    if (md1 < 0){
        *m1 = 2;
        *mp1 = -md1;
    }else{
        *m1 = 1;
        *mp1 = md1;
    } 
    if (md2 < 0){
        *m2 = 2;
        *mp2 = -md2;
    }else{
        *m2 = 1;
        *mp2 = md2;
    } 
    if (md3 < 0){
        *m3 = 2;
        *mp3 = -md3;
    }else{
        *m3 = 1;
        *mp3 = md3;
    }
} 
int8_t get_hunter()
{
    int8_t s;
    if (hunter.readable()){
        if (hunter.getc() == 48){
            s = hunter.getc();
        }
    }
    if (s == 255)
    {
        allstop();
    }
    pc.printf("s-->%d\n\r",s);
    return s;
}
int main() {
    hunter.baud(9600);
    sousin.baud(9600);
    int ec,motor[3];
    int8_t sw1,hun;
    double sx,sy,motp[3],theta;
    while(1) {
        //pc.printf("START\n\r");
        hun = get_hunter();
        check2 = hun;
        ec = get_serial(&sx,&sy,&sw1);
        ec =0;
        //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
        //pc.printf("%d\n\r",hun);
        if (ec == 0){
            em.reset();
            theta = get_theta(sx,sy);
            if (sw1 == -1){
                omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
            }else if(hun ==1){
                motor[0] = 1;
                motor[1] = 1;
                motor[2] = 1;
                motp[0] = 0.5;
                motp[1] = 0.5;
                motp[2] = 0.5;
            }else if (hun == 2){
                motor[0] = 2;
                motor[1] = 2;
                motor[2] = 2;
                motp[0] = 0.5;
                motp[1] = 0.5;
                motp[2] = 0.5;
            }else{
                motor[0] = 0;
                motor[1] = 0;
                motor[2] = 0;
                motp[0] = 0;
                motp[1] = 0;
                motp[2] = 0;
            }
            mo1 = motor[0];
            mo2 = motor[1];
            mo3 = motor[2];
            mop1 = motp[0];
            mop2 = motp[1];
            mop3 = motp[2];
        }else{
            em.start();
            //if(em.read_ms()>200){
            //    allstop();
            //}
        }                     
    }
}