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@9:1d9b24d7ac77, 2015-12-05 (annotated)

Committer:
amandaghassaei
Date:
Sat Dec 05 00:40:42 2015 +0000
Revision:
9:1d9b24d7ac77
Parent:
8:1a3a69fecedf
Child:
10:769cc457c3a4
basic motor pwm working

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 9:1d9b24d7ac77 6 #include "QEI.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 9:1d9b24d7ac77 11
amandaghassaei 8:1a3a69fecedf 12 class Motor {
amandaghassaei 8:1a3a69fecedf 13
amandaghassaei 8:1a3a69fecedf 14 public:
amandaghassaei 8:1a3a69fecedf 15
amandaghassaei 9:1d9b24d7ac77 16 Motor(): motorEN(p25), motorPWM(p26), motorCurrent(p20)//, encoder(p29, p30, NC, ENCODER_RES)//encoder(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE)
amandaghassaei 9:1d9b24d7ac77 17 {
amandaghassaei 9:1d9b24d7ac77 18 _maxCurrent = 10;
amandaghassaei 9:1d9b24d7ac77 19 _pwmSlope = (0.9 - 0.1) / (_maxCurrent + _maxCurrent); // slope for desired current to PWM
amandaghassaei 9:1d9b24d7ac77 20
amandaghassaei 9:1d9b24d7ac77 21 motorPWM.period_us(200); // set motor PWM 5kHz (this is max val)
amandaghassaei 9:1d9b24d7ac77 22 motorEN.write(0); // turn off motor driver (high active)
amandaghassaei 9:1d9b24d7ac77 23 motorPWM.write(0.5f); // zero current to motor, coasting mode
amandaghassaei 9:1d9b24d7ac77 24 // encoder.Reset(QEI_RESET_POS); // clear the encoder
amandaghassaei 9:1d9b24d7ac77 25 //encoder.reset();
amandaghassaei 9:1d9b24d7ac77 26 motorEnable();
amandaghassaei 9:1d9b24d7ac77 27 };
amandaghassaei 9:1d9b24d7ac77 28
amandaghassaei 9:1d9b24d7ac77 29 void setPC(Serial *pc){
amandaghassaei 9:1d9b24d7ac77 30 _pc = pc;
amandaghassaei 9:1d9b24d7ac77 31 }
amandaghassaei 8:1a3a69fecedf 32
amandaghassaei 8:1a3a69fecedf 33 float getTheta(){
amandaghassaei 9:1d9b24d7ac77 34 // Return angle in radians
amandaghassaei 9:1d9b24d7ac77 35 // return float( encoder.GetPosition() ) / ( 1 * 4 * float(ENCODER_RES) ) * 2.0f * M_PI;
amandaghassaei 9:1d9b24d7ac77 36 return float( 0 ) / ( 1 * 2 * float(ENCODER_RES) ) * 2.0f * M_PI;//encoder.getPulses()
amandaghassaei 8:1a3a69fecedf 37 }
amandaghassaei 8:1a3a69fecedf 38
amandaghassaei 8:1a3a69fecedf 39 float getDTheta(){
amandaghassaei 9:1d9b24d7ac77 40 // return encoder.GetVelocity() / ( 1 * 4 * float(ENCODER_RES) ) * 2.0f * M_PI;
amandaghassaei 9:1d9b24d7ac77 41 return 0 / ( 1 * 2 * float(ENCODER_RES) ) * 2.0f * M_PI;//encoder.getVelocity()
amandaghassaei 9:1d9b24d7ac77 42 }
amandaghassaei 9:1d9b24d7ac77 43
amandaghassaei 9:1d9b24d7ac77 44 void setTorque(float desTorque){
amandaghassaei 9:1d9b24d7ac77 45 // Desired torque should be signed.
amandaghassaei 9:1d9b24d7ac77 46 // There is no direction pin on this controller, instead,
amandaghassaei 9:1d9b24d7ac77 47 // current is defined by a PWM % centered at 0.5, 0.1 is full reverse, 0.9 is full foward
amandaghassaei 9:1d9b24d7ac77 48 float desCurrent = (desTorque / Kt);
amandaghassaei 9:1d9b24d7ac77 49 float pwm = _pwmSlope * desCurrent + 0.5f; // corrected pwm range
amandaghassaei 9:1d9b24d7ac77 50
amandaghassaei 9:1d9b24d7ac77 51 // check bounds on current output
amandaghassaei 9:1d9b24d7ac77 52 if (pwm < 0.1f) pwm = 0.1f;
amandaghassaei 9:1d9b24d7ac77 53 else if (pwm > 0.9f) pwm = 0.9f;
amandaghassaei 9:1d9b24d7ac77 54
amandaghassaei 9:1d9b24d7ac77 55 //set motor current
amandaghassaei 9:1d9b24d7ac77 56 motorPWM.write(pwm);
amandaghassaei 9:1d9b24d7ac77 57 _pwm = pwm;
amandaghassaei 9:1d9b24d7ac77 58 // _pc->printf("motor PWM Command: %f\n", pwm);
amandaghassaei 9:1d9b24d7ac77 59 }
amandaghassaei 9:1d9b24d7ac77 60
amandaghassaei 9:1d9b24d7ac77 61 float getTorque(){
amandaghassaei 9:1d9b24d7ac77 62 return Kt * motorCurrent.read();
amandaghassaei 9:1d9b24d7ac77 63 }
amandaghassaei 9:1d9b24d7ac77 64
amandaghassaei 9:1d9b24d7ac77 65 void printPWM(){
amandaghassaei 9:1d9b24d7ac77 66 _pc->printf("%f\n", _pwm);
amandaghassaei 8:1a3a69fecedf 67 }
amandaghassaei 8:1a3a69fecedf 68
amandaghassaei 8:1a3a69fecedf 69
amandaghassaei 8:1a3a69fecedf 70 private:
amandaghassaei 8:1a3a69fecedf 71
amandaghassaei 9:1d9b24d7ac77 72 Serial *_pc;
amandaghassaei 9:1d9b24d7ac77 73
amandaghassaei 9:1d9b24d7ac77 74 float _maxCurrent;
amandaghassaei 9:1d9b24d7ac77 75 float _pwmSlope;
amandaghassaei 9:1d9b24d7ac77 76
amandaghassaei 9:1d9b24d7ac77 77 float _pwm;
amandaghassaei 9:1d9b24d7ac77 78
amandaghassaei 9:1d9b24d7ac77 79 DigitalOut motorEN;// enable motor, high is Enables
amandaghassaei 9:1d9b24d7ac77 80 PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A
amandaghassaei 9:1d9b24d7ac77 81 AnalogIn motorCurrent;// "Actual Current" from ESCON
amandaghassaei 8:1a3a69fecedf 82
amandaghassaei 9:1d9b24d7ac77 83 //QEI encoder;
amandaghassaei 9:1d9b24d7ac77 84
amandaghassaei 9:1d9b24d7ac77 85
amandaghassaei 9:1d9b24d7ac77 86 void motorEnable(){
amandaghassaei 9:1d9b24d7ac77 87 motorEN.write(1);
amandaghassaei 9:1d9b24d7ac77 88 }
amandaghassaei 9:1d9b24d7ac77 89
amandaghassaei 9:1d9b24d7ac77 90 void motorDisable(){
amandaghassaei 9:1d9b24d7ac77 91 motorEN.write(0);
amandaghassaei 9:1d9b24d7ac77 92 }
amandaghassaei 8:1a3a69fecedf 93
amandaghassaei 8:1a3a69fecedf 94 };
amandaghassaei 8:1a3a69fecedf 95
amandaghassaei 8:1a3a69fecedf 96 #endif