Based on PS3_BlueUSB with reference to http://blog.goo.ne.jp/roboz80/e/10e7bf38d3a63b996ca2894e9fb5e3b6
Diff: motordriver.cpp
- Revision:
- 0:44619612f575
diff -r 000000000000 -r 44619612f575 motordriver.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motordriver.cpp Tue Jul 05 08:25:40 2011 +0000 @@ -0,0 +1,142 @@ +/*motor driver libary modified from the following libary, +* +* mbed simple H-bridge motor controller +* Copyright (c) 2007-2010, sford +* +* by Christopher Hasler. +* +* from sford's libary, +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +* THE SOFTWARE. +*/ + +#include "motordriver.h" + +#include "mbed.h" + +Motor::Motor(PinName pwm, PinName fwd, PinName rev, bool brakeable): + _pwm(pwm), _fwd(fwd), _rev(rev), _brakeable(brakeable), _sign(0) { + + // Set initial condition of PWM + _pwm.period(0.001); + _pwm = 0; + + // Initial condition of output enables + _fwd = 0; + _rev = 0; +} + +float Motor::speed(float speed) { + float temp = 0; + if (_sign == 0) { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + temp = abs(speed); + _pwm = temp; + } else if (_sign == 1) { + if (speed < 0) { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + _pwm = 0; + temp = 0; + } else { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + temp = abs(speed); + _pwm = temp; + } + } else if (_sign == -1) { + if (speed > 0) { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + _pwm = 0; + temp = 0; + } else { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + temp = abs(speed); + _pwm = temp; + } + } + if (speed > 0) + _sign = 1; + else if (speed < 0) { + _sign = -1; + } else if (speed == 0) { + _sign = 0; + } + return temp; +} +// (additions) +void Motor::coast(void) { + _fwd = 0; + _rev = 0; + _pwm = 0; + _sign = 0; +} + +float Motor::stop(float duty) { + if (_brakeable == 1) { + _fwd = 1; + _rev = 1; + _pwm = duty; + _sign = 0; + return duty; + } else + Motor::coast(); + return -1; // error, can't brake +} + +float Motor::state(void) const { + int fwd = _fwd.read(); + int rev = _rev.read(); + float pwm = _pwm.read(); + + if ((fwd == rev) && (pwm > 0)) { + return -2;//braking + } else if (pwm == 0) { + return 2;//coasting + } else if ((fwd == 0) && (rev == 1)) { + return -pwm;//reversing + } else if ((fwd == 1) && (rev == 0)) { + return pwm;//fowards + } else + return -3;//error +} + +//test code, this demonstrates working motor drivers. + +//Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break +/* +Motor A(p22, p7, p8, 1); // pwm, fwd, rev, can break +Motor B(p21, p5, p6, 1); // pwm, fwd, rev, can break +int main() { + for (float s=-1.0; s < 1.0 ; s += 0.01) { + //A.speed(s); + A.speed(s); + B.speed(s); + wait(0.02); + } + A.stop(0); + B.stop(0); + wait(1); + A.coast(); + B.coast(); +} +*/ \ No newline at end of file