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?
User | Revision | Line number | New 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 |