YY L
使用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