YY L / Mbed 2 deprecated motor_driver_VPID

使用AB编码器进行双电机速度闭环

Dependencies:   mbed QEI

main.cpp@0:de53af3b0a0b, 2022-03-16 (annotated)

Committer:
YYL5213
Date:
Wed Mar 16 02:49:08 2022 +0000
Revision:
0:de53af3b0a0b
push

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YYL5213 0:de53af3b0a0b 1 #include "mbed.h"
YYL5213 0:de53af3b0a0b 2 #include "QEI.h"
YYL5213 0:de53af3b0a0b 3
YYL5213 0:de53af3b0a0b 4 Serial pc(SERIAL_TX, SERIAL_RX);//电脑串口通讯
YYL5213 0:de53af3b0a0b 5 //Serial core(PA_9, PA_10);//电脑串口通讯
YYL5213 0:de53af3b0a0b 6
YYL5213 0:de53af3b0a0b 7 QEI EnA(PB_3,PA_4,NC,20);//编码器
YYL5213 0:de53af3b0a0b 8 QEI EnB(PA_5,PA_6,NC,20);//编码器
YYL5213 0:de53af3b0a0b 9 DigitalOut M1INA(PA_12);//方向1
YYL5213 0:de53af3b0a0b 10 DigitalOut M1INB(PB_7);//方向2
YYL5213 0:de53af3b0a0b 11 DigitalOut M2INA(PB_0);//方向1
YYL5213 0:de53af3b0a0b 12 DigitalOut M2INB(PB_6);//方向2
YYL5213 0:de53af3b0a0b 13 PwmOut M1PWM(PB_1);//M1pwm速度
YYL5213 0:de53af3b0a0b 14 PwmOut M2PWM(PA_8);//M2pwm速度
YYL5213 0:de53af3b0a0b 15 DigitalIn M1Iln(PA_11);//M1限流指示
YYL5213 0:de53af3b0a0b 16 DigitalIn M2Iln(PB_5);//M2限流指示
YYL5213 0:de53af3b0a0b 17 AnalogIn MI1(PA_0);//读电流M1
YYL5213 0:de53af3b0a0b 18 AnalogIn MI2(PA_1);//读电流M2
YYL5213 0:de53af3b0a0b 19 AnalogIn adj(PA_7);//读设定速度
YYL5213 0:de53af3b0a0b 20
YYL5213 0:de53af3b0a0b 21 Ticker Velo;
YYL5213 0:de53af3b0a0b 22 Ticker PID;
YYL5213 0:de53af3b0a0b 23 Timer timer;
YYL5213 0:de53af3b0a0b 24 float adj_point=0.0f;
YYL5213 0:de53af3b0a0b 25
YYL5213 0:de53af3b0a0b 26 int VA=0,sttA=0,stpA=0,VB=0,sttB=0,stpB=0;
YYL5213 0:de53af3b0a0b 27 float errorA = 0.0f, lasterrorA = 0.0f, errorsumA=0.0f, powerA=0.0f,powerAA=0.0f;
YYL5213 0:de53af3b0a0b 28 float errorB = 0.0f, lasterrorB = 0.0f, errorsumB=0.0f, powerB=0.0f,powerBB=0.0f;
YYL5213 0:de53af3b0a0b 29
YYL5213 0:de53af3b0a0b 30 float KpA = 0.25f;
YYL5213 0:de53af3b0a0b 31 float KiA = 0.00004f;
YYL5213 0:de53af3b0a0b 32 float KdA = 1.3f;
YYL5213 0:de53af3b0a0b 33 float KpB = 0.25f;
YYL5213 0:de53af3b0a0b 34 float KiB = 0.00004f;
YYL5213 0:de53af3b0a0b 35 float KdB = 1.3f;
YYL5213 0:de53af3b0a0b 36
YYL5213 0:de53af3b0a0b 37 float maxspeedA=1000.0f,maxspeedB=1000.0f;
YYL5213 0:de53af3b0a0b 38 float setspeed=0.0f;
YYL5213 0:de53af3b0a0b 39
YYL5213 0:de53af3b0a0b 40 void motor_run(int M,float pwr)
YYL5213 0:de53af3b0a0b 41 {
YYL5213 0:de53af3b0a0b 42 if(M==1) {
YYL5213 0:de53af3b0a0b 43 if(pwr>0.0f) {
YYL5213 0:de53af3b0a0b 44 M1INA=1;
YYL5213 0:de53af3b0a0b 45 M1INB=0;
YYL5213 0:de53af3b0a0b 46 } else {
YYL5213 0:de53af3b0a0b 47 M1INA=0;
YYL5213 0:de53af3b0a0b 48 M1INB=1;
YYL5213 0:de53af3b0a0b 49 }
YYL5213 0:de53af3b0a0b 50 M1PWM.period_us(50);
YYL5213 0:de53af3b0a0b 51 M1PWM.write(abs(pwr));
YYL5213 0:de53af3b0a0b 52 } else {
YYL5213 0:de53af3b0a0b 53 if(pwr>0.0f) {
YYL5213 0:de53af3b0a0b 54 M2INA=1;
YYL5213 0:de53af3b0a0b 55 M2INB=0;
YYL5213 0:de53af3b0a0b 56 } else {
YYL5213 0:de53af3b0a0b 57 M2INA=0;
YYL5213 0:de53af3b0a0b 58 M2INB=1;
YYL5213 0:de53af3b0a0b 59 }
YYL5213 0:de53af3b0a0b 60 M2PWM.period_us(50);
YYL5213 0:de53af3b0a0b 61 M2PWM.write(abs(pwr));
YYL5213 0:de53af3b0a0b 62 }
YYL5213 0:de53af3b0a0b 63
YYL5213 0:de53af3b0a0b 64 }
YYL5213 0:de53af3b0a0b 65 float limit(float power)
YYL5213 0:de53af3b0a0b 66 {
YYL5213 0:de53af3b0a0b 67 if(power<-1.0f)
YYL5213 0:de53af3b0a0b 68 power=-1.0f;
YYL5213 0:de53af3b0a0b 69 if(power>1.0f)
YYL5213 0:de53af3b0a0b 70 power=1.0f;
YYL5213 0:de53af3b0a0b 71 return power;
YYL5213 0:de53af3b0a0b 72 }
YYL5213 0:de53af3b0a0b 73 float dead_off(float power,float dead)
YYL5213 0:de53af3b0a0b 74 { if(power>0.0f)
YYL5213 0:de53af3b0a0b 75 power=power*(1.0f-dead)+dead;
YYL5213 0:de53af3b0a0b 76 if(power<0.0f)
YYL5213 0:de53af3b0a0b 77 power=power*(1.0f-dead)-dead;
YYL5213 0:de53af3b0a0b 78 return power;
YYL5213 0:de53af3b0a0b 79 }
YYL5213 0:de53af3b0a0b 80 void Read_Velocity()
YYL5213 0:de53af3b0a0b 81 {
YYL5213 0:de53af3b0a0b 82 stpA=EnA.getPulses();
YYL5213 0:de53af3b0a0b 83 stpB=EnB.getPulses();
YYL5213 0:de53af3b0a0b 84 VA=stpA-sttA;
YYL5213 0:de53af3b0a0b 85 VB=stpB-sttB;
YYL5213 0:de53af3b0a0b 86 sttA= stpA;
YYL5213 0:de53af3b0a0b 87 sttB= stpB;
YYL5213 0:de53af3b0a0b 88 adj_point=(adj.read()-0.5f)*2.0f;
YYL5213 0:de53af3b0a0b 89 setspeed=adj_point;
YYL5213 0:de53af3b0a0b 90 errorA = setspeed - float(VA)/maxspeedA;
YYL5213 0:de53af3b0a0b 91 errorB = setspeed - float(VB)/maxspeedB;
YYL5213 0:de53af3b0a0b 92 errorsumA += errorA;
YYL5213 0:de53af3b0a0b 93 errorsumB += errorB;
YYL5213 0:de53af3b0a0b 94 powerA = powerA + KpA * errorA + KiA * errorsumA + KdA * (errorA - lasterrorA);
YYL5213 0:de53af3b0a0b 95 powerB = powerB + KpB * errorB + KiB * errorsumB + KdB * (errorB - lasterrorB);
YYL5213 0:de53af3b0a0b 96 lasterrorA=errorA;
YYL5213 0:de53af3b0a0b 97 lasterrorB=errorB;
YYL5213 0:de53af3b0a0b 98 powerA=limit(powerA);
YYL5213 0:de53af3b0a0b 99 powerB=limit(powerB);
YYL5213 0:de53af3b0a0b 100 powerAA=dead_off(powerA,0.4f);
YYL5213 0:de53af3b0a0b 101 powerBB=dead_off(powerB,0.4f);
YYL5213 0:de53af3b0a0b 102 motor_run(1,powerAA);
YYL5213 0:de53af3b0a0b 103 motor_run(2,powerBB);
YYL5213 0:de53af3b0a0b 104 //pc.printf("%.2f %.2f %.2f\n",setspeed,powerAA,powerBB);
YYL5213 0:de53af3b0a0b 105 //pc.printf("%.1f %.1f\n",errorA*100.0f,errorB*100.0f);
YYL5213 0:de53af3b0a0b 106 pc.printf("%d %d %d\n",VA,VB,int(setspeed*maxspeedA));
YYL5213 0:de53af3b0a0b 107 }
YYL5213 0:de53af3b0a0b 108 int main()
YYL5213 0:de53af3b0a0b 109
YYL5213 0:de53af3b0a0b 110 {
YYL5213 0:de53af3b0a0b 111 Velo.attach(&Read_Velocity, 0.01);
YYL5213 0:de53af3b0a0b 112 pc.baud(2000000);
YYL5213 0:de53af3b0a0b 113 //core.baud(2000000);
YYL5213 0:de53af3b0a0b 114 pc.printf("PC connected!\n");
YYL5213 0:de53af3b0a0b 115 //core.printf("PC connected!\n");
YYL5213 0:de53af3b0a0b 116 while(1) {
YYL5213 0:de53af3b0a0b 117
YYL5213 0:de53af3b0a0b 118 }
YYL5213 0:de53af3b0a0b 119 }
YYL5213 0:de53af3b0a0b 120
YYL5213 0:de53af3b0a0b 121
YYL5213 0:de53af3b0a0b 122