MotorDrive制御
Dependencies: mbed Motor PID QEI2 fusion_Motor
Fork of Nucleo_Motor by
main.cpp
- Committer:
- kikoaac
- Date:
- 2015-09-23
- Revision:
- 0:8719ed91e6fd
- Child:
- 1:ad8acc6cfba7
File content as of revision 0:8719ed91e6fd:
/* 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); } } } } }