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