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