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