Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Escon/Motor.h@20:f13b949b623b, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
18:0cfe72d8a006 
publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 8:1a3a69fecedf 1 #ifndef Motor_h
amandaghassaei 8:1a3a69fecedf 2 #define Motor_h
amandaghassaei 8:1a3a69fecedf 3
amandaghassaei 9:1d9b24d7ac77 4 #include "mbed.h"
amandaghassaei 9:1d9b24d7ac77 5 //#include "qeihw.h"
amandaghassaei 10:769cc457c3a4 6 #include "qeihw.h"
amandaghassaei 9:1d9b24d7ac77 7 #define ENCODER_RES 1024
amandaghassaei 9:1d9b24d7ac77 8 #define M_PI 3.14159265358979323846
amandaghassaei 9:1d9b24d7ac77 9 #define Kt 0.0534// [ Nm/A ]
amandaghassaei 9:1d9b24d7ac77 10
amandaghassaei 11:711d3c207e8c 11 #define PWM_MAX 0.4
amandaghassaei 10:769cc457c3a4 12 #define PPR 1024
amandaghassaei 10:769cc457c3a4 13
amandaghassaei 9:1d9b24d7ac77 14
amandaghassaei 8:1a3a69fecedf 15 class Motor {
amandaghassaei 8:1a3a69fecedf 16
amandaghassaei 8:1a3a69fecedf 17 public:
amandaghassaei 8:1a3a69fecedf 18
amandaghassaei 10:769cc457c3a4 19 Motor(): motorEN(p25), motorPWM(p26), motorCurrent(p20), encoder(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE)
amandaghassaei 9:1d9b24d7ac77 20 {
amandaghassaei 18:0cfe72d8a006 21 _gearRatio = 9.629;
amandaghassaei 18:0cfe72d8a006 22 _maxCurrent = 15.0;
amandaghassaei 11:711d3c207e8c 23 _pwmSlope = -(0.9 - 0.1) / (_maxCurrent + _maxCurrent); // slope for desired current to PWM
amandaghassaei 9:1d9b24d7ac77 24
amandaghassaei 9:1d9b24d7ac77 25 motorPWM.period_us(200); // set motor PWM 5kHz (this is max val)
amandaghassaei 9:1d9b24d7ac77 26 motorEN.write(0); // turn off motor driver (high active)
amandaghassaei 9:1d9b24d7ac77 27 motorPWM.write(0.5f); // zero current to motor, coasting mode
amandaghassaei 10:769cc457c3a4 28 encoder.Reset(QEI_RESET_POS); // clear the encoder
amandaghassaei 10:769cc457c3a4 29 setupEncoder();
amandaghassaei 9:1d9b24d7ac77 30 motorEnable();
amandaghassaei 9:1d9b24d7ac77 31 };
amandaghassaei 9:1d9b24d7ac77 32
amandaghassaei 18:0cfe72d8a006 33 void setGearRatio(float gearRatio){
amandaghassaei 18:0cfe72d8a006 34 _gearRatio = gearRatio;
amandaghassaei 18:0cfe72d8a006 35 }
amandaghassaei 18:0cfe72d8a006 36
amandaghassaei 10:769cc457c3a4 37 void setupEncoder()
amandaghassaei 10:769cc457c3a4 38 {
amandaghassaei 10:769cc457c3a4 39 encoder.SetDigiFilter(480UL);
amandaghassaei 10:769cc457c3a4 40 encoder.SetMaxPosition(0xFFFFFFFF);
amandaghassaei 11:711d3c207e8c 41 encoder.SetVelocityTimerReload(1000000);
amandaghassaei 10:769cc457c3a4 42 }
amandaghassaei 10:769cc457c3a4 43
amandaghassaei 9:1d9b24d7ac77 44 void setPC(Serial *pc){
amandaghassaei 9:1d9b24d7ac77 45 _pc = pc;
amandaghassaei 9:1d9b24d7ac77 46 }
amandaghassaei 8:1a3a69fecedf 47
amandaghassaei 8:1a3a69fecedf 48 float getTheta(){
amandaghassaei 10:769cc457c3a4 49 int32_t counts = encoder.GetPosition();
amandaghassaei 18:0cfe72d8a006 50 float angle = (float)counts / (4.0*PPR) * 2.0 * M_PI / _gearRatio;// - 2.583;
amandaghassaei 10:769cc457c3a4 51 return angle;
amandaghassaei 8:1a3a69fecedf 52 }
amandaghassaei 8:1a3a69fecedf 53
amandaghassaei 8:1a3a69fecedf 54 float getDTheta(){
amandaghassaei 18:0cfe72d8a006 55 int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4.0*PPR );
amandaghassaei 18:0cfe72d8a006 56 float angularVel = countVel * 2.0 * M_PI / 60.0 / _gearRatio;
amandaghassaei 18:0cfe72d8a006 57 if (encoder.Direction()) angularVel *= -1.0;
amandaghassaei 10:769cc457c3a4 58 return angularVel;
amandaghassaei 9:1d9b24d7ac77 59 }
amandaghassaei 9:1d9b24d7ac77 60
amandaghassaei 9:1d9b24d7ac77 61 void setTorque(float desTorque){
amandaghassaei 9:1d9b24d7ac77 62 // Desired torque should be signed.
amandaghassaei 9:1d9b24d7ac77 63 // There is no direction pin on this controller, instead,
amandaghassaei 9:1d9b24d7ac77 64 // current is defined by a PWM % centered at 0.5, 0.1 is full reverse, 0.9 is full foward
amandaghassaei 18:0cfe72d8a006 65 float desCurrent = (desTorque*1.04/_gearRatio)/Kt;//104% bump to make up for gear efficiency
amandaghassaei 9:1d9b24d7ac77 66 float pwm = _pwmSlope * desCurrent + 0.5f; // corrected pwm range
amandaghassaei 9:1d9b24d7ac77 67
amandaghassaei 9:1d9b24d7ac77 68 // check bounds on current output
amandaghassaei 10:769cc457c3a4 69 if (pwm < (0.5f-PWM_MAX)) pwm = 0.5f-PWM_MAX;
amandaghassaei 10:769cc457c3a4 70 else if (pwm > (0.5f+PWM_MAX)) pwm = 0.5f+PWM_MAX;
amandaghassaei 9:1d9b24d7ac77 71
amandaghassaei 9:1d9b24d7ac77 72 //set motor current
amandaghassaei 9:1d9b24d7ac77 73 motorPWM.write(pwm);
amandaghassaei 9:1d9b24d7ac77 74 _pwm = pwm;
amandaghassaei 9:1d9b24d7ac77 75 // _pc->printf("motor PWM Command: %f\n", pwm);
amandaghassaei 9:1d9b24d7ac77 76 }
amandaghassaei 9:1d9b24d7ac77 77
amandaghassaei 9:1d9b24d7ac77 78 float getTorque(){
amandaghassaei 18:0cfe72d8a006 79 return Kt * motorCurrent.read()*10.0 * _gearRatio;//10 translates V to pwm max
amandaghassaei 9:1d9b24d7ac77 80 }
amandaghassaei 9:1d9b24d7ac77 81
amandaghassaei 10:769cc457c3a4 82 float getPWM(){
amandaghassaei 10:769cc457c3a4 83 return _pwm;
amandaghassaei 8:1a3a69fecedf 84 }
amandaghassaei 10:769cc457c3a4 85
amandaghassaei 8:1a3a69fecedf 86
amandaghassaei 8:1a3a69fecedf 87
amandaghassaei 8:1a3a69fecedf 88 private:
amandaghassaei 8:1a3a69fecedf 89
amandaghassaei 9:1d9b24d7ac77 90 Serial *_pc;
amandaghassaei 9:1d9b24d7ac77 91
amandaghassaei 9:1d9b24d7ac77 92 float _maxCurrent;
amandaghassaei 9:1d9b24d7ac77 93 float _pwmSlope;
amandaghassaei 9:1d9b24d7ac77 94
amandaghassaei 18:0cfe72d8a006 95 float _gearRatio;
amandaghassaei 18:0cfe72d8a006 96
amandaghassaei 9:1d9b24d7ac77 97 float _pwm;
amandaghassaei 9:1d9b24d7ac77 98
amandaghassaei 9:1d9b24d7ac77 99 DigitalOut motorEN;// enable motor, high is Enables
amandaghassaei 9:1d9b24d7ac77 100 PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A
amandaghassaei 9:1d9b24d7ac77 101 AnalogIn motorCurrent;// "Actual Current" from ESCON
amandaghassaei 8:1a3a69fecedf 102
amandaghassaei 10:769cc457c3a4 103 QEIHW encoder;
amandaghassaei 9:1d9b24d7ac77 104
amandaghassaei 9:1d9b24d7ac77 105
amandaghassaei 9:1d9b24d7ac77 106 void motorEnable(){
amandaghassaei 9:1d9b24d7ac77 107 motorEN.write(1);
amandaghassaei 9:1d9b24d7ac77 108 }
amandaghassaei 9:1d9b24d7ac77 109
amandaghassaei 9:1d9b24d7ac77 110 void motorDisable(){
amandaghassaei 9:1d9b24d7ac77 111 motorEN.write(0);
amandaghassaei 9:1d9b24d7ac77 112 }
amandaghassaei 8:1a3a69fecedf 113
amandaghassaei 8:1a3a69fecedf 114 };
amandaghassaei 8:1a3a69fecedf 115
amandaghassaei 8:1a3a69fecedf 116 #endif