bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Escon/Motor.h
- Revision:
- 18:0cfe72d8a006
- Parent:
- 17:8a0e647cf551
--- a/Escon/Motor.h Fri Dec 11 00:44:45 2015 +0000 +++ b/Escon/Motor.h Fri Dec 11 06:16:33 2015 +0000 @@ -18,7 +18,8 @@ Motor(): motorEN(p25), motorPWM(p26), motorCurrent(p20), encoder(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE) { - _maxCurrent = 15; + _gearRatio = 9.629; + _maxCurrent = 15.0; _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) @@ -29,6 +30,10 @@ motorEnable(); }; + void setGearRatio(float gearRatio){ + _gearRatio = gearRatio; + } + void setupEncoder() { encoder.SetDigiFilter(480UL); @@ -42,14 +47,14 @@ float getTheta(){ int32_t counts = encoder.GetPosition(); - float angle = (float)counts / (4*PPR) * 2 * M_PI; + float angle = (float)counts / (4.0*PPR) * 2.0 * M_PI / _gearRatio;// - 2.583; return angle; } float getDTheta(){ - int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4*PPR ); - float angularVel = countVel * 2 * M_PI / 60; - if (encoder.Direction()) angularVel *= -1; + int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4.0*PPR ); + float angularVel = countVel * 2.0 * M_PI / 60.0 / _gearRatio; + if (encoder.Direction()) angularVel *= -1.0; return angularVel; } @@ -57,7 +62,7 @@ // 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 desCurrent = (desTorque*1.04/_gearRatio)/Kt;//104% bump to make up for gear efficiency float pwm = _pwmSlope * desCurrent + 0.5f; // corrected pwm range // check bounds on current output @@ -71,7 +76,7 @@ } float getTorque(){ - return Kt * motorCurrent.read()*10; + return Kt * motorCurrent.read()*10.0 * _gearRatio;//10 translates V to pwm max } float getPWM(){ @@ -87,6 +92,8 @@ float _maxCurrent; float _pwmSlope; + float _gearRatio; + float _pwm; DigitalOut motorEN;// enable motor, high is Enables