for control a pololu motor with bd65496muv motor driver
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Mon Jul 18 2022 17:38:10 by
1.7.2