for control a pololu motor with bd65496muv motor driver

Dependencies:   QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 
00004 class MotorWithEncoder {
00005 private:
00006     PwmOut *pwm;
00007     DigitalOut *dir;
00008     float input_voltage;
00009     // gear
00010     const float GEAR_RATIO;
00011     // encoder
00012     QEI *enc;
00013     int enc_pulse_rate;  // per control period
00014     // controller
00015     const int CONTROL_PERIOD;  // control period (ms)
00016     bool controller_enable;
00017     int des_enc_pulse_rate;  // per control period
00018     float Kp, Ki;  // gains
00019     int error;  // error
00020     int acc_error;  // accumulated error;
00021 
00022     EventQueue *queue;
00023 public:
00024     MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio, 
00025         PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi, 
00026         int _control_period, float _Kp, float _Ki);
00027     // _pinPWM: pin outputs PWM
00028     // _pinDir: pin outputs LOW or HIGH for changing the input voltage direction
00029     // _pinA, _pinB: pins receive pulses from encoder
00030     // _res: resolution
00031     // _multi: QEI::X2_ENCODING or QEI::X4_ENCODING
00032     // _control_period: ms
00033     // _Kp, _Ki: gains
00034     
00035     float getInputVoltage();
00036     void updateInputVoltage();
00037     void applyInputVoltage();  // callback
00038     
00039     void resetEnc();
00040     void updateEncPulseRate();  // callback
00041     int getEncPulseRate();
00042     
00043     bool getControllerEnable();
00044     void switchControllerEnable(bool _switch);
00045     int getControlPeriod();
00046     int getDesEncPulseRate();
00047     void setDesEncPulseRate(int _des_enc_pulse_rate);
00048     float getKp();
00049     void setKp(float _Kp);
00050     float getKi();
00051     void setKi(float _Ki);
00052 };
00053 
00054 MotorWithEncoder::MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio, 
00055     PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi, 
00056     int _control_period, float _Kp, float _Ki) 
00057     : GEAR_RATIO(_gear_ratio), CONTROL_PERIOD(_control_period) {
00058     pwm = new PwmOut(_pinPWM);
00059     pwm->period(0.0001);  // 0.1 ms
00060     dir = new DigitalOut(_pinDir);
00061     enc = new QEI(_pinA, _pinB, NC, _res, _multi);
00062     controller_enable = false;
00063     des_enc_pulse_rate = 0;
00064     Kp = _Kp;
00065     Ki = _Ki;
00066 }
00067 
00068 float MotorWithEncoder::getInputVoltage() {
00069     return input_voltage;
00070 }
00071     
00072 void MotorWithEncoder::updateInputVoltage() {
00073     float temp_input_voltage;
00074     
00075     error = des_enc_pulse_rate - enc_pulse_rate;
00076     acc_error += error;
00077     
00078     temp_input_voltage =  (float)(Kp * error + Ki * acc_error);
00079     
00080     if (temp_input_voltage > 1.0) {
00081         input_voltage = 1.0;
00082         acc_error -= error;
00083     }
00084     else if (temp_input_voltage < - 1.0) {
00085         input_voltage = - 1.0;
00086         acc_error -= error;
00087     }
00088     else {
00089         input_voltage = temp_input_voltage;
00090     }
00091 }
00092 
00093 void MotorWithEncoder::applyInputVoltage() {
00094     if (controller_enable) {
00095         if (input_voltage < 0) *dir = 0;  // CW
00096         else *dir = 1;                    // CCW
00097         *pwm = abs(input_voltage);
00098     }
00099     else {
00100         *pwm = 0;
00101     }
00102 }
00103 
00104 void MotorWithEncoder::resetEnc() {
00105     enc->reset();
00106 }
00107 
00108 void MotorWithEncoder::updateEncPulseRate() {
00109     enc_pulse_rate = enc->getPulses();
00110     enc->reset();
00111 }
00112 
00113 int MotorWithEncoder::getEncPulseRate() {
00114     return enc_pulse_rate;
00115 }
00116 
00117 bool MotorWithEncoder::getControllerEnable() {
00118     return controller_enable;
00119 }
00120 
00121 void MotorWithEncoder::switchControllerEnable(bool _switch) {
00122     error = 0.0;
00123     acc_error = 0.0;
00124     input_voltage = 0.0;
00125     controller_enable = _switch;
00126 }
00127 
00128 int MotorWithEncoder::getControlPeriod() {
00129     return CONTROL_PERIOD;
00130 }
00131 
00132 int MotorWithEncoder::getDesEncPulseRate() {
00133     return des_enc_pulse_rate;
00134 }
00135 
00136 void MotorWithEncoder::setDesEncPulseRate(int _des_enc_pulse_rate) {
00137     des_enc_pulse_rate = _des_enc_pulse_rate;
00138 }
00139 
00140 float MotorWithEncoder::getKp() {
00141     return Kp;
00142 }
00143 
00144 void MotorWithEncoder::setKp(float _Kp) {
00145     Kp = _Kp;
00146 }
00147 
00148 float MotorWithEncoder::getKi() {
00149     return Ki;
00150 }
00151 
00152 void MotorWithEncoder::setKi(float _Ki) {
00153     Ki = _Ki;
00154 }
00155 
00156 // interface to pc
00157 Serial pc(USBTX, USBRX);
00158 
00159 // motor1
00160 MotorWithEncoder motor0(p21, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
00161 MotorWithEncoder motor1(p22, p28, 50, p7, p8, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
00162 // gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001
00163 
00164 // key input from pc (callback)
00165 void key_input_pc() {
00166     if (pc.readable() == 1) {
00167         switch(pc.getc()) {
00168             case 'z':
00169                 motor0.switchControllerEnable(true);
00170                 break;
00171             case 'q':
00172                 motor0.switchControllerEnable(false);
00173                 break;
00174             case 'a':
00175                 motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() + 10);
00176                 break;
00177             case 's':
00178                 motor0.setDesEncPulseRate(0);
00179                 break;
00180             case 'd':
00181                 motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() - 10);
00182                 break;
00183             case 'v':
00184                 motor1.switchControllerEnable(true);
00185                 break;
00186             case 'r':
00187                 motor1.switchControllerEnable(false);
00188                 break;
00189             case 'f':
00190                 motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() + 10);
00191                 break;
00192             case 'g':
00193                 motor1.setDesEncPulseRate(0);
00194                 break;
00195             case 'h':
00196                 motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() - 10);
00197                 break;
00198         }
00199     }
00200 }
00201 
00202 // encoder output to pc
00203 void enc_output_pc() {
00204     if (motor0.getControllerEnable()) {
00205         pc.printf("ON0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
00206     }
00207     else {
00208         pc.printf("OFF0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
00209     }
00210     if (motor1.getControllerEnable()) {
00211         pc.printf("ON1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
00212     }
00213     else {
00214         pc.printf("OFF1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
00215     }
00216 }
00217 
00218 // event queue
00219 EventQueue queue;
00220 
00221 // callback per control period
00222 // update motor voltage
00223 // update enc_pulse_ratge
00224 void callbackControlPeriod0() {
00225     motor0.updateEncPulseRate();
00226     motor0.applyInputVoltage();
00227     motor0.updateInputVoltage();
00228 }
00229 
00230 void callbackControlPeriod1() {
00231     motor1.updateEncPulseRate();
00232     motor1.applyInputVoltage();
00233     motor1.updateInputVoltage();
00234 }
00235 
00236 // main() runs in its own thread in the OS
00237 int main() {
00238     pc.baud(9600);
00239     
00240     // event queue
00241     queue.call_every(motor0.getControlPeriod(), &callbackControlPeriod0);
00242     queue.call_every(motor1.getControlPeriod(), &callbackControlPeriod1);
00243     queue.call_every(100, &key_input_pc);
00244     queue.call_every(100, &enc_output_pc);
00245     queue.dispatch();
00246 }