bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
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?
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 | 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 |