bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Escon/Motor.h@18:0cfe72d8a006, 2015-12-11 (annotated)
- Committer:
- amandaghassaei
- Date:
- Fri Dec 11 06:16:33 2015 +0000
- Revision:
- 18:0cfe72d8a006
- Parent:
- 17:8a0e647cf551
in progress
Who changed what in which revision?
User | Revision | Line number | New 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 |