Diff: main.cpp

Revision:

0:8719ed91e6fd

Child:

1:ad8acc6cfba7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 23 06:06:49 2015 +0000
@@ -0,0 +1,280 @@
+/*
+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.32200//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;
+/*
+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);
+*/
+
+
+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(D5,D4,D3,PB_8,PB_9),Motor(D12,D11,D10,D9,D7)};
+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;
+PwmOut LED(LED1);
+#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-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
+}
+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++)
+        pid[i].pid_reset();
+}
+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");
+}
+bool MotorState()
+{
+    for(int a=0; a<4; a++) {
+        if(speed==0) {
+            mecanam[a].run(Free,1);
+        } else if(speed==1) {
+            mecanam[a].run(Stop,1);
+
+        } else if(speed==2) {
+            mecanam[a].duty_max(0.3);
+            mecanam[a].duty_min(-0.3);
+        } else if(speed==3) {
+            mecanam[a].duty_max(1.0);
+            mecanam[a].duty_min(-1.0);
+        }
+    }
+    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()
+{
+    for(int i=0; i<4; i++) {
+        mecanam[i](fusion_Motor::Normal,1);
+        mecanam[i].run(Free,1.0);
+    }
+for(int a=0;a<4;a++){
+    mecanam[a].duty_max(1.0);
+    mecanam[a].duty_min(-1.0);}
+    led=0.5;
+    //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);
+    while(1) {
+       //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;
+                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 {
+                    for(int i=0; i<4; i++)
+                        mecanam[i].run(Stop,1);
+                }
+            }
+        }
+    }
+}
+
+