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@0:caa8ee3bd882, 2017-11-12 (annotated)

Committer:
megrootens
Date:
Sun Nov 12 00:14:05 2017 +0000
Revision:
0:caa8ee3bd882
Child:
2:df0c6af898ac
setup code. too much play in gears and axles slip. But performance already better than bio robotics.

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 0:caa8ee3bd882 7 #define PWM_PERIOD_US 10
megrootens 0:caa8ee3bd882 8
megrootens 0:caa8ee3bd882 9 class Motor
megrootens 0:caa8ee3bd882 10 {
megrootens 0:caa8ee3bd882 11 public:
megrootens 0:caa8ee3bd882 12 Motor(PinName pwm, PinName dir, PinName enc_a, PinName enc_b,
megrootens 0:caa8ee3bd882 13 int pulses_per_rev, bool invert_dir = false) :
megrootens 0:caa8ee3bd882 14 encoder_(enc_a, enc_b, NC, NULL, QEI::X4_ENCODING),
megrootens 0:caa8ee3bd882 15 dir_(dir),
megrootens 0:caa8ee3bd882 16 speed_(pwm),
megrootens 0:caa8ee3bd882 17 kPulsesPerRev_(pulses_per_rev),
megrootens 0:caa8ee3bd882 18 kInvertDir_(invert_dir)
megrootens 0:caa8ee3bd882 19 {
megrootens 0:caa8ee3bd882 20 Stop();
megrootens 0:caa8ee3bd882 21 offset_ = 0;
megrootens 0:caa8ee3bd882 22 speed_.period_us(PWM_PERIOD_US);
megrootens 0:caa8ee3bd882 23 }
megrootens 0:caa8ee3bd882 24
megrootens 0:caa8ee3bd882 25 void Stop() {
megrootens 0:caa8ee3bd882 26 power_ = false;
megrootens 0:caa8ee3bd882 27 dir_=0;
megrootens 0:caa8ee3bd882 28 speed_=0;
megrootens 0:caa8ee3bd882 29 }
megrootens 0:caa8ee3bd882 30
megrootens 0:caa8ee3bd882 31 void Start() {
megrootens 0:caa8ee3bd882 32 power_ = true;
megrootens 0:caa8ee3bd882 33 }
megrootens 0:caa8ee3bd882 34
megrootens 0:caa8ee3bd882 35 bool has_power() {
megrootens 0:caa8ee3bd882 36 return power_;
megrootens 0:caa8ee3bd882 37 }
megrootens 0:caa8ee3bd882 38
megrootens 0:caa8ee3bd882 39 void set_pwm(double pwm) {
megrootens 0:caa8ee3bd882 40 if (power_) {
megrootens 0:caa8ee3bd882 41 if (pwm<0) {
megrootens 0:caa8ee3bd882 42 dir_ = kInvertDir_;
megrootens 0:caa8ee3bd882 43 } else {
megrootens 0:caa8ee3bd882 44 dir_ = !kInvertDir_;
megrootens 0:caa8ee3bd882 45 }
megrootens 0:caa8ee3bd882 46 if (abs(pwm)>1) {
megrootens 0:caa8ee3bd882 47 speed_ = 1;
megrootens 0:caa8ee3bd882 48 } else {
megrootens 0:caa8ee3bd882 49 speed_ = abs(pwm);
megrootens 0:caa8ee3bd882 50 }
megrootens 0:caa8ee3bd882 51 }
megrootens 0:caa8ee3bd882 52 }
megrootens 0:caa8ee3bd882 53
megrootens 0:caa8ee3bd882 54 double get_angle() {
megrootens 0:caa8ee3bd882 55 return offset_ + 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
megrootens 0:caa8ee3bd882 56 }
megrootens 0:caa8ee3bd882 57
megrootens 0:caa8ee3bd882 58 void set_angle(double angle) {
megrootens 0:caa8ee3bd882 59 offset_ = angle - 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
megrootens 0:caa8ee3bd882 60 }
megrootens 0:caa8ee3bd882 61
megrootens 0:caa8ee3bd882 62 private:
megrootens 0:caa8ee3bd882 63
megrootens 0:caa8ee3bd882 64 QEI encoder_;
megrootens 0:caa8ee3bd882 65 DigitalOut dir_;
megrootens 0:caa8ee3bd882 66 FastPWM speed_;
megrootens 0:caa8ee3bd882 67 const int kPulsesPerRev_;
megrootens 0:caa8ee3bd882 68 const bool kInvertDir_;
megrootens 0:caa8ee3bd882 69 bool power_;
megrootens 0:caa8ee3bd882 70 double offset_;
megrootens 0:caa8ee3bd882 71
megrootens 0:caa8ee3bd882 72 int get_pulses() {
megrootens 0:caa8ee3bd882 73 return kInvertDir_?-encoder_.getPulses():encoder_.getPulses();
megrootens 0:caa8ee3bd882 74 }
megrootens 0:caa8ee3bd882 75
megrootens 0:caa8ee3bd882 76 };
megrootens 0:caa8ee3bd882 77
megrootens 0:caa8ee3bd882 78 #endif