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);
+        }
 
 };