MotCon.cpp

00001 // J. Bradshaw - Motor Control Libary for support of most H-Bridge drivers with
00002 //  a single PWM (enable active high) pin and one or two direction pins
00003 //  to support forward/reverse (complimentary)
00004 #include "mbed.h"
00005 #include "MotCon.h"
00006 
00007 //Constructor
00008 MotCon::MotCon(PinName pwm_pin, PinName dir1_pin) : _pwm_pin(pwm_pin), _dir1_pin(dir1_pin), _dir2_pin(NC) {
00009     _dir2 = false;
00010     _pwm_pin.period_us(50);
00011     _pwm_pin = 0.0;
00012     _dir1_pin = 0;
00013 }
00014 MotCon::MotCon(PinName pwm_pin, PinName dir1_pin, PinName dir2_pin) : _pwm_pin(pwm_pin), _dir1_pin(dir1_pin), _dir2_pin(dir2_pin) {
00015     _dir2 = true;
00016     _pwm_pin.period_us(50);
00017     _pwm_pin = 0.0;
00018     _dir1_pin = 0;
00019     _dir2_pin = 0;
00020     
00021     mc_mode = 0;    //mode pin determines braking (1 = dynamic braking, 0 = free-wheeling)    
00022 }
00023 // dc is signed duty cycle (+/-1.0)
00024 void MotCon::mot_control(float dc){    
00025     if(dc>1.0)
00026         dc=1.0;
00027     if(dc<-1.0)
00028         dc=-1.0;
00029             
00030     if(_dir2){            
00031         if(dc > 0.001){
00032             _dir1_pin = 0;
00033             _dir2_pin = 1;
00034             _pwm_pin = dc;
00035         }
00036         else if(dc < -0.001){
00037             _dir2_pin = 0;
00038             _dir1_pin = 1;        
00039             _pwm_pin = abs(dc);
00040         }
00041         else{
00042             if(mc_mode){
00043                 _dir1_pin = 0;
00044                 _dir2_pin = 0;
00045                 _pwm_pin = 1.0;                
00046             }
00047             else{
00048                 _dir1_pin = 0;
00049                 _dir2_pin = 0;
00050                 _pwm_pin = 0.0;
00051             }
00052         }         
00053     }
00054     else{            
00055         if(dc > 0.001){
00056             _dir1_pin = 0;
00057             _pwm_pin = dc;
00058         }
00059         else if(dc < -0.001){
00060             _dir1_pin = 1;
00061             _pwm_pin = abs(dc);
00062         }
00063         else{
00064             _dir1_pin = 0;
00065             _pwm_pin = 0.0;
00066         }             
00067     }
00068 }
00069 
00070 // dc is signed duty cycle (+/-1.0)
00071 void MotCon::mot_control(float dc, int invert){
00072     if(dc>1.0)
00073         dc=1.0;
00074     if(dc<-1.0)
00075         dc=-1.0;
00076                 
00077     if(_dir2){
00078         if(invert==0){
00079             if(dc > 0.001){
00080                 _dir1_pin = 0;
00081                 _dir2_pin = 1;
00082                 _pwm_pin = dc;
00083             }
00084             else if(dc < -0.001){
00085                 _dir2_pin = 0;
00086                 _dir1_pin = 1;
00087                 _pwm_pin = abs(dc);
00088             }
00089             else{
00090                 if(mc_mode){
00091                     _dir1_pin = 0;
00092                     _dir2_pin = 0;
00093                     _pwm_pin = 1.0;                
00094                 }
00095                 else{
00096                     _dir1_pin = 0;
00097                     _dir2_pin = 0;
00098                     _pwm_pin = 0.0;
00099                 }
00100             }
00101         }
00102         else{
00103             if(dc > 0.001){
00104                 _dir2_pin = 0;
00105                 _dir1_pin = 1;
00106                 _pwm_pin = dc;
00107             }
00108             else if(dc < -0.001){
00109                 _dir1_pin = 0;
00110                 _dir2_pin = 1;
00111                 _pwm_pin = abs(dc);
00112             }
00113             else{
00114                 if(mc_mode){
00115                     _dir1_pin = 0;
00116                     _dir2_pin = 0;
00117                     _pwm_pin = 1.0;                
00118                 }
00119                 else{
00120                     _dir1_pin = 0;
00121                     _dir2_pin = 0;
00122                     _pwm_pin = 0.0;
00123                 }
00124             }
00125         }
00126     }
00127     else{        
00128         if(invert==0){
00129             if(dc > 0.001){
00130                 _dir1_pin = 0;
00131                 _pwm_pin = dc;
00132             }
00133             else if(dc < -0.001){
00134                 _dir1_pin = 1;
00135                 _pwm_pin = abs(dc);
00136             }
00137             else{
00138                 _dir1_pin = 0;
00139                 _pwm_pin = 0.0;
00140             }
00141         }
00142         else{
00143             if(dc > 0.001){
00144                 _dir1_pin = 1;
00145                 _pwm_pin = dc;
00146             }
00147             else if(dc < -0.001){
00148                 _dir1_pin = 0;
00149                 _pwm_pin = abs(dc);
00150             }
00151             else{
00152                 _dir1_pin = 0;
00153                 _pwm_pin = 0.0;
00154             }
00155         }    
00156     }    
00157 }
00158 
00159 void MotCon::setMode(int mode){
00160     mc_mode = mode;
00161 }
00162 
00163 int MotCon::getMode(void){
00164     return mc_mode;
00165 }
00166 
00167 float MotCon::read(void){
00168     float ret = this->duty_cycle;
00169 
00170     return ret;
00171 }
00172 
00173 MotCon& MotCon::operator= (float value){
00174     mot_control(value);
00175     
00176     return *this;
00177 }
00178 
00179 MotCon& MotCon::operator= (MotCon& rhs) {
00180   // Underlying call is thread safe
00181   mot_control(rhs.read());
00182   
00183   return *this;
00184 }
00185   
00186 MotCon::operator float(){
00187 // Underlying call is thread safe
00188    return this->read(); 
00189 }