Jun KOBAYASHI / Mbed OS pololu_motor_bd65496muv

Dependencies:   QEI

main.cpp@1:e4fe1b3b7376, 2019-05-21 (annotated)

Committer:
jkoba0512
Date:
Tue May 21 07:12:26 2019 +0000
Revision:
1:e4fe1b3b7376
Parent:
0:fe9f19382f7a
Child:
2:cc875033357d
add anti-windup to updateInputVoltage

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 0:fe9f19382f7a 159 // motor
jkoba0512 0:fe9f19382f7a 160 MotorWithEncoder motor(p26, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
jkoba0512 0:fe9f19382f7a 161 // gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001
jkoba0512 0:fe9f19382f7a 162
jkoba0512 0:fe9f19382f7a 163 // key input from pc (callback)
jkoba0512 0:fe9f19382f7a 164 void key_input_pc() {
jkoba0512 0:fe9f19382f7a 165 if (pc.readable() == 1) {
jkoba0512 0:fe9f19382f7a 166 switch(pc.getc()) {
jkoba0512 0:fe9f19382f7a 167 case 'z':
jkoba0512 0:fe9f19382f7a 168 motor.switchControllerEnable(true);
jkoba0512 0:fe9f19382f7a 169 break;
jkoba0512 0:fe9f19382f7a 170 case 'q':
jkoba0512 0:fe9f19382f7a 171 motor.switchControllerEnable(false);
jkoba0512 0:fe9f19382f7a 172 break;
jkoba0512 0:fe9f19382f7a 173 case 'a':
jkoba0512 0:fe9f19382f7a 174 motor.setDesEncPulseRate(motor.getDesEncPulseRate() + 10);
jkoba0512 0:fe9f19382f7a 175 break;
jkoba0512 0:fe9f19382f7a 176 case 's':
jkoba0512 0:fe9f19382f7a 177 motor.setDesEncPulseRate(0);
jkoba0512 0:fe9f19382f7a 178 break;
jkoba0512 0:fe9f19382f7a 179 case 'd':
jkoba0512 0:fe9f19382f7a 180 motor.setDesEncPulseRate(motor.getDesEncPulseRate() - 10);
jkoba0512 0:fe9f19382f7a 181 }
jkoba0512 0:fe9f19382f7a 182 }
jkoba0512 0:fe9f19382f7a 183 }
jkoba0512 0:fe9f19382f7a 184
jkoba0512 0:fe9f19382f7a 185 // encoder output to pc
jkoba0512 0:fe9f19382f7a 186 void enc_output_pc() {
jkoba0512 0:fe9f19382f7a 187 if (motor.getControllerEnable()) {
jkoba0512 0:fe9f19382f7a 188 pc.printf("ON: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
jkoba0512 0:fe9f19382f7a 189 }
jkoba0512 0:fe9f19382f7a 190 else {
jkoba0512 0:fe9f19382f7a 191 pc.printf("OFF: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
jkoba0512 0:fe9f19382f7a 192 }
jkoba0512 0:fe9f19382f7a 193 }
jkoba0512 0:fe9f19382f7a 194
jkoba0512 0:fe9f19382f7a 195 // event queue
jkoba0512 0:fe9f19382f7a 196 EventQueue queue;
jkoba0512 0:fe9f19382f7a 197
jkoba0512 0:fe9f19382f7a 198 // callback per control period
jkoba0512 0:fe9f19382f7a 199 // update motor voltage
jkoba0512 0:fe9f19382f7a 200 // update enc_pulse_ratge
jkoba0512 0:fe9f19382f7a 201 void callbackControlPeriod() {
jkoba0512 0:fe9f19382f7a 202 motor.updateEncPulseRate();
jkoba0512 0:fe9f19382f7a 203 motor.applyInputVoltage();
jkoba0512 0:fe9f19382f7a 204
jkoba0512 0:fe9f19382f7a 205 motor.updateInputVoltage();
jkoba0512 0:fe9f19382f7a 206 }
jkoba0512 0:fe9f19382f7a 207
jkoba0512 0:fe9f19382f7a 208 // main() runs in its own thread in the OS
jkoba0512 0:fe9f19382f7a 209 int main() {
jkoba0512 0:fe9f19382f7a 210 pc.baud(9600);
jkoba0512 0:fe9f19382f7a 211
jkoba0512 0:fe9f19382f7a 212 // event queue
jkoba0512 0:fe9f19382f7a 213 queue.call_every(motor.getControlPeriod(), &callbackControlPeriod);
jkoba0512 0:fe9f19382f7a 214 queue.call_every(100, &key_input_pc);
jkoba0512 0:fe9f19382f7a 215 queue.call_every(100, &enc_output_pc);
jkoba0512 0:fe9f19382f7a 216 queue.dispatch();
jkoba0512 0:fe9f19382f7a 217 }