MotorDrive制御
Dependencies: mbed Motor PID QEI2 fusion_Motor
Fork of Nucleo_Motor by
Diff: main.cpp
- Revision:
- 1:ad8acc6cfba7
- Parent:
- 0:8719ed91e6fd
--- a/main.cpp Wed Sep 23 06:06:49 2015 +0000 +++ b/main.cpp Fri Oct 30 12:48:08 2015 +0000 @@ -11,7 +11,7 @@ #include "fusion_Motor.h" #include "Pin.h" #define Lx 0.0001 -#define Ku 0.32200//0.922000 +#define Ku 0.48000//0.922000 #define Pu 0.0125 #define Kp Ku*0.6 #define Ti Pu*0.5 @@ -27,6 +27,7 @@ #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); @@ -34,7 +35,7 @@ fusion_Motor mecanam3(i,p,d); */ - +float duty_max; Serial device(D8,D2); /* PID *pid; @@ -43,7 +44,7 @@ */ 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)}; +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 @@ -98,7 +99,6 @@ enum STATE { R,L,F,B } state; -PwmOut LED(LED1); #define PI 3.141592653589793 float DUTY_fill[4]= {0}; float DUTY_inte[4]= {0}; @@ -113,7 +113,7 @@ *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte; *fill = (dat - *prev_dat)*inter; *prev_dat =dat; - + /* if(tardata>*data) *data+=0.01f; @@ -135,7 +135,16 @@ 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); + 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) { @@ -163,7 +172,12 @@ void pid_reset() { for(int i=0; i<4; i++) - pid[i].pid_reset(); + { + DUTY_fill[i]= 0; + DUTY_inte[i]= 0; + DUTY[i]= 0; + DUTY_prev[i]= 0; + } } void getData() { @@ -176,20 +190,33 @@ //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(Free,1); + mecanam[a].run(Stop,1); + pid_reset(); + //mecanam[a].pid_reset(); } else if(speed==1) { - mecanam[a].run(Stop,1); - + if(stop_flag()) + { + mecanam[a].run(Stop,1.0); + pid_reset(); + } + else + { + motruns(R,0); + motruns(L,0); + } } else if(speed==2) { - mecanam[a].duty_max(0.3); - mecanam[a].duty_min(-0.3); + duty_max=0.2f; + mecanam[a].duty_max(duty_max); + mecanam[a].duty_min(-duty_max); } else if(speed==3) { - mecanam[a].duty_max(1.0); - mecanam[a].duty_min(-1.0); + duty_max=1.0f; + mecanam[a].duty_max(duty_max); + mecanam[a].duty_min(-duty_max); } } return speed==0||speed==1; @@ -207,14 +234,17 @@ } 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);} - led=0.5; + for(int a=0; a<4; a++) { + mecanam[a].duty_max(1.0); + mecanam[a].duty_min(-1.0); + } //M=0x1f; //wait(3); //setup(); @@ -225,28 +255,61 @@ bool prev_sot=0; wait(1); - Ticker tic; - tic.attach(&f,3); +// 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) { - //printf("start main\r\n"); - + //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);*/ + //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; @@ -255,6 +318,7 @@ if(sot==0) { prev_sot=sot; float angle = Data2*11.25; + prevang=Data2; vector(angle); } else if(sot==1) { if(Data2==0) { @@ -262,12 +326,15 @@ mecanam[i]=1; mecanam[i+2]=-1; } - } - else if(Data2==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); @@ -278,3 +345,4 @@ } +