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