YY L
/
motor_driver_VPID
使用AB编码器进行双电机速度闭环
main.cpp
- Committer:
- YYL5213
- Date:
- 2022-03-16
- Revision:
- 0:de53af3b0a0b
File content as of revision 0:de53af3b0a0b:
#include "mbed.h" #include "QEI.h" Serial pc(SERIAL_TX, SERIAL_RX);//电脑串口通讯 //Serial core(PA_9, PA_10);//电脑串口通讯 QEI EnA(PB_3,PA_4,NC,20);//编码器 QEI EnB(PA_5,PA_6,NC,20);//编码器 DigitalOut M1INA(PA_12);//方向1 DigitalOut M1INB(PB_7);//方向2 DigitalOut M2INA(PB_0);//方向1 DigitalOut M2INB(PB_6);//方向2 PwmOut M1PWM(PB_1);//M1pwm速度 PwmOut M2PWM(PA_8);//M2pwm速度 DigitalIn M1Iln(PA_11);//M1限流指示 DigitalIn M2Iln(PB_5);//M2限流指示 AnalogIn MI1(PA_0);//读电流M1 AnalogIn MI2(PA_1);//读电流M2 AnalogIn adj(PA_7);//读设定速度 Ticker Velo; Ticker PID; Timer timer; float adj_point=0.0f; int VA=0,sttA=0,stpA=0,VB=0,sttB=0,stpB=0; float errorA = 0.0f, lasterrorA = 0.0f, errorsumA=0.0f, powerA=0.0f,powerAA=0.0f; float errorB = 0.0f, lasterrorB = 0.0f, errorsumB=0.0f, powerB=0.0f,powerBB=0.0f; float KpA = 0.25f; float KiA = 0.00004f; float KdA = 1.3f; float KpB = 0.25f; float KiB = 0.00004f; float KdB = 1.3f; float maxspeedA=1000.0f,maxspeedB=1000.0f; float setspeed=0.0f; void motor_run(int M,float pwr) { if(M==1) { if(pwr>0.0f) { M1INA=1; M1INB=0; } else { M1INA=0; M1INB=1; } M1PWM.period_us(50); M1PWM.write(abs(pwr)); } else { if(pwr>0.0f) { M2INA=1; M2INB=0; } else { M2INA=0; M2INB=1; } M2PWM.period_us(50); M2PWM.write(abs(pwr)); } } float limit(float power) { if(power<-1.0f) power=-1.0f; if(power>1.0f) power=1.0f; return power; } float dead_off(float power,float dead) { if(power>0.0f) power=power*(1.0f-dead)+dead; if(power<0.0f) power=power*(1.0f-dead)-dead; return power; } void Read_Velocity() { stpA=EnA.getPulses(); stpB=EnB.getPulses(); VA=stpA-sttA; VB=stpB-sttB; sttA= stpA; sttB= stpB; adj_point=(adj.read()-0.5f)*2.0f; setspeed=adj_point; errorA = setspeed - float(VA)/maxspeedA; errorB = setspeed - float(VB)/maxspeedB; errorsumA += errorA; errorsumB += errorB; powerA = powerA + KpA * errorA + KiA * errorsumA + KdA * (errorA - lasterrorA); powerB = powerB + KpB * errorB + KiB * errorsumB + KdB * (errorB - lasterrorB); lasterrorA=errorA; lasterrorB=errorB; powerA=limit(powerA); powerB=limit(powerB); powerAA=dead_off(powerA,0.4f); powerBB=dead_off(powerB,0.4f); motor_run(1,powerAA); motor_run(2,powerBB); //pc.printf("%.2f %.2f %.2f\n",setspeed,powerAA,powerBB); //pc.printf("%.1f %.1f\n",errorA*100.0f,errorB*100.0f); pc.printf("%d %d %d\n",VA,VB,int(setspeed*maxspeedA)); } int main() { Velo.attach(&Read_Velocity, 0.01); pc.baud(2000000); //core.baud(2000000); pc.printf("PC connected!\n"); //core.printf("PC connected!\n"); while(1) { } }