Motor.h

00001 #ifndef Motor_h
00002 #define Motor_h
00003 
00004 #include "mbed.h"
00005 //#include "qeihw.h"
00006 #include "qeihw.h"
00007 #define ENCODER_RES 1024
00008 #define M_PI 3.14159265358979323846
00009 #define Kt 0.0534// [ Nm/A ]
00010 
00011 #define PWM_MAX 0.4
00012 #define PPR 1024
00013 
00014 
00015 class Motor {
00016     
00017     public:
00018     
00019         Motor(): motorEN(p25), motorPWM(p26), motorCurrent(p20), encoder(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE)
00020         {
00021             _gearRatio = 9.629;
00022             _maxCurrent = 15.0;
00023             _pwmSlope = -(0.9 - 0.1) / (_maxCurrent + _maxCurrent);   // slope for desired current to PWM
00024             
00025             motorPWM.period_us(200);     // set motor PWM 5kHz (this is max val)
00026             motorEN.write(0);            // turn off motor driver (high active)
00027             motorPWM.write(0.5f);        // zero current to motor, coasting mode
00028             encoder.Reset(QEI_RESET_POS);             // clear the encoder
00029             setupEncoder();
00030             motorEnable();
00031         };
00032         
00033         void setGearRatio(float gearRatio){
00034             _gearRatio = gearRatio;
00035         }
00036         
00037         void setupEncoder()
00038         {   
00039             encoder.SetDigiFilter(480UL);
00040             encoder.SetMaxPosition(0xFFFFFFFF);
00041             encoder.SetVelocityTimerReload(1000000);
00042         } 
00043         
00044         void setPC(Serial *pc){
00045             _pc = pc;
00046         }
00047         
00048         float getTheta(){
00049             int32_t counts = encoder.GetPosition();    
00050             float angle = (float)counts / (4.0*PPR) * 2.0 * M_PI / _gearRatio;// - 2.583;
00051             return angle;
00052         }
00053         
00054         float getDTheta(){
00055             int32_t countVel = encoder.CalculateRPM( encoder.GetVelocityCap(), 4.0*PPR );
00056             float angularVel = countVel * 2.0 * M_PI / 60.0 / _gearRatio;
00057             if (encoder.Direction()) angularVel *= -1.0;
00058             return angularVel; 
00059         }
00060         
00061         void setTorque(float desTorque){
00062             // Desired torque should be signed. 
00063             // There is no direction pin on this controller, instead,
00064             // current is defined by a PWM % centered at 0.5, 0.1 is full reverse, 0.9 is full foward
00065             float desCurrent = (desTorque*1.04/_gearRatio)/Kt;//104% bump to make up for gear efficiency
00066             float pwm = _pwmSlope * desCurrent + 0.5f;    // corrected pwm range
00067            
00068             // check bounds on current output
00069             if (pwm < (0.5f-PWM_MAX)) pwm = 0.5f-PWM_MAX;
00070             else if (pwm > (0.5f+PWM_MAX)) pwm = 0.5f+PWM_MAX;
00071                
00072             //set motor current
00073             motorPWM.write(pwm);
00074             _pwm = pwm;
00075 //            _pc->printf("motor PWM Command: %f\n", pwm);
00076         }
00077         
00078         float getTorque(){
00079             return Kt * motorCurrent.read()*10.0 * _gearRatio;//10 translates V to pwm max
00080         }
00081         
00082         float getPWM(){
00083             return _pwm;
00084         }
00085         
00086     
00087     
00088     private:
00089     
00090         Serial *_pc;
00091     
00092         float _maxCurrent;
00093         float _pwmSlope;
00094         
00095         float _gearRatio;
00096         
00097         float _pwm;
00098         
00099         DigitalOut motorEN;// enable motor, high is Enables
00100         PwmOut motorPWM;// Motor PWM output, 0.1 <-> 0.9 = -10A <-> +10A
00101         AnalogIn motorCurrent;// "Actual Current" from ESCON
00102 
00103         QEIHW encoder;
00104         
00105         
00106         void motorEnable(){
00107             motorEN.write(1);
00108         }
00109         
00110         void motorDisable(){
00111             motorEN.write(0);
00112         }
00113 
00114 };
00115 
00116 #endif