BD6211_motordriver.cpp

00001 /*motor driver libary modified from the following libary,
00002 *
00003 * mbed simple H-bridge motor controller
00004 * Copyright (c) 2007-2010, sford
00005 *
00006 * by Christopher Hasler.
00007 *
00008 * from sford's libary,
00009 *
00010 * Permission is hereby granted, free of charge, to any person obtaining a copy
00011 * of this software and associated documentation files (the "Software"), to deal
00012 * in the Software without restriction, including without limitation the rights
00013 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00014 * copies of the Software, and to permit persons to whom the Software is
00015 * furnished to do so, subject to the following conditions:
00016 *
00017 * The above copyright notice and this permission notice shall be included in
00018 * all copies or substantial portions of the Software.
00019 *
00020 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00021 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00022 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00023 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00024 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00025 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00026 * THE SOFTWARE.
00027 */
00028 
00029 #include "BD6211_motordriver.h"
00030 
00031 #include "mbed.h"
00032 
00033 //Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable):
00034 //        _pwm(pwm), _fwd(fwd), _rev(rev) {
00035 Motor::Motor(PinName fwd, PinName rev, int brakeable):
00036     _pwmfwd(fwd), _pwmrev(rev)
00037 {
00038 
00039     // Set initial condition of PWM
00040     //_pwm.period(0.001);
00041     //_pwm = 0;
00042     _pwmfwd.period(0.0001);
00043     _pwmfwd = 0;
00044     
00045     _pwmrev.period(0.0001);
00046     _pwmrev = 0;
00047 
00048     // Initial condition of output enables
00049 //    _fwd = 0;
00050 //    _rev = 0;
00051 
00052     //set if the motor dirver is capable of braking. (addition)
00053     Brakeable= brakeable;
00054     sign = 0;//i.e nothing.
00055 }
00056 
00057 float Motor::speed(float speed)
00058 {
00059     float temp = 0;
00060     if (sign == 0) {
00061 //        _fwd = (speed > 0.0);
00062 //        _rev = (speed < 0.0);
00063 //       temp = abs(speed);
00064 //        _pwm = temp;
00065         temp = abs(speed);
00066         if(speed > 0.0) {
00067             _pwmfwd = temp;
00068             _pwmrev = 0;
00069         } else if(speed < 0.0) {
00070             _pwmfwd = 0;
00071             _pwmrev = temp;
00072         } else {
00073             _pwmfwd = 0;
00074             _pwmrev = 0;
00075             temp = 0;
00076         }
00077     } else if (sign == 1) {
00078 //        if (speed < 0) {
00079 //            _fwd = (speed > 0.0);
00080 //            _rev = (speed < 0.0);
00081 //            _pwm = 0;
00082 //            temp = 0;
00083 //         } else {
00084 //            _fwd = (speed > 0.0);
00085 //            _rev = (speed < 0.0);
00086 //            temp = abs(speed);
00087 //            _pwm = temp;
00088         temp = abs(speed);
00089         if(speed > 0.0) {
00090             _pwmfwd = temp;
00091             _pwmrev = 0;
00092         } else if(speed < 0.0) {
00093             _pwmfwd = 0;
00094             _pwmrev = 0;
00095             temp = 0;
00096         } else {
00097             _pwmfwd = 0;
00098             _pwmrev = 0;
00099             temp = 0;
00100         }
00101 
00102     } else if (sign == -1) {
00103 //    if (speed > 0) {
00104 //        _fwd = (speed > 0.0);
00105 //        _rev = (speed < 0.0);
00106 //        _pwm = 0;
00107 //        temp = 0;
00108 //    } else {
00109 //        _fwd = (speed > 0.0);
00110 //        _rev = (speed < 0.0);
00111 //        temp = abs(speed);
00112 //        _pwm = temp;
00113 //    }
00114         temp = abs(speed);
00115         if(speed > 0.0) {
00116             _pwmfwd = 0;
00117             _pwmrev = 0;
00118             temp = 0;
00119         } else if(speed < 0.0) {
00120             _pwmfwd = 0;
00121             _pwmrev = temp;
00122         } else {
00123             _pwmfwd = 0;
00124             _pwmrev = 0;
00125             temp = 0;
00126         }
00127     }
00128     if (speed > 0)
00129         sign = 1;
00130     else if (speed < 0) {
00131         sign = -1;
00132     } else if (speed == 0) {
00133         sign = 0;
00134     }
00135     return temp;
00136 }
00137 //  (additions)
00138 void Motor::coast(void)
00139 {
00140 //    _fwd = 0;
00141 //    _rev = 0;
00142 //    _pwm = 0;
00143 //    sign = 0;
00144     _pwmfwd = 0;
00145     _pwmrev = 0;
00146     sign = 0;
00147 }
00148 
00149 float Motor::stop(float duty)
00150 {
00151     if (Brakeable == 1) {
00152 //        _fwd = 1;
00153 //        _rev = 1;
00154 //        _pwm = duty;
00155 //        sign = 0;
00156 //        return duty;
00157         _pwmfwd = 1;
00158         _pwmrev = 1;
00159         sign = 0;
00160         return duty;
00161     } else
00162         Motor::coast();
00163     return -1;
00164 }
00165 
00166 float Motor::state(void)
00167 {
00168 //    if ((_fwd == _rev) && (_pwm > 0)) {
00169 //        return -2;//braking
00170 //    } else if (_pwm == 0) {
00171 //        return 2;//coasting
00172 //    } else if ((_fwd == 0) && (_rev == 1)) {
00173 //        return -(_pwm);//reversing
00174 //    }  else if ((_fwd == 1) && (_rev == 0)) {
00175 //        return _pwm;//fowards
00176 //    } else
00177 //        return -3;//error
00178     if ((_pwmfwd == _pwmrev) && (_pwmfwd > 0)) {
00179         return -2;//braking
00180     } else if ((_pwmfwd == 0) && (_pwmrev == 0)) {
00181         return 2;//coasting
00182     } else if ((_pwmfwd == 0) && (_pwmrev != 0)) {
00183         return -(_pwmrev);//reversing
00184     }  else if ((_pwmfwd != 0) && (_pwmrev == 0)) {
00185         return _pwmfwd;//fowards
00186     } else
00187         return -3;//error
00188 }
00189 
00190 /*
00191  test code, this demonstrates working motor drivers.
00192 
00193 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
00194 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
00195 int main() {
00196     for (float s=-1.0; s < 1.0 ; s += 0.01) {
00197        A.speed(s);
00198        B.speed(s);
00199        wait(0.02);
00200     }
00201     A.stop();
00202     B.stop();
00203     wait(1);
00204     A.coast();
00205     B.coast();
00206 }
00207 */