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 "motordriver.h"
00030 
00031 #include "mbed.h"
00032 
00033 Motor::Motor(PinName pwm, PinName fwd, PinName rev, bool brakeable):
00034         _pwm(pwm), _fwd(fwd), _rev(rev), _brakeable(brakeable), _sign(0) {
00035 
00036     // Set initial condition of PWM
00037     _pwm.period(0.001);
00038     _pwm = 0;
00039 
00040     // Initial condition of output enables
00041     _fwd = 0;
00042     _rev = 0;
00043 }
00044 
00045 float Motor::speed(float speed) {
00046     float temp = 0;
00047     if (_sign == 0) {
00048         _fwd = (speed > 0.0);
00049         _rev = (speed < 0.0);
00050         temp = abs(speed);
00051         _pwm = temp;
00052     } else if (_sign == 1) {
00053         if (speed < 0) {
00054             _fwd = (speed > 0.0);
00055             _rev = (speed < 0.0);
00056             _pwm = 0;
00057             temp = 0;
00058        } else {
00059             _fwd = (speed > 0.0);
00060             _rev = (speed < 0.0);
00061             temp = abs(speed);
00062             _pwm = temp;
00063         }
00064     } else if (_sign == -1) {
00065         if (speed > 0) {
00066             _fwd = (speed > 0.0);
00067             _rev = (speed < 0.0);
00068             _pwm = 0;
00069             temp = 0;
00070         } else {
00071             _fwd = (speed > 0.0);
00072             _rev = (speed < 0.0);
00073             temp = abs(speed);
00074             _pwm = temp;
00075         }
00076     }
00077     if (speed > 0)
00078         _sign = 1;
00079     else if (speed < 0) {
00080         _sign = -1;
00081     } else if (speed == 0) {
00082         _sign = 0;
00083     }
00084     return temp;
00085 }
00086 //  (additions)
00087 void Motor::coast(void) {
00088     _fwd = 0;
00089     _rev = 0;
00090     _pwm = 0;
00091     _sign = 0;
00092 }
00093 
00094 float Motor::stop(float duty) {
00095     if (_brakeable == 1) {
00096         _fwd = 1;
00097         _rev = 1;
00098         _pwm = duty;
00099         _sign = 0;
00100         return duty;
00101     } else
00102         Motor::coast();
00103         return -1; // error, can't brake
00104 }
00105 
00106 float Motor::state(void) const {
00107     int fwd = _fwd.read();
00108     int rev = _rev.read();
00109     float pwm = _pwm.read();
00110     
00111     if ((fwd == rev) && (pwm > 0)) {
00112         return -2;//braking
00113     } else if (pwm == 0) {
00114         return 2;//coasting
00115     } else if ((fwd == 0) && (rev == 1)) {
00116         return -pwm;//reversing
00117     }  else if ((fwd == 1) && (rev == 0)) {
00118         return pwm;//fowards
00119     } else
00120         return -3;//error
00121 }
00122 
00123 /*
00124  test code, this demonstrates working motor drivers.
00125 
00126 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
00127 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
00128 int main() {
00129     for (float s=-1.0; s < 1.0 ; s += 0.01) {
00130        A.speed(s);
00131        B.speed(s);
00132        wait(0.02);
00133     }
00134     A.stop();
00135     B.stop();
00136     wait(1);
00137     A.coast();
00138     B.coast();
00139 }
00140 */