![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Escon/Motor.h
- Revision:
- 9:1d9b24d7ac77
- Parent:
- 8:1a3a69fecedf
- Child:
- 10:769cc457c3a4
--- a/Escon/Motor.h Thu Dec 03 23:55:44 2015 +0000 +++ b/Escon/Motor.h Sat Dec 05 00:40:42 2015 +0000 @@ -1,24 +1,95 @@ #ifndef Motor_h #define Motor_h +#include "mbed.h" +//#include "qeihw.h" +#include "QEI.h" +#define ENCODER_RES 1024 +#define M_PI 3.14159265358979323846 +#define Kt 0.0534// [ Nm/A ] + + class Motor { public: - Motor(){}; + 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) + { + _maxCurrent = 10; + _pwmSlope = (0.9 - 0.1) / (_maxCurrent + _maxCurrent); // slope for desired current to PWM + + motorPWM.period_us(200); // set motor PWM 5kHz (this is max val) + motorEN.write(0); // turn off motor driver (high active) + motorPWM.write(0.5f); // zero current to motor, coasting mode +// encoder.Reset(QEI_RESET_POS); // clear the encoder + //encoder.reset(); + motorEnable(); + }; + + void setPC(Serial *pc){ + _pc = pc; + } float getTheta(){ - return 0.0; + // Return angle in radians +// return float( encoder.GetPosition() ) / ( 1 * 4 * float(ENCODER_RES) ) * 2.0f * M_PI; + return float( 0 ) / ( 1 * 2 * float(ENCODER_RES) ) * 2.0f * M_PI;//encoder.getPulses() } float getDTheta(){ - return 0.0; +// return encoder.GetVelocity() / ( 1 * 4 * float(ENCODER_RES) ) * 2.0f * M_PI; + return 0 / ( 1 * 2 * float(ENCODER_RES) ) * 2.0f * M_PI;//encoder.getVelocity() + } + + void setTorque(float desTorque){ + // Desired torque should be signed. + // There is no direction pin on this controller, instead, + // current is defined by a PWM % centered at 0.5, 0.1 is full reverse, 0.9 is full foward + float desCurrent = (desTorque / Kt); + float pwm = _pwmSlope * desCurrent + 0.5f; // corrected pwm range + + // check bounds on current output + if (pwm < 0.1f) pwm = 0.1f; + else if (pwm > 0.9f) pwm = 0.9f; + + //set motor current + motorPWM.write(pwm); + _pwm = pwm; +// _pc->printf("motor PWM Command: %f\n", pwm); + } + + float getTorque(){ + return Kt * motorCurrent.read(); + } + + void printPWM(){ + _pc->printf("%f\n", _pwm); } private: + Serial *_pc; + + float _maxCurrent; + float _pwmSlope; + + float _pwm; + + DigitalOut motorEN;// enable motor, high is Enables + PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A + AnalogIn motorCurrent;// "Actual Current" from ESCON + //QEI encoder; + + + void motorEnable(){ + motorEN.write(1); + } + + void motorDisable(){ + motorEN.write(0); + } };