File content as of revision 3:7fe433d24ff7:

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

class MotorWithEncoder {
private:
    PwmOut *pwm;
    DigitalOut *dir;
    float input_voltage;
    // gear
    const float GEAR_RATIO;
    // encoder
    QEI *enc;
    int enc_pulse_rate;  // per control period
    // controller
    const int CONTROL_PERIOD;  // control period (ms)
    bool controller_enable;
    int des_enc_pulse_rate;  // per control period
    float Kp, Ki;  // gains
    int error;  // error
    int acc_error;  // accumulated error;

    EventQueue *queue;
public:
    MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio, 
        PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi, 
        int _control_period, float _Kp, float _Ki);
    // _pinPWM: pin outputs PWM
    // _pinDir: pin outputs LOW or HIGH for changing the input voltage direction
    // _pinA, _pinB: pins receive pulses from encoder
    // _res: resolution
    // _multi: QEI::X2_ENCODING or QEI::X4_ENCODING
    // _control_period: ms
    // _Kp, _Ki: gains
    
    float getInputVoltage();
    void updateInputVoltage();
    void applyInputVoltage();  // callback
    
    void resetEnc();
    void updateEncPulseRate();  // callback
    int getEncPulseRate();
    
    bool getControllerEnable();
    void switchControllerEnable(bool _switch);
    int getControlPeriod();
    int getDesEncPulseRate();
    void setDesEncPulseRate(int _des_enc_pulse_rate);
    float getKp();
    void setKp(float _Kp);
    float getKi();
    void setKi(float _Ki);
};

MotorWithEncoder::MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio, 
    PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi, 
    int _control_period, float _Kp, float _Ki) 
    : GEAR_RATIO(_gear_ratio), CONTROL_PERIOD(_control_period) {
    pwm = new PwmOut(_pinPWM);
    pwm->period(0.0001);  // 0.1 ms
    dir = new DigitalOut(_pinDir);
    enc = new QEI(_pinA, _pinB, NC, _res, _multi);
    controller_enable = false;
    des_enc_pulse_rate = 0;
    Kp = _Kp;
    Ki = _Ki;
}

float MotorWithEncoder::getInputVoltage() {
    return input_voltage;
}
    
void MotorWithEncoder::updateInputVoltage() {
    float temp_input_voltage;
    
    error = des_enc_pulse_rate - enc_pulse_rate;
    acc_error += error;
    
    temp_input_voltage =  (float)(Kp * error + Ki * acc_error);
    
    if (temp_input_voltage > 1.0) {
        input_voltage = 1.0;
        acc_error -= error;
    }
    else if (temp_input_voltage < - 1.0) {
        input_voltage = - 1.0;
        acc_error -= error;
    }
    else {
        input_voltage = temp_input_voltage;
    }
}

void MotorWithEncoder::applyInputVoltage() {
    if (controller_enable) {
        if (input_voltage < 0) *dir = 0;  // CW
        else *dir = 1;                    // CCW
        *pwm = abs(input_voltage);
    }
    else {
        *pwm = 0;
    }
}

void MotorWithEncoder::resetEnc() {
    enc->reset();
}

void MotorWithEncoder::updateEncPulseRate() {
    enc_pulse_rate = enc->getPulses();
    enc->reset();
}

int MotorWithEncoder::getEncPulseRate() {
    return enc_pulse_rate;
}

bool MotorWithEncoder::getControllerEnable() {
    return controller_enable;
}

void MotorWithEncoder::switchControllerEnable(bool _switch) {
    error = 0.0;
    acc_error = 0.0;
    input_voltage = 0.0;
    controller_enable = _switch;
}

int MotorWithEncoder::getControlPeriod() {
    return CONTROL_PERIOD;
}

int MotorWithEncoder::getDesEncPulseRate() {
    return des_enc_pulse_rate;
}

void MotorWithEncoder::setDesEncPulseRate(int _des_enc_pulse_rate) {
    des_enc_pulse_rate = _des_enc_pulse_rate;
}

float MotorWithEncoder::getKp() {
    return Kp;
}

void MotorWithEncoder::setKp(float _Kp) {
    Kp = _Kp;
}

float MotorWithEncoder::getKi() {
    return Ki;
}

void MotorWithEncoder::setKi(float _Ki) {
    Ki = _Ki;
}

// interface to pc
Serial pc(USBTX, USBRX);

// motor1
MotorWithEncoder motor0(p21, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
MotorWithEncoder motor1(p22, p28, 50, p7, p8, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
// gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001

// key input from pc (callback)
void key_input_pc() {
    if (pc.readable() == 1) {
        switch(pc.getc()) {
            case 'z':
                motor0.switchControllerEnable(true);
                break;
            case 'q':
                motor0.switchControllerEnable(false);
                break;
            case 'a':
                motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() + 10);
                break;
            case 's':
                motor0.setDesEncPulseRate(0);
                break;
            case 'd':
                motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() - 10);
                break;
            case 'v':
                motor1.switchControllerEnable(true);
                break;
            case 'r':
                motor1.switchControllerEnable(false);
                break;
            case 'f':
                motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() + 10);
                break;
            case 'g':
                motor1.setDesEncPulseRate(0);
                break;
            case 'h':
                motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() - 10);
                break;
        }
    }
}

// encoder output to pc
void enc_output_pc() {
    if (motor0.getControllerEnable()) {
        pc.printf("ON0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
    }
    else {
        pc.printf("OFF0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
    }
    if (motor1.getControllerEnable()) {
        pc.printf("ON1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
    }
    else {
        pc.printf("OFF1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
    }
}

// event queue
EventQueue queue;

// callback per control period
// update motor voltage
// update enc_pulse_ratge
void callbackControlPeriod0() {
    motor0.updateEncPulseRate();
    motor0.applyInputVoltage();
    motor0.updateInputVoltage();
}

void callbackControlPeriod1() {
    motor1.updateEncPulseRate();
    motor1.applyInputVoltage();
    motor1.updateInputVoltage();
}

// main() runs in its own thread in the OS
int main() {
    pc.baud(9600);
    
    // event queue
    queue.call_every(motor0.getControlPeriod(), &callbackControlPeriod0);
    queue.call_every(motor1.getControlPeriod(), &callbackControlPeriod1);
    queue.call_every(100, &key_input_pc);
    queue.call_every(100, &enc_output_pc);
    queue.dispatch();
}