MotorDriveDBH1.h

00001 /*
00002 
00003 Motor driver for DBH-1A/B/C series Motor controler (aliexpress, WINGXINE).
00004 This controller has two inputs for each H-Bridge, and an enable pin,
00005 and a current-sense.
00006 It also provides a 5v-ish output appropriate for running the MCU
00007 
00008 It was found that motors seemed to run a bit faster and more efficient
00009 with PWM on the Enable.  However, since spec claims only 0-98% PWM,
00010 We still wish to PWM the direction enable.  There might be a charge
00011 pump someplace that needs some down time.
00012 
00013 It seems to have a hard brake when both inputs set to 0, and enabled.
00014 However, it does seem to coast when enable is 0, inputs don't matter in this state.
00015 
00016 The goal of this driver is to PWM (mostly) EN when driving, but
00017 hard-brake on speed 0 and speed 0 transitions.
00018 
00019 */
00020 
00021 // Don't allow immediate change of direction.
00022 // let it "brake" for a little bit before starting in other direction
00023 #define MOTOR_STOP  0
00024 #define MOTOR_2STOP 1     // 1's is transition state indicator bit
00025 #define MOTOR_FWD   4
00026 #define MOTOR_REV   8
00027 #define MOTOR_2FWD  5
00028 #define MOTOR_2REV  9
00029 
00030 #ifndef ABS
00031 #define ABS(x)  (((x)<0)?(-(x)):(x))
00032 #endif
00033 
00034 // ----------- Global state classes:
00035 #include "MotorDriveBase.h"
00036 class MotorDrive : public MotorDriveBase
00037 {
00038 protected:
00039   static inline float getPWM(float pwm)
00040   {
00041     pwm = ABS(pwm);
00042     if (pwm >  1.0f) return(1.0f);
00043     if (pwm <= 0.0f) return(0.0f);
00044     return(pwm);
00045   }
00046 
00047   void setReverse(bool rev) // true==set reverse direction, false==forward
00048   {
00049     if (rev)
00050       {
00051         //digitalWrite(Pin.IN2,    0   );
00052         //analogWrite( Pin.IN1, _maxPWM);
00053         IN2.write(0.0f);
00054         IN1.write(_maxPWM);
00055       }
00056     else
00057       {
00058         //digitalWrite(Pin.IN1,    0   );
00059         //analogWrite( Pin.IN2, _maxPWM);
00060         IN1.write(0.0f);
00061         IN2.write(_maxPWM);
00062       }
00063   }
00064   
00065 public:
00066     // it does not seem to hurt to have IN1=IN2=1, but it doesn't seem
00067     // to do anything different/useful, so try to avoid this state.
00068   //struct {
00069     PwmOut IN1, IN2;  // forward/reverse selectors.
00070     PwmOut EN;        // enable pin
00071     AnalogIn CS;        // current sense analog input pin
00072   //} Pin;
00073   
00074   float _speed;  // current speed 0..1
00075   float _decel; // time to allow to stop, in ms / PWM frac
00076   unsigned char _mode;
00077   int _modeDoneTime;
00078   int _deadTime, _startupTime, _stopTime;
00079   float _minPWM, _maxPWM;
00080   int _msgCount;
00081   
00082   MotorDrive(PinName namEN, PinName namIN1, PinName namIN2, PinName namCS) :
00083      IN1(namIN1),
00084      IN2(namIN2),
00085      EN( namEN),
00086      CS( namCS)
00087   {
00088     EN.write(0.0f);
00089     IN2.write(0.0f);
00090     IN2.write(0.0f);
00091     
00092     IN1.period(0.001f);  // these can be audible.  They are just the little 98% limits the driver demands
00093     IN2.period(0.001f);
00094     EN.period(1.0f/8000.0f);  // may want this higher frequency to make it ultrasonic 
00095     
00096     _deadTime = 250; // emergency stop if no command update in this time interval
00097     _minPWM = 0.004f;     // Motors won't move below this level
00098     _maxPWM = 0.98f;     // don't go over this PWM level.  driver can't do it
00099     _startupTime = 5; // when starting from still, issue full power pulse this long to get motors started
00100     _stopTime = 3000;  // Pause at least this long after emergency stop before restarting
00101     _decel = 0.02f;   // frac. of fullspeed/ms decel time allowance      
00102 
00103     _msgCount = 11;  // issue this many diagnostic messages before going quiet
00104 
00105     emergencyStop();
00106   }
00107   
00108   void setCommandTimeout(int dt)
00109   {
00110       _deadTime = dt;
00111 cSerial.print("\r\nDeadman timeout ");cSerial.print(_deadTime);cSerial.println(" ms");
00112   }
00113 
00114   virtual void stop()
00115   {
00116     //analogWrite(Pin.IN1, 0);
00117     //analogWrite(Pin.IN2, 0);
00118     //abalogWrite(Pin.EN , 0);
00119     EN.write(0.0f);
00120     IN1.write(0.0f);
00121     IN2.write(0.0f);
00122     EN.write(1.0f);
00123 
00124     int stoppingTime = (int)ABS(_speed * _decel);
00125 if(_msgCount>0){_msgCount--;cSerial.print(stoppingTime);cSerial.println(" ms to stop.");}
00126     _modeDoneTime = millis() + stoppingTime;
00127     //speed=0;  don't clobber command in case of direction change
00128     _mode = MOTOR_2STOP;
00129   }
00130 
00131   virtual void emergencyStop()
00132   {
00133     cSerial.print("Emergency ");
00134     _msgCount = 11;  // turn on diagnostics for a few commands
00135     stop();
00136     _speed=0;
00137     _modeDoneTime += _stopTime;
00138   }
00139 /*
00140   void begin(const int en, const int in1, const int in2, const int cs,
00141          const float de=2.0)
00142   {
00143     pinMode(en,OUTPUT);   digitalWrite(en,0);
00144     Pin.EN  =en;
00145     Pin.IN1 =in1;
00146     Pin.IN2 =in2;
00147     Pin.CS  =cs;
00148     _decel=de;
00149 
00150     pinMode(Pin.IN1,OUTPUT);  digitalWrite(Pin.IN1,0);
00151     pinMode(Pin.IN2,OUTPUT);  digitalWrite(Pin.IN2,0);
00152     
00153     emergencyStop();
00154   }
00155 */
00156 
00157   // Set speed -MAX_PWM for max reverse, MAX_PWM for max forward
00158   virtual void setSpeed(const float spdReq, const long t)
00159   {
00160     unsigned char prevMode = _mode;
00161     bool rev;
00162     switch(prevMode)
00163       {
00164       case MOTOR_2STOP :
00165 if(_msgCount>0){_msgCount--;cSerial.print("2STOP-->");}
00166         if (t < _modeDoneTime)
00167           {  // make sure things are stopped
00168 //        digitalWrite(Pin.IN1,0);
00169 //        digitalWrite(Pin.IN2,0);
00170 //        digitalWrite(Pin.EN ,0);
00171             EN.write(0.0f);
00172             IN1.write(0.0f);
00173             IN2.write(0.0f);
00174             EN.write(1.0f);
00175             return;
00176           }
00177         // done stoping, continue to STOP mode
00178         _mode = MOTOR_STOP;
00179 if(_msgCount>0){_msgCount--;cSerial.println("stopped.");}
00180       case MOTOR_STOP :
00181 if(_msgCount>0){_msgCount--;cSerial.print("STOP-->");}
00182         if (ABS(spdReq) < _minPWM) return;
00183         _mode = (spdReq < 0) ? MOTOR_2REV : MOTOR_2FWD;
00184         rev = (_mode == MOTOR_2REV);
00185         //digitalWrite(rev?Pin.IN1:Pin.IN2,1); // don't worry about PWM
00186         //digitalWrite(rev?Pin.IN2:Pin.IN1,0); // this is transistional state
00187         //digitalWrite(Pin.EN,1);   // hard kick to get started
00188         IN1.write(rev?1.0f:0.0f);
00189         IN2.write(rev?0.0f:1.0f);
00190         EN.write(1.0f);  // hard kick to get started
00191         _modeDoneTime = t + _startupTime;
00192         _speed = spdReq;
00193 if(_msgCount>0){_msgCount--;cSerial.print("Start ");cSerial.println(rev ? "REV" : "FWD");}
00194         return;
00195       case MOTOR_FWD :
00196       case MOTOR_REV :
00197 if(_msgCount>0){_msgCount--;cSerial.print(_speed);cSerial.print("-->");cSerial.println(spdReq);}
00198         setReverse(_mode == MOTOR_2REV);
00199         if (t > _modeDoneTime) { emergencyStop(); return; } // deadman expired
00200         if ( (ABS(spdReq) < _minPWM)  ||  // stop or change direction
00201             ((spdReq < 0) && (prevMode == MOTOR_FWD)) ||
00202             ((spdReq > 0) && (prevMode == MOTOR_REV)) )
00203           {
00204             stop();
00205             // set speed so that it goes to this speed after coast-down
00206             _speed = (ABS(spdReq) < _minPWM) ? 0 : spdReq;
00207             return;
00208           }
00209         _speed = spdReq;
00210         //analogWrite(EN,getPWM(_speed));
00211         EN.write(_speed);
00212         _modeDoneTime = t + _deadTime;
00213 if(_msgCount>0){_msgCount--;cSerial.println(_speed);}
00214         return;
00215       case MOTOR_2REV :
00216       case MOTOR_2FWD :
00217 if(_msgCount>0){_msgCount--;cSerial.print("start");cSerial.println(spdReq);}
00218         if (ABS(spdReq) < _minPWM)
00219           {
00220             _speed = 100;  // give it some time to decel, although just starting
00221             stop();
00222             _speed = 0;
00223             return;
00224           }
00225         if ( ((spdReq < 0.0f) && (_mode == MOTOR_2FWD)) ||
00226              ((spdReq > 0.0f) && (_mode == MOTOR_2REV)) )
00227           { // direction change
00228             _speed = 0.5f;  // give it some time to decel, although just starting
00229             stop();
00230             _speed = spdReq;  // go to this speed after coast-down period
00231             return;
00232           }
00233         // same direction, but speed request change
00234         _speed = spdReq;
00235         if (t >= _modeDoneTime)
00236           {
00237             _mode = (_speed > 0) ? MOTOR_FWD : MOTOR_REV;
00238             _modeDoneTime = t + _deadTime;
00239             setReverse(_mode == MOTOR_REV);  // make sure direction is correct
00240             EN.write(_speed);
00241 if(_msgCount>0){_msgCount--;cSerial.print("Started ");cSerial.print((int)t);cSerial.print("  done ");cSerial.println(_modeDoneTime);}
00242           }
00243         return;
00244 
00245       default:
00246         cSerial.println("unrecognized state");
00247       }
00248   }
00249 
00250   // update state, but no new command (no deadman reset)
00251   // Checks if previous command is complete, and an automatic state transition
00252   // is needed
00253   virtual void update(long t)  // current time, from millis()
00254   {
00255 //Serial.print(F("Update "));  Serial.println(t);
00256     if ((_modeDoneTime > 0xfffff000) && (t < 999))
00257       {  // time counter must have wrapped around
00258         _modeDoneTime = 0;
00259         cSerial.println("Clock wrap-around");
00260       }
00261 
00262     unsigned char prevMode = _mode;
00263     switch(prevMode)
00264       {
00265       case MOTOR_2STOP : 
00266       case MOTOR_STOP :
00267         if ((t > _modeDoneTime) && _speed)
00268           { // this was a temp stop in a direction change.  Command desired speed.
00269 if(_msgCount>0){_msgCount--;cSerial.print("Restart ");cSerial.println(_speed);}
00270             setSpeed(_speed,t);
00271           }
00272 //else Serial.println("stopped.");
00273         return;
00274       case MOTOR_FWD :
00275       case MOTOR_REV :
00276     if (t > _modeDoneTime) emergencyStop(); // deadman expired
00277     return;
00278       case MOTOR_2REV :
00279       case MOTOR_2FWD :
00280     if (t > _modeDoneTime)
00281           {
00282             //mode = (prevMode == MOTOR_2REV) ? MOTOR_REV : MOTOR_FWD;
00283 if(_msgCount>0){_msgCount--;cSerial.println("moving");}
00284             setSpeed(_speed,t);
00285           }
00286     return;
00287       }
00288   }
00289 
00290   float getCurrent()
00291   {
00292     //return(analogRead(Pin.CS));
00293     return(CS.read());
00294   }
00295 };