File content as of revision 1:ad8acc6cfba7:

/*
motor 555
Lipo 4sel
Linear accelerator
Not mount QEI
*/
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#include "fusion_Motor.h"
#include "Pin.h"
#define Lx 0.0001
#define Ku 0.48000//0.922000
#define Pu 0.0125
#define Kp Ku*0.6
#define Ti Pu*0.5
#define Td Pu*0.125


char data=0,speed=0,sot=0,Data2=0;
#define P Kp
#define I Kp/Ti
#define D Kp*Td
#define Mstate 6
#define SorT 5
#define Databit 0
#define TIME 0.0125
Timer Time;
DigitalOut Elec(D15);
/*
PID p(P,I,D,&Time);
QEI i(QEI1_A,QEI1_B,NC,50,&Time);
Motor d(PD_2,PA_13,PA_14,PA_15,PC_9);
fusion_Motor mecanam3(i,p,d);
*/

float duty_max;
Serial device(D8,D2);
/*
PID *pid;
QEI *wheel;
Motor *motor;
*/
PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
QEI wheel[4] = {QEI(QEI1_A,QEI1_B,NC,100,&Time),QEI(QEI2_A,QEI2_B,NC,100,&Time),QEI(QEI3_A,QEI3_B,NC,100,&Time),QEI(QEI4_A,QEI4_B,NC,100,&Time)};
Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D7,D6,D5,D4,D3),Motor(D13,D12,D11,D10,D9)};
fusion_Motor mecanam[4] = {
    fusion_Motor(wheel[0],pid[0],motor[0]),//front right
    fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
    fusion_Motor(wheel[2],pid[2],motor[2]),//front left
    fusion_Motor(wheel[3],pid[3],motor[3])
};
PwmOut led(A0);
//back left
//Motor d(D5,D4,D3,D1,D0);
/*
void Delete()
{
    delete pid;
    delete wheel;
    delete motor;
}
fusion_Motor *mecanam1;
fusion_Motor *mecanam2;
fusion_Motor *mecanam3;
fusion_Motor *mecanam4;
void setup()
{
    pid = new PID(P,I,D,&Time);
    wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
    motor = new Motor(A1,A2,A3,A4,A0);
    mecanam1 = new fusion_Motor(*wheel,*pid,*motor);
    Delete();
    pid = new PID(P,I,D,&Time);
    wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
    motor = new Motor(D4,D3,D2,D1,D0);
    mecanam2 = new fusion_Motor(*wheel,*pid,*motor);
    Delete();
    pid = new PID(P,I,D,&Time);
    wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
    motor = new Motor(PD_2,PA_13,PA_14,PA_15,PC_9);
    mecanam3 = new fusion_Motor(*wheel,*pid,*motor);
    Delete();
    pid = new PID(P,I,D,&Time);
    wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
    motor = new Motor(PC_7,PB_6,PA_7,PA_6,PA_8);
    mecanam4 = new fusion_Motor(*wheel,*pid,*motor);
    Delete();
}
*/
/*
                |
             L\   /R
             -     -
             R/   \L
                |
*/
enum STATE {
    R,L,F,B
} state;
#define PI 3.141592653589793
float DUTY_fill[4]= {0};
float DUTY_inte[4]= {0};
float DUTY[4]= {0};
float DUTY_prev[4]= {0};
float GAIN_P=P,GAIN_I=I,GAIN_D=D;
void ACT(float tardata,float *data,fusion_Motor *fusion,float *fill,float *inte,float *prev_dat)
{
    float inter =  TIME;
    float dat = tardata-*data;
    *inte += (dat + *prev_dat)/2*inter;
    *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
    *fill = (dat - *prev_dat)*inter;
    *prev_dat  =dat;

    /*
    if(tardata>*data)
        *data+=0.01f;
    if(tardata<*data)
        *data-=0.01f;*/
    *fusion=*data;
}
void motruns(STATE RorL,float data)
{
    if(RorL == R) {

        ACT(data,&DUTY[R],&mecanam[R],&DUTY_fill[R],&DUTY_inte[R],&DUTY_prev[R]);
        ACT(data,&DUTY[R+2],&mecanam[R+2],&DUTY_fill[R+2],&DUTY_inte[R+2],&DUTY_prev[R+2]);
    } else if(RorL == L) {
        ACT(data,&DUTY[L],&mecanam[L],&DUTY_fill[L],&DUTY_inte[L],&DUTY_prev[L]);
        ACT(data,&DUTY[L+2],&mecanam[L+2],&DUTY_fill[L+2],&DUTY_inte[L+2],&DUTY_prev[L+2]);
    }
}
double mecCot(float angle,STATE RorL)
{
    double radian = (double)angle * PI / 180.0;
    return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4) : 1.4142135623730950488016887242097*cos(radian-PI/4);
}
bool stop_flag()
{
    bool flag=1;
    for(int i=0;i<4;i++)
    {
        flag &= (DUTY[i] <duty_max*0.8f && DUTY[i]>-duty_max*0.8f);
    }
    return flag;
}
void vector(float angle)
{
    angle-=90-360;
    if(angle<0)angle+=360;
    angle-=360;
    if(angle>0)angle-=360;
    angle*=-1;
    motruns(R,mecCot(angle,R));
    motruns(L,mecCot(angle,L));
    printf("%5.5f %5.5f %5.5f   ",angle , mecCot(angle,L),mecCot(angle,R));
}


void pid_setpoint()
{
    for(int i=0; i<4; i++)
        pid[i].dPoint=wheel[i].getRPM();
}
void pid_start()
{
    for(int i=0; i<4; i++)
        pid[i].start();
}
void pid_reset()
{
    for(int i=0; i<4; i++)
    {
     DUTY_fill[i]= 0;
     DUTY_inte[i]= 0;
     DUTY[i]= 0;
     DUTY_prev[i]= 0;
     }
}
void getData()
{

    data = device.getc();
    speed = data>>Mstate;
    sot = (data>>SorT)&0x01;
    Data2 = data&0x1f;
    //printf("%d%d%d%d%d%d%d%d  ",(data&0x80)>>7,(data&0x40)>>6,(data&0x20)>>5,(data&0x10)>>4,(data&0x08)>>3,(data&0x04)>>2,(data&0x02)>>1,(data&0x01));
    //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
    //printf("getdata!!\r\n");
}
int flag=0;
bool MotorState()
{
    for(int a=0; a<4; a++) {
        if(speed==0) {
            mecanam[a].run(Stop,1);
            pid_reset();
            //mecanam[a].pid_reset();
        } else if(speed==1) {
            if(stop_flag())
            {
                mecanam[a].run(Stop,1.0);
                pid_reset();
            }
            else 
            {                            
                motruns(R,0);
                motruns(L,0);
            }   
        } else if(speed==2) {
            duty_max=0.2f;
            mecanam[a].duty_max(duty_max);
            mecanam[a].duty_min(-duty_max);
        } else if(speed==3) {
            duty_max=1.0f;
            mecanam[a].duty_max(duty_max);
            mecanam[a].duty_min(-duty_max);
        }
    }
    return speed==0||speed==1;
}
float Y=1;
void f()
{
    static int i;
    float x[]= {-100,102,503,-4040};
    Y=x[i];
    //for(int x=0;x<4;x++)mecanam[x]=x[i];
    //printf("Target %f\r\n",x[i]);
    i++;
    if(i==3)i=0;
}
int main()
{
    Elec=0;
    for(int i=0; i<4; i++) {
        mecanam[i](fusion_Motor::Normal,1);
        mecanam[i].run(Free,1.0);
        mecanam[i].setbias(0.0);
        mecanam[i].setbias_motor(0.0);
    }
    for(int a=0; a<4; a++) {
        mecanam[a].duty_max(1.0);
        mecanam[a].duty_min(-1.0);
    }
    //M=0x1f;
    //wait(3);
    //setup();
    device.baud(230400);
    printf("start main\r\n");
    //device.attach(&getData);
    bool pidf=0;
    bool prev_sot=0;

    wait(1);
//    Ticker tic;
//    tic.attach(&f,3);
    //mecanam3=200;
    //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
    //for(int i = 0 ;i<4;i++)mecanam[i]=100;
    //wait(5);
    DigitalIn PIN(USER_BUTTON);
    if(PIN==0) {
        
        bool flag=0;
        float duty=1;
        while(1) {
            for(int i=0;i<4;i++)
            mecanam[i].run(Front,1);
            wait(4);
            for(int i=0;i<4;i++)
            mecanam[i].run(Front,0);
            wait(4);
            /*if(PIN==0&&flag==1) {
                flag=0;
                duty-=0.1;
            } else if(PIN==1&&flag==0) {
                flag=1;
                for(int i=0; i<4; i++)
                    mecanam[i].run(Front,duty);
            }*/
        }
    }
    flag=PIN;
    int prevang=0;
    Timer waittimer;
    while(1) {
        //Elec=!PIN;
        /*
        for(int x = 0; 1; x++) {
        vector(x);
            for(int i=0; i<4; i++)
                printf("%f   ",mecanam[i].getduty());
            printf("\r\n");
            }
            */
        //printf("start main\r\n");
        for(int x = 0; 1; x++) {
            if(PIN==0)GAIN_P+=0.001;
            //printf("start main\r\n");
            /*
            vector(Y);
                for(int i=0; i<4; i++)
                    printf("%f   ",DUTY[i]);
                printf("%F \r\n",GAIN_P);*/
            wait(TIME);
            //mecanam3=mecCot(i,R);
            //wait(0.5);
            //printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));

            getData();
            //wait(0.01);
            if(MotorState())break;
            //if(sot!=prev_sot);//pid_reset();

            if(sot==0) {
                prev_sot=sot;
                float angle = Data2*11.25;
                prevang=Data2;
                vector(angle);
            } else if(sot==1) {
                if(Data2==0) {
                    for(int i=0; i<2; i++) {
                        mecanam[i]=1;
                        mecanam[i+2]=-1;
                    }
                } else if(Data2==1) {
                    for(int i=0; i<2; i++) {
                        mecanam[i]=-1;
                        mecanam[i+2]=1;
                    }
                } else if(Data2==2){
                    Elec=1;
                } else if(Data2==3){
                    Elec=0;
                } else {
                    for(int i=0; i<4; i++)
                        mecanam[i].run(Stop,1);
                }
            }
        }
    }
}