YY L / Mbed 2 deprecated motor_driver_test_1

Dependencies:   mbed QEI

main.cpp

Committer:
YYL5213
Date:
2022-03-24
Revision:
0:ad5e5d970dd9
Child:
1:350bf501d1af

File content as of revision 0:ad5e5d970dd9:

#include "mbed.h"
#include "QEI.h"

Serial pc(SERIAL_TX, SERIAL_RX);//电脑串口通讯

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);//pwm速度
PwmOut M2PWM(PA_8);//pwm速度
DigitalIn M1Iln(PA_11);//方向1
DigitalIn M2Iln(PB_5);//方向2
AnalogIn MI1(PA_0);//读电流
AnalogIn MI2(PA_1);//读电流
AnalogIn adj(PA_3);//读电流

Ticker Velo;
int VA=0,sttA=0,stpA=0,VB=0,sttB=0,stpB=0;

float adj_point=0.0f;

void motor_run(int M,float pwr,int drc)
{
    if(M==1) {
        if(drc==0) {
            M1INA=1;
            M1INB=0;
        } else {
            M1INA=0;
            M1INB=1;
        }
        M1PWM.period_us(100);
        M1PWM.write(pwr);}
    else{
        if(drc==0) {
            M2INA=1;
            M2INB=0;
        } else {
            M2INA=0;
            M2INB=1;
        }
        M2PWM.period_us(100);
        M2PWM.write(pwr);
    }
        
}
void Read_Velocity(){
     stpA=EnA.getPulses();
     stpB=EnB.getPulses();
     VA=stpA-sttA;
     VB=stpB-sttB;
     sttA= stpA;
     sttB= stpB;
     pc.printf("%d  %d   %.3f\n",VA,VB,adj_point);
    }
int main()
{   Velo.attach(&Read_Velocity, 0.01);
    pc.baud(115200);
    pc.printf("PC connected!\n");
    while(1){
    adj_point=adj.read();
    motor_run(1,adj_point,0);
    motor_run(2,adj_point,0);
    wait_ms(50);
    }
}