Diff: main.cpp

Revision:

0:de53af3b0a0b

--- /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) {
+        
+    }
+}
+
+
+