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@17:8a0e647cf551, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 00:44:45 2015 +0000
Revision:
17:8a0e647cf551
Parent:
11:711d3c207e8c
Child:
18:0cfe72d8a006
eod

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 17:8a0e647cf551 21 _maxCurrent = 15;
amandaghassaei 11:711d3c207e8c 22 _pwmSlope = -(0.9 - 0.1) / (_maxCurrent + _maxCurrent); // slope for desired current to PWM
amandaghassaei 9:1d9b24d7ac77 23
amandaghassaei 9:1d9b24d7ac77 24 motorPWM.period_us(200); // set motor PWM 5kHz (this is max val)
amandaghassaei 9:1d9b24d7ac77 25 motorEN.write(0); // turn off motor driver (high active)
amandaghassaei 9:1d9b24d7ac77 26 motorPWM.write(0.5f); // zero current to motor, coasting mode
amandaghassaei 10:769cc457c3a4 27 encoder.Reset(QEI_RESET_POS); // clear the encoder
amandaghassaei 10:769cc457c3a4 28 setupEncoder();
amandaghassaei 9:1d9b24d7ac77 29 motorEnable();
amandaghassaei 9:1d9b24d7ac77 30 };
amandaghassaei 9:1d9b24d7ac77 31
amandaghassaei 10:769cc457c3a4 32 void setupEncoder()
amandaghassaei 10:769cc457c3a4 33 {
amandaghassaei 10:769cc457c3a4 34 encoder.SetDigiFilter(480UL);
amandaghassaei 10:769cc457c3a4 35 encoder.SetMaxPosition(0xFFFFFFFF);
amandaghassaei 11:711d3c207e8c 36 encoder.SetVelocityTimerReload(1000000);
amandaghassaei 10:769cc457c3a4 37 }
amandaghassaei 10:769cc457c3a4 38
amandaghassaei 9:1d9b24d7ac77 39 void setPC(Serial *pc){
amandaghassaei 9:1d9b24d7ac77 40 _pc = pc;
amandaghassaei 9:1d9b24d7ac77 41 }
amandaghassaei 8:1a3a69fecedf 42
amandaghassaei 8:1a3a69fecedf 43 float getTheta(){
amandaghassaei 10:769cc457c3a4 44 int32_t counts = encoder.GetPosition();
amandaghassaei 10:769cc457c3a4 45 float angle = (float)counts / (4*PPR) * 2 * M_PI;
amandaghassaei 10:769cc457c3a4 46 return angle;
amandaghassaei 8:1a3a69fecedf 47 }
amandaghassaei 8:1a3a69fecedf 48
amandaghassaei 8:1a3a69fecedf 49 float getDTheta(){
amandaghassaei 10:769cc457c3a4 50 int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4*PPR );
amandaghassaei 10:769cc457c3a4 51 float angularVel = countVel * 2 * M_PI / 60;
amandaghassaei 10:769cc457c3a4 52 if (encoder.Direction()) angularVel *= -1;
amandaghassaei 10:769cc457c3a4 53 return angularVel;
amandaghassaei 9:1d9b24d7ac77 54 }
amandaghassaei 9:1d9b24d7ac77 55
amandaghassaei 9:1d9b24d7ac77 56 void setTorque(float desTorque){
amandaghassaei 9:1d9b24d7ac77 57 // Desired torque should be signed.
amandaghassaei 9:1d9b24d7ac77 58 // There is no direction pin on this controller, instead,
amandaghassaei 9:1d9b24d7ac77 59 // current is defined by a PWM % centered at 0.5, 0.1 is full reverse, 0.9 is full foward
amandaghassaei 9:1d9b24d7ac77 60 float desCurrent = (desTorque / Kt);
amandaghassaei 9:1d9b24d7ac77 61 float pwm = _pwmSlope * desCurrent + 0.5f; // corrected pwm range
amandaghassaei 9:1d9b24d7ac77 62
amandaghassaei 9:1d9b24d7ac77 63 // check bounds on current output
amandaghassaei 10:769cc457c3a4 64 if (pwm < (0.5f-PWM_MAX)) pwm = 0.5f-PWM_MAX;
amandaghassaei 10:769cc457c3a4 65 else if (pwm > (0.5f+PWM_MAX)) pwm = 0.5f+PWM_MAX;
amandaghassaei 9:1d9b24d7ac77 66
amandaghassaei 9:1d9b24d7ac77 67 //set motor current
amandaghassaei 9:1d9b24d7ac77 68 motorPWM.write(pwm);
amandaghassaei 9:1d9b24d7ac77 69 _pwm = pwm;
amandaghassaei 9:1d9b24d7ac77 70 // _pc->printf("motor PWM Command: %f\n", pwm);
amandaghassaei 9:1d9b24d7ac77 71 }
amandaghassaei 9:1d9b24d7ac77 72
amandaghassaei 9:1d9b24d7ac77 73 float getTorque(){
amandaghassaei 10:769cc457c3a4 74 return Kt * motorCurrent.read()*10;
amandaghassaei 9:1d9b24d7ac77 75 }
amandaghassaei 9:1d9b24d7ac77 76
amandaghassaei 10:769cc457c3a4 77 float getPWM(){
amandaghassaei 10:769cc457c3a4 78 return _pwm;
amandaghassaei 8:1a3a69fecedf 79 }
amandaghassaei 10:769cc457c3a4 80
amandaghassaei 8:1a3a69fecedf 81
amandaghassaei 8:1a3a69fecedf 82
amandaghassaei 8:1a3a69fecedf 83 private:
amandaghassaei 8:1a3a69fecedf 84
amandaghassaei 9:1d9b24d7ac77 85 Serial *_pc;
amandaghassaei 9:1d9b24d7ac77 86
amandaghassaei 9:1d9b24d7ac77 87 float _maxCurrent;
amandaghassaei 9:1d9b24d7ac77 88 float _pwmSlope;
amandaghassaei 9:1d9b24d7ac77 89
amandaghassaei 9:1d9b24d7ac77 90 float _pwm;
amandaghassaei 9:1d9b24d7ac77 91
amandaghassaei 9:1d9b24d7ac77 92 DigitalOut motorEN;// enable motor, high is Enables
amandaghassaei 9:1d9b24d7ac77 93 PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A
amandaghassaei 9:1d9b24d7ac77 94 AnalogIn motorCurrent;// "Actual Current" from ESCON
amandaghassaei 8:1a3a69fecedf 95
amandaghassaei 10:769cc457c3a4 96 QEIHW encoder;
amandaghassaei 9:1d9b24d7ac77 97
amandaghassaei 9:1d9b24d7ac77 98
amandaghassaei 9:1d9b24d7ac77 99 void motorEnable(){
amandaghassaei 9:1d9b24d7ac77 100 motorEN.write(1);
amandaghassaei 9:1d9b24d7ac77 101 }
amandaghassaei 9:1d9b24d7ac77 102
amandaghassaei 9:1d9b24d7ac77 103 void motorDisable(){
amandaghassaei 9:1d9b24d7ac77 104 motorEN.write(0);
amandaghassaei 9:1d9b24d7ac77 105 }
amandaghassaei 8:1a3a69fecedf 106
amandaghassaei 8:1a3a69fecedf 107 };
amandaghassaei 8:1a3a69fecedf 108
amandaghassaei 8:1a3a69fecedf 109 #endif