main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 
00004 Serial pc(SERIAL_TX, SERIAL_RX);//电脑串口通讯
00005 //Serial core(PA_9, PA_10);//电脑串口通讯
00006 
00007 QEI EnA(PB_3,PA_4,NC,20);//编码器
00008 QEI EnB(PA_5,PA_6,NC,20);//编码器
00009 DigitalOut M1INA(PA_12);//方向1
00010 DigitalOut M1INB(PB_7);//方向2
00011 DigitalOut M2INA(PB_0);//方向1
00012 DigitalOut M2INB(PB_6);//方向2
00013 PwmOut M1PWM(PB_1);//M1pwm速度
00014 PwmOut M2PWM(PA_8);//M2pwm速度
00015 DigitalIn M1Iln(PA_11);//M1限流指示
00016 DigitalIn M2Iln(PB_5);//M2限流指示
00017 AnalogIn MI1(PA_0);//读电流M1
00018 AnalogIn MI2(PA_1);//读电流M2
00019 AnalogIn adj(PA_7);//读设定速度
00020 
00021 Ticker Velo;
00022 Ticker PID;
00023 Timer timer;
00024 float adj_point=0.0f;
00025 
00026 int VA=0,sttA=0,stpA=0,VB=0,sttB=0,stpB=0;
00027 float errorA = 0.0f, lasterrorA = 0.0f, errorsumA=0.0f, powerA=0.0f,powerAA=0.0f;
00028 float errorB = 0.0f, lasterrorB = 0.0f, errorsumB=0.0f, powerB=0.0f,powerBB=0.0f;
00029 
00030 float KpA = 0.25f;
00031 float KiA = 0.00004f;
00032 float KdA = 1.3f;
00033 float KpB = 0.25f;
00034 float KiB = 0.00004f;
00035 float KdB = 1.3f;
00036 
00037 float maxspeedA=1000.0f,maxspeedB=1000.0f;
00038 float setspeed=0.0f;
00039 
00040 void motor_run(int M,float pwr)
00041 {
00042     if(M==1) {
00043         if(pwr>0.0f) {
00044             M1INA=1;
00045             M1INB=0;
00046         } else {
00047             M1INA=0;
00048             M1INB=1;
00049         }
00050         M1PWM.period_us(50);
00051         M1PWM.write(abs(pwr));
00052     } else {
00053         if(pwr>0.0f) {
00054             M2INA=1;
00055             M2INB=0;
00056         } else {
00057             M2INA=0;
00058             M2INB=1;
00059         }
00060         M2PWM.period_us(50);
00061         M2PWM.write(abs(pwr));
00062     }
00063 
00064 }
00065 float limit(float power)
00066 {
00067     if(power<-1.0f)
00068         power=-1.0f;
00069     if(power>1.0f)
00070         power=1.0f;
00071     return power;
00072 }
00073 float dead_off(float power,float dead)
00074 {   if(power>0.0f)
00075     power=power*(1.0f-dead)+dead;
00076     if(power<0.0f)
00077     power=power*(1.0f-dead)-dead;
00078     return power;
00079 }
00080 void Read_Velocity()
00081 { 
00082     stpA=EnA.getPulses();
00083     stpB=EnB.getPulses();
00084     VA=stpA-sttA;
00085     VB=stpB-sttB;
00086     sttA= stpA;
00087     sttB= stpB;
00088     adj_point=(adj.read()-0.5f)*2.0f;
00089     setspeed=adj_point;
00090     errorA = setspeed - float(VA)/maxspeedA;
00091     errorB = setspeed - float(VB)/maxspeedB;
00092     errorsumA += errorA;
00093     errorsumB += errorB;
00094     powerA = powerA + KpA * errorA + KiA * errorsumA + KdA * (errorA - lasterrorA);
00095     powerB = powerB + KpB * errorB + KiB * errorsumB + KdB * (errorB - lasterrorB);
00096     lasterrorA=errorA;
00097     lasterrorB=errorB;
00098     powerA=limit(powerA);
00099     powerB=limit(powerB);
00100     powerAA=dead_off(powerA,0.4f);
00101     powerBB=dead_off(powerB,0.4f);
00102     motor_run(1,powerAA);
00103     motor_run(2,powerBB);
00104     //pc.printf("%.2f %.2f %.2f\n",setspeed,powerAA,powerBB);
00105     //pc.printf("%.1f %.1f\n",errorA*100.0f,errorB*100.0f);
00106     pc.printf("%d %d %d\n",VA,VB,int(setspeed*maxspeedA));
00107 }
00108 int main()
00109 
00110 {
00111     Velo.attach(&Read_Velocity, 0.01);
00112     pc.baud(2000000);
00113     //core.baud(2000000);
00114     pc.printf("PC connected!\n");
00115     //core.printf("PC connected!\n");
00116     while(1) {
00117         
00118     }
00119 }
00120 
00121 
00122