Diff: main.cpp

Revision:

0:fe9f19382f7a

Child:

1:e4fe1b3b7376

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 31 22:29:03 2018 +0000
@@ -0,0 +1,200 @@
+#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
+    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() {
+    int error = 0;
+    static int acc_error = 0;
+    
+    error = des_enc_pulse_rate - enc_pulse_rate;
+    acc_error += error;
+    
+    input_voltage =  (float)(Kp * error + Ki * acc_error);
+}
+
+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) {
+    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);
+
+// motor
+MotorWithEncoder motor(p26, p27, 50, p5, p6, 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':
+                motor.switchControllerEnable(true);
+                break;
+            case 'q':
+                motor.switchControllerEnable(false);
+                break;
+            case 'a':
+                motor.setDesEncPulseRate(motor.getDesEncPulseRate() + 10);
+                break;
+            case 's':
+                motor.setDesEncPulseRate(0);
+                break;
+            case 'd':
+                motor.setDesEncPulseRate(motor.getDesEncPulseRate() - 10);
+        }
+    }
+}
+
+// encoder output to pc
+void enc_output_pc() {
+    if (motor.getControllerEnable()) {
+        pc.printf("ON: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
+    }
+    else {
+        pc.printf("OFF: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
+    }
+}
+
+// event queue
+EventQueue queue;
+
+// callback per control period
+// update motor voltage
+// update enc_pulse_ratge
+void callbackControlPeriod() {
+    motor.updateEncPulseRate();
+    motor.applyInputVoltage();
+    
+    motor.updateInputVoltage();
+}
+
+// main() runs in its own thread in the OS
+int main() {
+    pc.baud(9600);
+    
+    // event queue
+    queue.call_every(motor.getControlPeriod(), &callbackControlPeriod);
+    queue.call_every(100, &key_input_pc);
+    queue.call_every(100, &enc_output_pc);
+    queue.dispatch();
+}