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(){