main.cpp

00001 /*
00002 motor 555
00003 Lipo 4sel
00004 Linear accelerator
00005 Not mount QEI
00006 */
00007 #include "mbed.h"
00008 #include "Motor.h"
00009 #include "PID.h"
00010 #include "QEI.h"
00011 #include "fusion_Motor.h"
00012 #include "Pin.h"
00013 #define Lx 0.0001
00014 #define Ku 0.32200//0.922000
00015 #define Pu 0.0125
00016 #define Kp Ku*0.6
00017 #define Ti Pu*0.5
00018 #define Td Pu*0.125
00019 
00020 
00021 char data=0,speed=0,sot=0,Data2=0;
00022 #define P Kp
00023 #define I Kp/Ti
00024 #define D Kp*Td
00025 #define Mstate 6
00026 #define SorT 5
00027 #define Databit 0
00028 #define TIME 0.0125
00029 Timer Time;
00030 /*
00031 PID p(P,I,D,&Time);
00032 QEI i(QEI1_A,QEI1_B,NC,50,&Time);
00033 Motor d(PD_2,PA_13,PA_14,PA_15,PC_9);
00034 fusion_Motor mecanam3(i,p,d);
00035 */
00036 
00037 
00038 Serial device(D8,D2);
00039 /*
00040 PID *pid;
00041 QEI *wheel;
00042 Motor *motor;
00043 */
00044 PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
00045 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)};
00046 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)};
00047 fusion_Motor mecanam[4] = {
00048     fusion_Motor(wheel[0],pid[0],motor[0]),//front right
00049     fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
00050     fusion_Motor(wheel[2],pid[2],motor[2]),//front left
00051     fusion_Motor(wheel[3],pid[3],motor[3])
00052 };
00053 PwmOut led(A0);
00054 //back left
00055 //Motor d(D5,D4,D3,D1,D0);
00056 /*
00057 void Delete()
00058 {
00059     delete pid;
00060     delete wheel;
00061     delete motor;
00062 }
00063 fusion_Motor *mecanam1;
00064 fusion_Motor *mecanam2;
00065 fusion_Motor *mecanam3;
00066 fusion_Motor *mecanam4;
00067 void setup()
00068 {
00069     pid = new PID(P,I,D,&Time);
00070     wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
00071     motor = new Motor(A1,A2,A3,A4,A0);
00072     mecanam1 = new fusion_Motor(*wheel,*pid,*motor);
00073     Delete();
00074     pid = new PID(P,I,D,&Time);
00075     wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
00076     motor = new Motor(D4,D3,D2,D1,D0);
00077     mecanam2 = new fusion_Motor(*wheel,*pid,*motor);
00078     Delete();
00079     pid = new PID(P,I,D,&Time);
00080     wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
00081     motor = new Motor(PD_2,PA_13,PA_14,PA_15,PC_9);
00082     mecanam3 = new fusion_Motor(*wheel,*pid,*motor);
00083     Delete();
00084     pid = new PID(P,I,D,&Time);
00085     wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
00086     motor = new Motor(PC_7,PB_6,PA_7,PA_6,PA_8);
00087     mecanam4 = new fusion_Motor(*wheel,*pid,*motor);
00088     Delete();
00089 }
00090 */
00091 /*
00092                 |
00093              L\   /R
00094              -     -
00095              R/   \L
00096                 |
00097 */
00098 enum STATE {
00099     R,L,F,B
00100 } state;
00101 PwmOut LED(LED1);
00102 #define PI 3.141592653589793
00103 float DUTY_fill[4]= {0};
00104 float DUTY_inte[4]= {0};
00105 float DUTY[4]= {0};
00106 float DUTY_prev[4]= {0};
00107 float GAIN_P=P,GAIN_I=I,GAIN_D=D;
00108 void ACT(float tardata,float *data,fusion_Motor *fusion,float *fill,float *inte,float *prev_dat)
00109 {
00110     float inter =  TIME;
00111     float dat = tardata-*data;
00112     *inte += (dat + *prev_dat)/2*inter;
00113     *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
00114     *fill = (dat - *prev_dat)*inter;
00115     *prev_dat  =dat;
00116     
00117     /*
00118     if(tardata>*data)
00119         *data+=0.01f;
00120     if(tardata<*data)
00121         *data-=0.01f;*/
00122     *fusion=*data;
00123 }
00124 void motruns(STATE RorL,float data)
00125 {
00126     if(RorL == R) {
00127 
00128         ACT(data,&DUTY[R],&mecanam[R],&DUTY_fill[R],&DUTY_inte[R],&DUTY_prev[R]);
00129         ACT(data,&DUTY[R+2],&mecanam[R+2],&DUTY_fill[R+2],&DUTY_inte[R+2],&DUTY_prev[R+2]);
00130     } else if(RorL == L) {
00131         ACT(data,&DUTY[L],&mecanam[L],&DUTY_fill[L],&DUTY_inte[L],&DUTY_prev[L]);
00132         ACT(data,&DUTY[L+2],&mecanam[L+2],&DUTY_fill[L+2],&DUTY_inte[L+2],&DUTY_prev[L+2]);
00133     }
00134 }
00135 double mecCot(float angle,STATE RorL)
00136 {
00137     double radian = (double)angle * PI / 180.0;
00138     return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
00139 }
00140 void vector(float angle)
00141 {
00142     angle-=90-360;
00143     if(angle<0)angle+=360;
00144     angle-=360;
00145     if(angle>0)angle-=360;
00146     angle*=-1;
00147     motruns(R,mecCot(angle,R));
00148     motruns(L,mecCot(angle,L));
00149     printf("%5.5f %5.5f %5.5f   ",angle , mecCot(angle,L),mecCot(angle,R));
00150 }
00151 
00152 
00153 void pid_setpoint()
00154 {
00155     for(int i=0; i<4; i++)
00156         pid[i].dPoint=wheel[i].getRPM();
00157 }
00158 void pid_start()
00159 {
00160     for(int i=0; i<4; i++)
00161         pid[i].start();
00162 }
00163 void pid_reset()
00164 {
00165     for(int i=0; i<4; i++)
00166         pid[i].pid_reset();
00167 }
00168 void getData()
00169 {
00170 
00171     data = device.getc();
00172     speed = data>>Mstate;
00173     sot = (data>>SorT)&0x01;
00174     Data2 = data&0x1f;
00175     //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));
00176     //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
00177     //printf("getdata!!\r\n");
00178 }
00179 bool MotorState()
00180 {
00181     for(int a=0; a<4; a++) {
00182         if(speed==0) {
00183             mecanam[a].run(Free,1);
00184         } else if(speed==1) {
00185             mecanam[a].run(Stop,1);
00186 
00187         } else if(speed==2) {
00188             mecanam[a].duty_max(0.3);
00189             mecanam[a].duty_min(-0.3);
00190         } else if(speed==3) {
00191             mecanam[a].duty_max(1.0);
00192             mecanam[a].duty_min(-1.0);
00193         }
00194     }
00195     return speed==0||speed==1;
00196 }
00197 float Y=1;
00198 void f()
00199 {
00200     static int i;
00201     float x[]= {-100,102,503,-4040};
00202     Y=x[i];
00203     //for(int x=0;x<4;x++)mecanam[x]=x[i];
00204     //printf("Target %f\r\n",x[i]);
00205     i++;
00206     if(i==3)i=0;
00207 }
00208 int main()
00209 {
00210     for(int i=0; i<4; i++) {
00211         mecanam[i](fusion_Motor::Normal,1);
00212         mecanam[i].run(Free,1.0);
00213     }
00214 for(int a=0;a<4;a++){
00215     mecanam[a].duty_max(1.0);
00216     mecanam[a].duty_min(-1.0);}
00217     led=0.5;
00218     //M=0x1f;
00219     //wait(3);
00220     //setup();
00221     device.baud(230400);
00222     printf("start main\r\n");
00223     //device.attach(&getData);
00224     bool pidf=0;
00225     bool prev_sot=0;
00226 
00227     wait(1);
00228     Ticker tic;
00229     tic.attach(&f,3);
00230     //mecanam3=200;
00231     //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
00232     //for(int i = 0 ;i<4;i++)mecanam[i]=100;
00233     //wait(5);
00234     DigitalIn PIN(USER_BUTTON);
00235     while(1) {
00236        //printf("start main\r\n");
00237      
00238         for(int x = 0; 1; x++) {
00239             if(PIN==0)GAIN_P+=0.001;
00240         //printf("start main\r\n");
00241         /*vector(Y);
00242             for(int i=0; i<4; i++)
00243                 printf("%f   ",DUTY[i]);
00244             printf("%F \r\n",GAIN_P);*/
00245             wait(TIME);
00246             //mecanam3=mecCot(i,R);
00247             //wait(0.5);
00248             //printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));
00249             
00250             getData();
00251             //wait(0.01);
00252             if(MotorState())break;
00253             //if(sot!=prev_sot);//pid_reset();
00254 
00255             if(sot==0) {
00256                 prev_sot=sot;
00257                 float angle = Data2*11.25;
00258                 vector(angle);
00259             } else if(sot==1) {
00260                 if(Data2==0) {
00261                     for(int i=0; i<2; i++) {
00262                         mecanam[i]=1;
00263                         mecanam[i+2]=-1;
00264                     }
00265                 }
00266                 else if(Data2==1) {
00267                     for(int i=0; i<2; i++) {
00268                         mecanam[i]=-1;
00269                         mecanam[i+2]=1;
00270                     }
00271                 } else {
00272                     for(int i=0; i<4; i++)
00273                         mecanam[i].run(Stop,1);
00274                 }
00275             }
00276         }
00277     }
00278 }
00279 
00280