YY L
/
motor_driver_VPID
使用AB编码器进行双电机速度闭环
Revision 0:de53af3b0a0b, committed 2022-03-16
- Comitter:
- YYL5213
- Date:
- Wed Mar 16 02:49:08 2022 +0000
- Commit message:
- push
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Mar 16 02:49:08 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 16 02:49:08 2022 +0000 @@ -0,0 +1,122 @@ +#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) { + + } +} + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 16 02:49:08 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file