Jun KOBAYASHI / Mbed OS pololu_motor_bd65496muv

for control a pololu motor with bd65496muv motor driver

Dependencies:   QEI

main.cpp@2:cc875033357d, 2019-05-23 (annotated)

Committer:
jkoba0512
Date:
Thu May 23 00:39:11 2019 +0000
Revision:
2:cc875033357d
Parent:
1:e4fe1b3b7376
Child:
3:7fe433d24ff7
confirm two motor control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jkoba0512 0:fe9f19382f7a 1 #include "mbed.h"
jkoba0512 0:fe9f19382f7a 2 #include "QEI.h"
jkoba0512 0:fe9f19382f7a 3
jkoba0512 0:fe9f19382f7a 4 class MotorWithEncoder {
jkoba0512 0:fe9f19382f7a 5 private:
jkoba0512 0:fe9f19382f7a 6 PwmOut *pwm;
jkoba0512 0:fe9f19382f7a 7 DigitalOut *dir;
jkoba0512 0:fe9f19382f7a 8 float input_voltage;
jkoba0512 0:fe9f19382f7a 9 // gear
jkoba0512 0:fe9f19382f7a 10 const float GEAR_RATIO;
jkoba0512 0:fe9f19382f7a 11 // encoder
jkoba0512 0:fe9f19382f7a 12 QEI *enc;
jkoba0512 0:fe9f19382f7a 13 int enc_pulse_rate; // per control period
jkoba0512 0:fe9f19382f7a 14 // controller
jkoba0512 0:fe9f19382f7a 15 const int CONTROL_PERIOD; // control period (ms)
jkoba0512 0:fe9f19382f7a 16 bool controller_enable;
jkoba0512 0:fe9f19382f7a 17 int des_enc_pulse_rate; // per control period
jkoba0512 0:fe9f19382f7a 18 float Kp, Ki; // gains
jkoba0512 1:e4fe1b3b7376 19 int error; // error
jkoba0512 1:e4fe1b3b7376 20 int acc_error; // accumulated error;
jkoba0512 1:e4fe1b3b7376 21
jkoba0512 0:fe9f19382f7a 22 EventQueue *queue;
jkoba0512 0:fe9f19382f7a 23 public:
jkoba0512 0:fe9f19382f7a 24 MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio,
jkoba0512 0:fe9f19382f7a 25 PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi,
jkoba0512 0:fe9f19382f7a 26 int _control_period, float _Kp, float _Ki);
jkoba0512 0:fe9f19382f7a 27 // _pinPWM: pin outputs PWM
jkoba0512 0:fe9f19382f7a 28 // _pinDir: pin outputs LOW or HIGH for changing the input voltage direction
jkoba0512 0:fe9f19382f7a 29 // _pinA, _pinB: pins receive pulses from encoder
jkoba0512 0:fe9f19382f7a 30 // _res: resolution
jkoba0512 0:fe9f19382f7a 31 // _multi: QEI::X2_ENCODING or QEI::X4_ENCODING
jkoba0512 0:fe9f19382f7a 32 // _control_period: ms
jkoba0512 0:fe9f19382f7a 33 // _Kp, _Ki: gains
jkoba0512 0:fe9f19382f7a 34
jkoba0512 0:fe9f19382f7a 35 float getInputVoltage();
jkoba0512 0:fe9f19382f7a 36 void updateInputVoltage();
jkoba0512 0:fe9f19382f7a 37 void applyInputVoltage(); // callback
jkoba0512 0:fe9f19382f7a 38
jkoba0512 0:fe9f19382f7a 39 void resetEnc();
jkoba0512 0:fe9f19382f7a 40 void updateEncPulseRate(); // callback
jkoba0512 0:fe9f19382f7a 41 int getEncPulseRate();
jkoba0512 0:fe9f19382f7a 42
jkoba0512 0:fe9f19382f7a 43 bool getControllerEnable();
jkoba0512 0:fe9f19382f7a 44 void switchControllerEnable(bool _switch);
jkoba0512 0:fe9f19382f7a 45 int getControlPeriod();
jkoba0512 0:fe9f19382f7a 46 int getDesEncPulseRate();
jkoba0512 0:fe9f19382f7a 47 void setDesEncPulseRate(int _des_enc_pulse_rate);
jkoba0512 0:fe9f19382f7a 48 float getKp();
jkoba0512 0:fe9f19382f7a 49 void setKp(float _Kp);
jkoba0512 0:fe9f19382f7a 50 float getKi();
jkoba0512 0:fe9f19382f7a 51 void setKi(float _Ki);
jkoba0512 0:fe9f19382f7a 52 };
jkoba0512 0:fe9f19382f7a 53
jkoba0512 0:fe9f19382f7a 54 MotorWithEncoder::MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio,
jkoba0512 0:fe9f19382f7a 55 PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi,
jkoba0512 0:fe9f19382f7a 56 int _control_period, float _Kp, float _Ki)
jkoba0512 0:fe9f19382f7a 57 : GEAR_RATIO(_gear_ratio), CONTROL_PERIOD(_control_period) {
jkoba0512 0:fe9f19382f7a 58 pwm = new PwmOut(_pinPWM);
jkoba0512 0:fe9f19382f7a 59 pwm->period(0.0001); // 0.1 ms
jkoba0512 0:fe9f19382f7a 60 dir = new DigitalOut(_pinDir);
jkoba0512 0:fe9f19382f7a 61 enc = new QEI(_pinA, _pinB, NC, _res, _multi);
jkoba0512 0:fe9f19382f7a 62 controller_enable = false;
jkoba0512 0:fe9f19382f7a 63 des_enc_pulse_rate = 0;
jkoba0512 0:fe9f19382f7a 64 Kp = _Kp;
jkoba0512 0:fe9f19382f7a 65 Ki = _Ki;
jkoba0512 0:fe9f19382f7a 66 }
jkoba0512 0:fe9f19382f7a 67
jkoba0512 0:fe9f19382f7a 68 float MotorWithEncoder::getInputVoltage() {
jkoba0512 0:fe9f19382f7a 69 return input_voltage;
jkoba0512 0:fe9f19382f7a 70 }
jkoba0512 0:fe9f19382f7a 71
jkoba0512 0:fe9f19382f7a 72 void MotorWithEncoder::updateInputVoltage() {
jkoba0512 1:e4fe1b3b7376 73 float temp_input_voltage;
jkoba0512 0:fe9f19382f7a 74
jkoba0512 0:fe9f19382f7a 75 error = des_enc_pulse_rate - enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 76 acc_error += error;
jkoba0512 0:fe9f19382f7a 77
jkoba0512 1:e4fe1b3b7376 78 temp_input_voltage = (float)(Kp * error + Ki * acc_error);
jkoba0512 1:e4fe1b3b7376 79
jkoba0512 1:e4fe1b3b7376 80 if (temp_input_voltage > 1.0) {
jkoba0512 1:e4fe1b3b7376 81 input_voltage = 1.0;
jkoba0512 1:e4fe1b3b7376 82 acc_error -= error;
jkoba0512 1:e4fe1b3b7376 83 }
jkoba0512 1:e4fe1b3b7376 84 else if (temp_input_voltage < - 1.0) {
jkoba0512 1:e4fe1b3b7376 85 input_voltage = - 1.0;
jkoba0512 1:e4fe1b3b7376 86 acc_error -= error;
jkoba0512 1:e4fe1b3b7376 87 }
jkoba0512 1:e4fe1b3b7376 88 else {
jkoba0512 1:e4fe1b3b7376 89 input_voltage = temp_input_voltage;
jkoba0512 1:e4fe1b3b7376 90 }
jkoba0512 0:fe9f19382f7a 91 }
jkoba0512 0:fe9f19382f7a 92
jkoba0512 0:fe9f19382f7a 93 void MotorWithEncoder::applyInputVoltage() {
jkoba0512 0:fe9f19382f7a 94 if (controller_enable) {
jkoba0512 0:fe9f19382f7a 95 if (input_voltage < 0) *dir = 0; // CW
jkoba0512 0:fe9f19382f7a 96 else *dir = 1; // CCW
jkoba0512 0:fe9f19382f7a 97 *pwm = abs(input_voltage);
jkoba0512 0:fe9f19382f7a 98 }
jkoba0512 0:fe9f19382f7a 99 else {
jkoba0512 0:fe9f19382f7a 100 *pwm = 0;
jkoba0512 0:fe9f19382f7a 101 }
jkoba0512 0:fe9f19382f7a 102 }
jkoba0512 0:fe9f19382f7a 103
jkoba0512 0:fe9f19382f7a 104 void MotorWithEncoder::resetEnc() {
jkoba0512 0:fe9f19382f7a 105 enc->reset();
jkoba0512 0:fe9f19382f7a 106 }
jkoba0512 0:fe9f19382f7a 107
jkoba0512 0:fe9f19382f7a 108 void MotorWithEncoder::updateEncPulseRate() {
jkoba0512 0:fe9f19382f7a 109 enc_pulse_rate = enc->getPulses();
jkoba0512 0:fe9f19382f7a 110 enc->reset();
jkoba0512 0:fe9f19382f7a 111 }
jkoba0512 0:fe9f19382f7a 112
jkoba0512 0:fe9f19382f7a 113 int MotorWithEncoder::getEncPulseRate() {
jkoba0512 0:fe9f19382f7a 114 return enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 115 }
jkoba0512 0:fe9f19382f7a 116
jkoba0512 0:fe9f19382f7a 117 bool MotorWithEncoder::getControllerEnable() {
jkoba0512 0:fe9f19382f7a 118 return controller_enable;
jkoba0512 0:fe9f19382f7a 119 }
jkoba0512 0:fe9f19382f7a 120
jkoba0512 0:fe9f19382f7a 121 void MotorWithEncoder::switchControllerEnable(bool _switch) {
jkoba0512 1:e4fe1b3b7376 122 error = 0.0;
jkoba0512 1:e4fe1b3b7376 123 acc_error = 0.0;
jkoba0512 1:e4fe1b3b7376 124 input_voltage = 0.0;
jkoba0512 0:fe9f19382f7a 125 controller_enable = _switch;
jkoba0512 0:fe9f19382f7a 126 }
jkoba0512 0:fe9f19382f7a 127
jkoba0512 0:fe9f19382f7a 128 int MotorWithEncoder::getControlPeriod() {
jkoba0512 0:fe9f19382f7a 129 return CONTROL_PERIOD;
jkoba0512 0:fe9f19382f7a 130 }
jkoba0512 0:fe9f19382f7a 131
jkoba0512 0:fe9f19382f7a 132 int MotorWithEncoder::getDesEncPulseRate() {
jkoba0512 0:fe9f19382f7a 133 return des_enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 134 }
jkoba0512 0:fe9f19382f7a 135
jkoba0512 0:fe9f19382f7a 136 void MotorWithEncoder::setDesEncPulseRate(int _des_enc_pulse_rate) {
jkoba0512 0:fe9f19382f7a 137 des_enc_pulse_rate = _des_enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 138 }
jkoba0512 0:fe9f19382f7a 139
jkoba0512 0:fe9f19382f7a 140 float MotorWithEncoder::getKp() {
jkoba0512 0:fe9f19382f7a 141 return Kp;
jkoba0512 0:fe9f19382f7a 142 }
jkoba0512 0:fe9f19382f7a 143
jkoba0512 0:fe9f19382f7a 144 void MotorWithEncoder::setKp(float _Kp) {
jkoba0512 0:fe9f19382f7a 145 Kp = _Kp;
jkoba0512 0:fe9f19382f7a 146 }
jkoba0512 0:fe9f19382f7a 147
jkoba0512 0:fe9f19382f7a 148 float MotorWithEncoder::getKi() {
jkoba0512 0:fe9f19382f7a 149 return Ki;
jkoba0512 0:fe9f19382f7a 150 }
jkoba0512 0:fe9f19382f7a 151
jkoba0512 0:fe9f19382f7a 152 void MotorWithEncoder::setKi(float _Ki) {
jkoba0512 0:fe9f19382f7a 153 Ki = _Ki;
jkoba0512 0:fe9f19382f7a 154 }
jkoba0512 0:fe9f19382f7a 155
jkoba0512 0:fe9f19382f7a 156 // interface to pc
jkoba0512 0:fe9f19382f7a 157 Serial pc(USBTX, USBRX);
jkoba0512 0:fe9f19382f7a 158
jkoba0512 2:cc875033357d 159 // motor1
jkoba0512 2:cc875033357d 160 MotorWithEncoder motor0(p21, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
jkoba0512 2:cc875033357d 161 MotorWithEncoder motor1(p22, p28, 50, p7, p8, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
jkoba0512 0:fe9f19382f7a 162 // gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001
jkoba0512 0:fe9f19382f7a 163
jkoba0512 0:fe9f19382f7a 164 // key input from pc (callback)
jkoba0512 0:fe9f19382f7a 165 void key_input_pc() {
jkoba0512 0:fe9f19382f7a 166 if (pc.readable() == 1) {
jkoba0512 0:fe9f19382f7a 167 switch(pc.getc()) {
jkoba0512 0:fe9f19382f7a 168 case 'z':
jkoba0512 2:cc875033357d 169 motor0.switchControllerEnable(true);
jkoba0512 0:fe9f19382f7a 170 break;
jkoba0512 0:fe9f19382f7a 171 case 'q':
jkoba0512 2:cc875033357d 172 motor0.switchControllerEnable(false);
jkoba0512 0:fe9f19382f7a 173 break;
jkoba0512 0:fe9f19382f7a 174 case 'a':
jkoba0512 2:cc875033357d 175 motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() + 10);
jkoba0512 0:fe9f19382f7a 176 break;
jkoba0512 0:fe9f19382f7a 177 case 's':
jkoba0512 2:cc875033357d 178 motor0.setDesEncPulseRate(0);
jkoba0512 0:fe9f19382f7a 179 break;
jkoba0512 0:fe9f19382f7a 180 case 'd':
jkoba0512 2:cc875033357d 181 motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() - 10);
jkoba0512 2:cc875033357d 182 case 'v':
jkoba0512 2:cc875033357d 183 motor1.switchControllerEnable(true);
jkoba0512 2:cc875033357d 184 break;
jkoba0512 2:cc875033357d 185 case 'r':
jkoba0512 2:cc875033357d 186 motor1.switchControllerEnable(false);
jkoba0512 2:cc875033357d 187 break;
jkoba0512 2:cc875033357d 188 case 'f':
jkoba0512 2:cc875033357d 189 motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() + 10);
jkoba0512 2:cc875033357d 190 break;
jkoba0512 2:cc875033357d 191 case 'g':
jkoba0512 2:cc875033357d 192 motor1.setDesEncPulseRate(0);
jkoba0512 2:cc875033357d 193 break;
jkoba0512 2:cc875033357d 194 case 'h':
jkoba0512 2:cc875033357d 195 motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() - 10);
jkoba0512 0:fe9f19382f7a 196 }
jkoba0512 0:fe9f19382f7a 197 }
jkoba0512 0:fe9f19382f7a 198 }
jkoba0512 0:fe9f19382f7a 199
jkoba0512 0:fe9f19382f7a 200 // encoder output to pc
jkoba0512 0:fe9f19382f7a 201 void enc_output_pc() {
jkoba0512 2:cc875033357d 202 if (motor0.getControllerEnable()) {
jkoba0512 2:cc875033357d 203 pc.printf("ON0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
jkoba0512 0:fe9f19382f7a 204 }
jkoba0512 0:fe9f19382f7a 205 else {
jkoba0512 2:cc875033357d 206 pc.printf("OFF0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
jkoba0512 2:cc875033357d 207 }
jkoba0512 2:cc875033357d 208 if (motor1.getControllerEnable()) {
jkoba0512 2:cc875033357d 209 pc.printf("ON1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
jkoba0512 2:cc875033357d 210 }
jkoba0512 2:cc875033357d 211 else {
jkoba0512 2:cc875033357d 212 pc.printf("OFF1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
jkoba0512 0:fe9f19382f7a 213 }
jkoba0512 0:fe9f19382f7a 214 }
jkoba0512 0:fe9f19382f7a 215
jkoba0512 0:fe9f19382f7a 216 // event queue
jkoba0512 0:fe9f19382f7a 217 EventQueue queue;
jkoba0512 0:fe9f19382f7a 218
jkoba0512 0:fe9f19382f7a 219 // callback per control period
jkoba0512 0:fe9f19382f7a 220 // update motor voltage
jkoba0512 0:fe9f19382f7a 221 // update enc_pulse_ratge
jkoba0512 0:fe9f19382f7a 222 void callbackControlPeriod() {
jkoba0512 2:cc875033357d 223 motor0.updateEncPulseRate();
jkoba0512 2:cc875033357d 224 motor1.updateEncPulseRate();
jkoba0512 2:cc875033357d 225 motor0.applyInputVoltage();
jkoba0512 2:cc875033357d 226 motor1.applyInputVoltage();
jkoba0512 0:fe9f19382f7a 227
jkoba0512 2:cc875033357d 228 motor0.updateInputVoltage();
jkoba0512 2:cc875033357d 229 motor1.updateInputVoltage();
jkoba0512 0:fe9f19382f7a 230 }
jkoba0512 0:fe9f19382f7a 231
jkoba0512 0:fe9f19382f7a 232 // main() runs in its own thread in the OS
jkoba0512 0:fe9f19382f7a 233 int main() {
jkoba0512 0:fe9f19382f7a 234 pc.baud(9600);
jkoba0512 0:fe9f19382f7a 235
jkoba0512 0:fe9f19382f7a 236 // event queue
jkoba0512 2:cc875033357d 237 queue.call_every(motor0.getControlPeriod(), &callbackControlPeriod);
jkoba0512 2:cc875033357d 238 queue.call_every(motor1.getControlPeriod(), &callbackControlPeriod);
jkoba0512 0:fe9f19382f7a 239 queue.call_every(100, &key_input_pc);
jkoba0512 0:fe9f19382f7a 240 queue.call_every(100, &enc_output_pc);
jkoba0512 0:fe9f19382f7a 241 queue.dispatch();
jkoba0512 0:fe9f19382f7a 242 }