Jun KOBAYASHI / Mbed OS pololu_motor_bd65496muv

Dependencies:   QEI

main.cpp@0:fe9f19382f7a, 2018-07-31 (annotated)

Committer:
jkoba0512
Date:
Tue Jul 31 22:29:03 2018 +0000
Revision:
0:fe9f19382f7a
Child:
1:e4fe1b3b7376
first commit

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 0:fe9f19382f7a 19 EventQueue *queue;
jkoba0512 0:fe9f19382f7a 20 public:
jkoba0512 0:fe9f19382f7a 21 MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio,
jkoba0512 0:fe9f19382f7a 22 PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi,
jkoba0512 0:fe9f19382f7a 23 int _control_period, float _Kp, float _Ki);
jkoba0512 0:fe9f19382f7a 24 // _pinPWM: pin outputs PWM
jkoba0512 0:fe9f19382f7a 25 // _pinDir: pin outputs LOW or HIGH for changing the input voltage direction
jkoba0512 0:fe9f19382f7a 26 // _pinA, _pinB: pins receive pulses from encoder
jkoba0512 0:fe9f19382f7a 27 // _res: resolution
jkoba0512 0:fe9f19382f7a 28 // _multi: QEI::X2_ENCODING or QEI::X4_ENCODING
jkoba0512 0:fe9f19382f7a 29 // _control_period: ms
jkoba0512 0:fe9f19382f7a 30 // _Kp, _Ki: gains
jkoba0512 0:fe9f19382f7a 31
jkoba0512 0:fe9f19382f7a 32 float getInputVoltage();
jkoba0512 0:fe9f19382f7a 33 void updateInputVoltage();
jkoba0512 0:fe9f19382f7a 34 void applyInputVoltage(); // callback
jkoba0512 0:fe9f19382f7a 35
jkoba0512 0:fe9f19382f7a 36 void resetEnc();
jkoba0512 0:fe9f19382f7a 37 void updateEncPulseRate(); // callback
jkoba0512 0:fe9f19382f7a 38 int getEncPulseRate();
jkoba0512 0:fe9f19382f7a 39
jkoba0512 0:fe9f19382f7a 40 bool getControllerEnable();
jkoba0512 0:fe9f19382f7a 41 void switchControllerEnable(bool _switch);
jkoba0512 0:fe9f19382f7a 42 int getControlPeriod();
jkoba0512 0:fe9f19382f7a 43 int getDesEncPulseRate();
jkoba0512 0:fe9f19382f7a 44 void setDesEncPulseRate(int _des_enc_pulse_rate);
jkoba0512 0:fe9f19382f7a 45 float getKp();
jkoba0512 0:fe9f19382f7a 46 void setKp(float _Kp);
jkoba0512 0:fe9f19382f7a 47 float getKi();
jkoba0512 0:fe9f19382f7a 48 void setKi(float _Ki);
jkoba0512 0:fe9f19382f7a 49 };
jkoba0512 0:fe9f19382f7a 50
jkoba0512 0:fe9f19382f7a 51 MotorWithEncoder::MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio,
jkoba0512 0:fe9f19382f7a 52 PinName _pinA, PinName _pinB, int _res, QEI::Encoding _multi,
jkoba0512 0:fe9f19382f7a 53 int _control_period, float _Kp, float _Ki)
jkoba0512 0:fe9f19382f7a 54 : GEAR_RATIO(_gear_ratio), CONTROL_PERIOD(_control_period) {
jkoba0512 0:fe9f19382f7a 55 pwm = new PwmOut(_pinPWM);
jkoba0512 0:fe9f19382f7a 56 pwm->period(0.0001); // 0.1 ms
jkoba0512 0:fe9f19382f7a 57 dir = new DigitalOut(_pinDir);
jkoba0512 0:fe9f19382f7a 58 enc = new QEI(_pinA, _pinB, NC, _res, _multi);
jkoba0512 0:fe9f19382f7a 59 controller_enable = false;
jkoba0512 0:fe9f19382f7a 60 des_enc_pulse_rate = 0;
jkoba0512 0:fe9f19382f7a 61 Kp = _Kp;
jkoba0512 0:fe9f19382f7a 62 Ki = _Ki;
jkoba0512 0:fe9f19382f7a 63 }
jkoba0512 0:fe9f19382f7a 64
jkoba0512 0:fe9f19382f7a 65 float MotorWithEncoder::getInputVoltage() {
jkoba0512 0:fe9f19382f7a 66 return input_voltage;
jkoba0512 0:fe9f19382f7a 67 }
jkoba0512 0:fe9f19382f7a 68
jkoba0512 0:fe9f19382f7a 69 void MotorWithEncoder::updateInputVoltage() {
jkoba0512 0:fe9f19382f7a 70 int error = 0;
jkoba0512 0:fe9f19382f7a 71 static int acc_error = 0;
jkoba0512 0:fe9f19382f7a 72
jkoba0512 0:fe9f19382f7a 73 error = des_enc_pulse_rate - enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 74 acc_error += error;
jkoba0512 0:fe9f19382f7a 75
jkoba0512 0:fe9f19382f7a 76 input_voltage = (float)(Kp * error + Ki * acc_error);
jkoba0512 0:fe9f19382f7a 77 }
jkoba0512 0:fe9f19382f7a 78
jkoba0512 0:fe9f19382f7a 79 void MotorWithEncoder::applyInputVoltage() {
jkoba0512 0:fe9f19382f7a 80 if (controller_enable) {
jkoba0512 0:fe9f19382f7a 81 if (input_voltage < 0) *dir = 0; // CW
jkoba0512 0:fe9f19382f7a 82 else *dir = 1; // CCW
jkoba0512 0:fe9f19382f7a 83 *pwm = abs(input_voltage);
jkoba0512 0:fe9f19382f7a 84 }
jkoba0512 0:fe9f19382f7a 85 else {
jkoba0512 0:fe9f19382f7a 86 *pwm = 0;
jkoba0512 0:fe9f19382f7a 87 }
jkoba0512 0:fe9f19382f7a 88 }
jkoba0512 0:fe9f19382f7a 89
jkoba0512 0:fe9f19382f7a 90 void MotorWithEncoder::resetEnc() {
jkoba0512 0:fe9f19382f7a 91 enc->reset();
jkoba0512 0:fe9f19382f7a 92 }
jkoba0512 0:fe9f19382f7a 93
jkoba0512 0:fe9f19382f7a 94 void MotorWithEncoder::updateEncPulseRate() {
jkoba0512 0:fe9f19382f7a 95 enc_pulse_rate = enc->getPulses();
jkoba0512 0:fe9f19382f7a 96 enc->reset();
jkoba0512 0:fe9f19382f7a 97 }
jkoba0512 0:fe9f19382f7a 98
jkoba0512 0:fe9f19382f7a 99 int MotorWithEncoder::getEncPulseRate() {
jkoba0512 0:fe9f19382f7a 100 return enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 101 }
jkoba0512 0:fe9f19382f7a 102
jkoba0512 0:fe9f19382f7a 103 bool MotorWithEncoder::getControllerEnable() {
jkoba0512 0:fe9f19382f7a 104 return controller_enable;
jkoba0512 0:fe9f19382f7a 105 }
jkoba0512 0:fe9f19382f7a 106
jkoba0512 0:fe9f19382f7a 107 void MotorWithEncoder::switchControllerEnable(bool _switch) {
jkoba0512 0:fe9f19382f7a 108 controller_enable = _switch;
jkoba0512 0:fe9f19382f7a 109 }
jkoba0512 0:fe9f19382f7a 110
jkoba0512 0:fe9f19382f7a 111 int MotorWithEncoder::getControlPeriod() {
jkoba0512 0:fe9f19382f7a 112 return CONTROL_PERIOD;
jkoba0512 0:fe9f19382f7a 113 }
jkoba0512 0:fe9f19382f7a 114
jkoba0512 0:fe9f19382f7a 115 int MotorWithEncoder::getDesEncPulseRate() {
jkoba0512 0:fe9f19382f7a 116 return des_enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 117 }
jkoba0512 0:fe9f19382f7a 118
jkoba0512 0:fe9f19382f7a 119 void MotorWithEncoder::setDesEncPulseRate(int _des_enc_pulse_rate) {
jkoba0512 0:fe9f19382f7a 120 des_enc_pulse_rate = _des_enc_pulse_rate;
jkoba0512 0:fe9f19382f7a 121 }
jkoba0512 0:fe9f19382f7a 122
jkoba0512 0:fe9f19382f7a 123 float MotorWithEncoder::getKp() {
jkoba0512 0:fe9f19382f7a 124 return Kp;
jkoba0512 0:fe9f19382f7a 125 }
jkoba0512 0:fe9f19382f7a 126
jkoba0512 0:fe9f19382f7a 127 void MotorWithEncoder::setKp(float _Kp) {
jkoba0512 0:fe9f19382f7a 128 Kp = _Kp;
jkoba0512 0:fe9f19382f7a 129 }
jkoba0512 0:fe9f19382f7a 130
jkoba0512 0:fe9f19382f7a 131 float MotorWithEncoder::getKi() {
jkoba0512 0:fe9f19382f7a 132 return Ki;
jkoba0512 0:fe9f19382f7a 133 }
jkoba0512 0:fe9f19382f7a 134
jkoba0512 0:fe9f19382f7a 135 void MotorWithEncoder::setKi(float _Ki) {
jkoba0512 0:fe9f19382f7a 136 Ki = _Ki;
jkoba0512 0:fe9f19382f7a 137 }
jkoba0512 0:fe9f19382f7a 138
jkoba0512 0:fe9f19382f7a 139 // interface to pc
jkoba0512 0:fe9f19382f7a 140 Serial pc(USBTX, USBRX);
jkoba0512 0:fe9f19382f7a 141
jkoba0512 0:fe9f19382f7a 142 // motor
jkoba0512 0:fe9f19382f7a 143 MotorWithEncoder motor(p26, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
jkoba0512 0:fe9f19382f7a 144 // gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001
jkoba0512 0:fe9f19382f7a 145
jkoba0512 0:fe9f19382f7a 146 // key input from pc (callback)
jkoba0512 0:fe9f19382f7a 147 void key_input_pc() {
jkoba0512 0:fe9f19382f7a 148 if (pc.readable() == 1) {
jkoba0512 0:fe9f19382f7a 149 switch(pc.getc()) {
jkoba0512 0:fe9f19382f7a 150 case 'z':
jkoba0512 0:fe9f19382f7a 151 motor.switchControllerEnable(true);
jkoba0512 0:fe9f19382f7a 152 break;
jkoba0512 0:fe9f19382f7a 153 case 'q':
jkoba0512 0:fe9f19382f7a 154 motor.switchControllerEnable(false);
jkoba0512 0:fe9f19382f7a 155 break;
jkoba0512 0:fe9f19382f7a 156 case 'a':
jkoba0512 0:fe9f19382f7a 157 motor.setDesEncPulseRate(motor.getDesEncPulseRate() + 10);
jkoba0512 0:fe9f19382f7a 158 break;
jkoba0512 0:fe9f19382f7a 159 case 's':
jkoba0512 0:fe9f19382f7a 160 motor.setDesEncPulseRate(0);
jkoba0512 0:fe9f19382f7a 161 break;
jkoba0512 0:fe9f19382f7a 162 case 'd':
jkoba0512 0:fe9f19382f7a 163 motor.setDesEncPulseRate(motor.getDesEncPulseRate() - 10);
jkoba0512 0:fe9f19382f7a 164 }
jkoba0512 0:fe9f19382f7a 165 }
jkoba0512 0:fe9f19382f7a 166 }
jkoba0512 0:fe9f19382f7a 167
jkoba0512 0:fe9f19382f7a 168 // encoder output to pc
jkoba0512 0:fe9f19382f7a 169 void enc_output_pc() {
jkoba0512 0:fe9f19382f7a 170 if (motor.getControllerEnable()) {
jkoba0512 0:fe9f19382f7a 171 pc.printf("ON: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
jkoba0512 0:fe9f19382f7a 172 }
jkoba0512 0:fe9f19382f7a 173 else {
jkoba0512 0:fe9f19382f7a 174 pc.printf("OFF: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
jkoba0512 0:fe9f19382f7a 175 }
jkoba0512 0:fe9f19382f7a 176 }
jkoba0512 0:fe9f19382f7a 177
jkoba0512 0:fe9f19382f7a 178 // event queue
jkoba0512 0:fe9f19382f7a 179 EventQueue queue;
jkoba0512 0:fe9f19382f7a 180
jkoba0512 0:fe9f19382f7a 181 // callback per control period
jkoba0512 0:fe9f19382f7a 182 // update motor voltage
jkoba0512 0:fe9f19382f7a 183 // update enc_pulse_ratge
jkoba0512 0:fe9f19382f7a 184 void callbackControlPeriod() {
jkoba0512 0:fe9f19382f7a 185 motor.updateEncPulseRate();
jkoba0512 0:fe9f19382f7a 186 motor.applyInputVoltage();
jkoba0512 0:fe9f19382f7a 187
jkoba0512 0:fe9f19382f7a 188 motor.updateInputVoltage();
jkoba0512 0:fe9f19382f7a 189 }
jkoba0512 0:fe9f19382f7a 190
jkoba0512 0:fe9f19382f7a 191 // main() runs in its own thread in the OS
jkoba0512 0:fe9f19382f7a 192 int main() {
jkoba0512 0:fe9f19382f7a 193 pc.baud(9600);
jkoba0512 0:fe9f19382f7a 194
jkoba0512 0:fe9f19382f7a 195 // event queue
jkoba0512 0:fe9f19382f7a 196 queue.call_every(motor.getControlPeriod(), &callbackControlPeriod);
jkoba0512 0:fe9f19382f7a 197 queue.call_every(100, &key_input_pc);
jkoba0512 0:fe9f19382f7a 198 queue.call_every(100, &enc_output_pc);
jkoba0512 0:fe9f19382f7a 199 queue.dispatch();
jkoba0512 0:fe9f19382f7a 200 }