bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Escon/Motor.h
- Revision:
- 10:769cc457c3a4
- Parent:
- 9:1d9b24d7ac77
- Child:
- 11:711d3c207e8c
--- a/Escon/Motor.h Sat Dec 05 00:40:42 2015 +0000 +++ b/Escon/Motor.h Sat Dec 05 09:04:23 2015 +0000 @@ -3,17 +3,20 @@ #include "mbed.h" //#include "qeihw.h" -#include "QEI.h" +#include "qeihw.h" #define ENCODER_RES 1024 #define M_PI 3.14159265358979323846 #define Kt 0.0534// [ Nm/A ] +#define PWM_MAX 0.2 +#define PPR 1024 + class Motor { public: - 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) + Motor(): motorEN(p25), motorPWM(p26), motorCurrent(p20), 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 @@ -21,24 +24,33 @@ 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(); + encoder.Reset(QEI_RESET_POS); // clear the encoder + setupEncoder(); motorEnable(); }; + void setupEncoder() + { + encoder.SetDigiFilter(480UL); + encoder.SetMaxPosition(0xFFFFFFFF); + encoder.SetVelocityTimerReload_us(1000); + } + void setPC(Serial *pc){ _pc = pc; } float getTheta(){ - // 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() + int32_t counts = encoder.GetPosition(); + float angle = (float)counts / (4*PPR) * 2 * M_PI; + return angle; } float getDTheta(){ -// 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() + int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4*PPR ); + float angularVel = countVel * 2 * M_PI / 60; + if (encoder.Direction()) angularVel *= -1; + return angularVel; } void setTorque(float desTorque){ @@ -49,8 +61,8 @@ 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; + if (pwm < (0.5f-PWM_MAX)) pwm = 0.5f-PWM_MAX; + else if (pwm > (0.5f+PWM_MAX)) pwm = 0.5f+PWM_MAX; //set motor current motorPWM.write(pwm); @@ -59,12 +71,13 @@ } float getTorque(){ - return Kt * motorCurrent.read(); + return Kt * motorCurrent.read()*10; } - void printPWM(){ - _pc->printf("%f\n", _pwm); + float getPWM(){ + return _pwm; } + private: @@ -80,7 +93,7 @@ PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A AnalogIn motorCurrent;// "Actual Current" from ESCON - //QEI encoder; + QEIHW encoder; void motorEnable(){