File content as of revision 9:1d9b24d7ac77:

#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(): 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 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 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);
        }

};

#endif