Martijn Grootens / Mbed 2 deprecated mbr_2017_robot_gr20

Code to let Gr20's BioRobotics2017 robot come to live.

Dependencies:   FastPWM MODSERIAL QEI mbed

motor.h@2:df0c6af898ac, 2017-11-12 (annotated)

Committer:
megrootens
Date:
Sun Nov 12 14:06:23 2017 +0000
Revision:
2:df0c6af898ac
Parent:
0:caa8ee3bd882
Child:
7:b9a209f889f5
this works, only controller needs to be updated since I broke the moving average filtered one....;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:caa8ee3bd882 1 #ifndef _MOTOR_H_
megrootens 0:caa8ee3bd882 2 #define _MOTOR_H_
megrootens 0:caa8ee3bd882 3
megrootens 0:caa8ee3bd882 4 #include "FastPWM.h"
megrootens 0:caa8ee3bd882 5 #include "QEI.h"
megrootens 0:caa8ee3bd882 6
megrootens 2:df0c6af898ac 7 // Very high-frequency PWM signal so as to cut out noise.
megrootens 0:caa8ee3bd882 8 #define PWM_PERIOD_US 10
megrootens 0:caa8ee3bd882 9
megrootens 2:df0c6af898ac 10 /**
megrootens 2:df0c6af898ac 11 * MOTOR CLASS
megrootens 2:df0c6af898ac 12 * The D-action is used on a moving average filtered version of the error that
megrootens 2:df0c6af898ac 13 * is put in, against (numerical) noise.
megrootens 2:df0c6af898ac 14 *
megrootens 2:df0c6af898ac 15 * __implementation can be found in main.cpp__
megrootens 2:df0c6af898ac 16 */
megrootens 0:caa8ee3bd882 17 class Motor
megrootens 0:caa8ee3bd882 18 {
megrootens 0:caa8ee3bd882 19 public:
megrootens 2:df0c6af898ac 20 /**
megrootens 2:df0c6af898ac 21 * Constructor
megrootens 2:df0c6af898ac 22 */
megrootens 0:caa8ee3bd882 23 Motor(PinName pwm, PinName dir, PinName enc_a, PinName enc_b,
megrootens 2:df0c6af898ac 24 int pulses_per_rev, bool invert_dir = false)
megrootens 2:df0c6af898ac 25 :
megrootens 0:caa8ee3bd882 26 encoder_(enc_a, enc_b, NC, NULL, QEI::X4_ENCODING),
megrootens 0:caa8ee3bd882 27 dir_(dir),
megrootens 0:caa8ee3bd882 28 speed_(pwm),
megrootens 0:caa8ee3bd882 29 kPulsesPerRev_(pulses_per_rev),
megrootens 2:df0c6af898ac 30 kInvertDir_(invert_dir)
megrootens 0:caa8ee3bd882 31 {
megrootens 0:caa8ee3bd882 32 Stop();
megrootens 0:caa8ee3bd882 33 offset_ = 0;
megrootens 0:caa8ee3bd882 34 speed_.period_us(PWM_PERIOD_US);
megrootens 0:caa8ee3bd882 35 }
megrootens 0:caa8ee3bd882 36
megrootens 2:df0c6af898ac 37 /**
megrootens 2:df0c6af898ac 38 * Turn off motor
megrootens 2:df0c6af898ac 39 */
megrootens 0:caa8ee3bd882 40 void Stop() {
megrootens 0:caa8ee3bd882 41 power_ = false;
megrootens 0:caa8ee3bd882 42 dir_=0;
megrootens 0:caa8ee3bd882 43 speed_=0;
megrootens 0:caa8ee3bd882 44 }
megrootens 0:caa8ee3bd882 45
megrootens 2:df0c6af898ac 46 /**
megrootens 2:df0c6af898ac 47 * Start motor
megrootens 2:df0c6af898ac 48 */
megrootens 0:caa8ee3bd882 49 void Start() {
megrootens 0:caa8ee3bd882 50 power_ = true;
megrootens 0:caa8ee3bd882 51 }
megrootens 0:caa8ee3bd882 52
megrootens 2:df0c6af898ac 53 /**
megrootens 2:df0c6af898ac 54 * Is the motor allowed to move?
megrootens 2:df0c6af898ac 55 */
megrootens 0:caa8ee3bd882 56 bool has_power() {
megrootens 0:caa8ee3bd882 57 return power_;
megrootens 0:caa8ee3bd882 58 }
megrootens 0:caa8ee3bd882 59
megrootens 2:df0c6af898ac 60 /**
megrootens 2:df0c6af898ac 61 * Apply PWM value in [-1,1] to motor
megrootens 2:df0c6af898ac 62 * Direction changes accordingly
megrootens 2:df0c6af898ac 63 * Only has effect when this.has_power()
megrootens 2:df0c6af898ac 64 */
megrootens 0:caa8ee3bd882 65 void set_pwm(double pwm) {
megrootens 2:df0c6af898ac 66 // only when allowed
megrootens 0:caa8ee3bd882 67 if (power_) {
megrootens 0:caa8ee3bd882 68 if (pwm<0) {
megrootens 0:caa8ee3bd882 69 dir_ = kInvertDir_;
megrootens 0:caa8ee3bd882 70 } else {
megrootens 0:caa8ee3bd882 71 dir_ = !kInvertDir_;
megrootens 0:caa8ee3bd882 72 }
megrootens 0:caa8ee3bd882 73 if (abs(pwm)>1) {
megrootens 0:caa8ee3bd882 74 speed_ = 1;
megrootens 0:caa8ee3bd882 75 } else {
megrootens 0:caa8ee3bd882 76 speed_ = abs(pwm);
megrootens 0:caa8ee3bd882 77 }
megrootens 0:caa8ee3bd882 78 }
megrootens 0:caa8ee3bd882 79 }
megrootens 0:caa8ee3bd882 80
megrootens 2:df0c6af898ac 81 /**
megrootens 2:df0c6af898ac 82 * Get motor angle [deg]
megrootens 2:df0c6af898ac 83 */
megrootens 0:caa8ee3bd882 84 double get_angle() {
megrootens 0:caa8ee3bd882 85 return offset_ + 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
megrootens 0:caa8ee3bd882 86 }
megrootens 0:caa8ee3bd882 87
megrootens 2:df0c6af898ac 88 /**
megrootens 2:df0c6af898ac 89 * Set the current motor angle to a given value [deg] (stored in offset)
megrootens 2:df0c6af898ac 90 */
megrootens 0:caa8ee3bd882 91 void set_angle(double angle) {
megrootens 0:caa8ee3bd882 92 offset_ = angle - 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
megrootens 0:caa8ee3bd882 93 }
megrootens 0:caa8ee3bd882 94
megrootens 0:caa8ee3bd882 95 private:
megrootens 0:caa8ee3bd882 96
megrootens 2:df0c6af898ac 97 // Encoder, direction pin, pwm pin
megrootens 0:caa8ee3bd882 98 QEI encoder_;
megrootens 0:caa8ee3bd882 99 DigitalOut dir_;
megrootens 0:caa8ee3bd882 100 FastPWM speed_;
megrootens 2:df0c6af898ac 101
megrootens 2:df0c6af898ac 102 // encoder/gear box constants
megrootens 0:caa8ee3bd882 103 const int kPulsesPerRev_;
megrootens 0:caa8ee3bd882 104 const bool kInvertDir_;
megrootens 2:df0c6af898ac 105
megrootens 2:df0c6af898ac 106 // allowed to move
megrootens 0:caa8ee3bd882 107 bool power_;
megrootens 2:df0c6af898ac 108
megrootens 2:df0c6af898ac 109 // internal offset s.t. the angle can be set.
megrootens 0:caa8ee3bd882 110 double offset_;
megrootens 0:caa8ee3bd882 111
megrootens 2:df0c6af898ac 112 // get encoder pulses; only for internal use
megrootens 0:caa8ee3bd882 113 int get_pulses() {
megrootens 0:caa8ee3bd882 114 return kInvertDir_?-encoder_.getPulses():encoder_.getPulses();
megrootens 0:caa8ee3bd882 115 }
megrootens 0:caa8ee3bd882 116
megrootens 0:caa8ee3bd882 117 };
megrootens 0:caa8ee3bd882 118
megrootens 2:df0c6af898ac 119 #endif