File content as of revision 16:2bb07173f8bf:

#include "mbed.h"
#include "HMC6352.h"
#define pi 3.141592653589793
Serial sousin(p28,p27);
Serial pc(USBTX,USBRX);
Timer em;
Timer t;
HMC6352 sensor(p9,p10);
DigitalOut  check (LED1);
BusOut check2 (LED2,LED3);
BusOut mo1 (p15,p16);
BusOut mo2 (p17,p18);
BusOut mo3 (p19,p20);
BusIn hunter(p13,p14);
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(int hun,double aval,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
{   
    
    double md1,md2,md3,t1;
    if (hun == 2)
    {
        t1 = 0.2;
    }else if(hun == 1)
    {
        t1 =-0.2;
    }else{
        t1 = 0;
    }
    md1 = sin(theta)*0.7+t1;
    md2 = sin(theta-pi*2/3)*0.7+t1;
    md3 = sin(theta-pi*4/3)*0.7+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;
    }
} 

int main() {
    sousin.baud(9600);
    sensor.setOpMode(HMC6352_CONTINUOUS,1,20);
    int i,ec,motor[3],val;
    double asen,asen2,sen,aval;
    int8_t sw1,hun;
    i = 0;
    asen = sensor.sample()/10.0;
    asen2 = asen;
    double sx,sy,motp[3],theta;
    while(1) {
        //pc.printf("START\n\r");
        sen = sensor.sample()/10.0;
        //pc.printf("%f\n\r",sen);
        hun = hunter;
        check2 = hun;
        ec = get_serial(&sx,&sy,&sw1);
        if (sw1 == -2){
            hun = 1;
        }
        if (sw1 == -3){
            hun =2;
        }
        ec =0;
        if(hun == 1||hun == 2){
            t.start();
        }else {
            i = 0;
            t.reset();
            t.stop();
        }
        if ((hun == 1) && (t.read_ms()>(i*1))){
            asen = asen+2;
            i++;
        }else if((hun == 2) && (t.read_ms()>(i*10))){
            asen = asen-2;
            i++;
        }
        if (asen > 360){
            asen = 0;
        }
        if (asen < 0){
            asen = 360;
        }
        val = sen+540-asen;
        val %= 360;
        val -= 180;
        printf("   :%d\n\r",val);
        if (val > 1){
            hun = 2;
        }else if (val <-1){
            hun = 1;
        }else{
            hun = 0;
        } 
        aval = val;
        /*if(sen> asen-180){
            hun = 2;
        }else if (sen< asen-180){
            hun = 1;
        }else {
            hun = 0;
        }
        */
        //pc.printf("asen-->%f",asen);
        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);
            theta -= (asen-asen2)*pi/180;
            //pc.printf("%f\n\r",theta);
            if (sw1 == -1){
                omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
                mo1 = motor[0];
                mo2 = motor[1];
                mo3 = motor[2];
                mop1 = motp[0];
                mop2 = motp[1];
                mop3 = motp[2];
            }else
            if (hun == 2){
                mo1 = 1;
                mo2 = 1;
                mo3 = 1;
                mop1 = 0.5;
                mop2 = 0.5;
                mop3 = 0.5;
            }else if(hun == 1){
                mo1 = 2;
                mo2 = 2;
                mo3 = 2;
                mop1 = 0.5;
                mop2 = 0.5;
                mop3 = 0.5;
            }else {
                mo1 = 0;
                mo2 = 0;
                mo3 = 0;
                mop1 = 0;
                mop2 = 0;
                mop3 = 0;
            }
        }else{
            em.start();
            
            //if(em.read_ms()>200){
            //    allstop();
            //}
        }                     
    }
}