Olivier Smeesters / Motordriver

Motor driver library. Based on Christopher Hasler\'s Motordriver library.

Dependents:   project1 motoresMicromouse micromouse coba_lib_lcd ... more

motordriver.cpp@0:83245b45ea70, 2011-02-23 (annotated)

Committer:
osmeest
Date:
Wed Feb 23 23:12:44 2011 +0000
Revision:
0:83245b45ea70
Slightly improvement over original: brakeable is a bool and state() should be const.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
osmeest 0:83245b45ea70 1 /*motor driver libary modified from the following libary,
osmeest 0:83245b45ea70 2 *
osmeest 0:83245b45ea70 3 * mbed simple H-bridge motor controller
osmeest 0:83245b45ea70 4 * Copyright (c) 2007-2010, sford
osmeest 0:83245b45ea70 5 *
osmeest 0:83245b45ea70 6 * by Christopher Hasler.
osmeest 0:83245b45ea70 7 *
osmeest 0:83245b45ea70 8 * from sford's libary,
osmeest 0:83245b45ea70 9 *
osmeest 0:83245b45ea70 10 * Permission is hereby granted, free of charge, to any person obtaining a copy
osmeest 0:83245b45ea70 11 * of this software and associated documentation files (the "Software"), to deal
osmeest 0:83245b45ea70 12 * in the Software without restriction, including without limitation the rights
osmeest 0:83245b45ea70 13 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
osmeest 0:83245b45ea70 14 * copies of the Software, and to permit persons to whom the Software is
osmeest 0:83245b45ea70 15 * furnished to do so, subject to the following conditions:
osmeest 0:83245b45ea70 16 *
osmeest 0:83245b45ea70 17 * The above copyright notice and this permission notice shall be included in
osmeest 0:83245b45ea70 18 * all copies or substantial portions of the Software.
osmeest 0:83245b45ea70 19 *
osmeest 0:83245b45ea70 20 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
osmeest 0:83245b45ea70 21 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
osmeest 0:83245b45ea70 22 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
osmeest 0:83245b45ea70 23 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
osmeest 0:83245b45ea70 24 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
osmeest 0:83245b45ea70 25 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
osmeest 0:83245b45ea70 26 * THE SOFTWARE.
osmeest 0:83245b45ea70 27 */
osmeest 0:83245b45ea70 28
osmeest 0:83245b45ea70 29 #include "motordriver.h"
osmeest 0:83245b45ea70 30
osmeest 0:83245b45ea70 31 #include "mbed.h"
osmeest 0:83245b45ea70 32
osmeest 0:83245b45ea70 33 Motor::Motor(PinName pwm, PinName fwd, PinName rev, bool brakeable):
osmeest 0:83245b45ea70 34 _pwm(pwm), _fwd(fwd), _rev(rev), _brakeable(brakeable), _sign(0) {
osmeest 0:83245b45ea70 35
osmeest 0:83245b45ea70 36 // Set initial condition of PWM
osmeest 0:83245b45ea70 37 _pwm.period(0.001);
osmeest 0:83245b45ea70 38 _pwm = 0;
osmeest 0:83245b45ea70 39
osmeest 0:83245b45ea70 40 // Initial condition of output enables
osmeest 0:83245b45ea70 41 _fwd = 0;
osmeest 0:83245b45ea70 42 _rev = 0;
osmeest 0:83245b45ea70 43 }
osmeest 0:83245b45ea70 44
osmeest 0:83245b45ea70 45 float Motor::speed(float speed) {
osmeest 0:83245b45ea70 46 float temp = 0;
osmeest 0:83245b45ea70 47 if (_sign == 0) {
osmeest 0:83245b45ea70 48 _fwd = (speed > 0.0);
osmeest 0:83245b45ea70 49 _rev = (speed < 0.0);
osmeest 0:83245b45ea70 50 temp = abs(speed);
osmeest 0:83245b45ea70 51 _pwm = temp;
osmeest 0:83245b45ea70 52 } else if (_sign == 1) {
osmeest 0:83245b45ea70 53 if (speed < 0) {
osmeest 0:83245b45ea70 54 _fwd = (speed > 0.0);
osmeest 0:83245b45ea70 55 _rev = (speed < 0.0);
osmeest 0:83245b45ea70 56 _pwm = 0;
osmeest 0:83245b45ea70 57 temp = 0;
osmeest 0:83245b45ea70 58 } else {
osmeest 0:83245b45ea70 59 _fwd = (speed > 0.0);
osmeest 0:83245b45ea70 60 _rev = (speed < 0.0);
osmeest 0:83245b45ea70 61 temp = abs(speed);
osmeest 0:83245b45ea70 62 _pwm = temp;
osmeest 0:83245b45ea70 63 }
osmeest 0:83245b45ea70 64 } else if (_sign == -1) {
osmeest 0:83245b45ea70 65 if (speed > 0) {
osmeest 0:83245b45ea70 66 _fwd = (speed > 0.0);
osmeest 0:83245b45ea70 67 _rev = (speed < 0.0);
osmeest 0:83245b45ea70 68 _pwm = 0;
osmeest 0:83245b45ea70 69 temp = 0;
osmeest 0:83245b45ea70 70 } else {
osmeest 0:83245b45ea70 71 _fwd = (speed > 0.0);
osmeest 0:83245b45ea70 72 _rev = (speed < 0.0);
osmeest 0:83245b45ea70 73 temp = abs(speed);
osmeest 0:83245b45ea70 74 _pwm = temp;
osmeest 0:83245b45ea70 75 }
osmeest 0:83245b45ea70 76 }
osmeest 0:83245b45ea70 77 if (speed > 0)
osmeest 0:83245b45ea70 78 _sign = 1;
osmeest 0:83245b45ea70 79 else if (speed < 0) {
osmeest 0:83245b45ea70 80 _sign = -1;
osmeest 0:83245b45ea70 81 } else if (speed == 0) {
osmeest 0:83245b45ea70 82 _sign = 0;
osmeest 0:83245b45ea70 83 }
osmeest 0:83245b45ea70 84 return temp;
osmeest 0:83245b45ea70 85 }
osmeest 0:83245b45ea70 86 // (additions)
osmeest 0:83245b45ea70 87 void Motor::coast(void) {
osmeest 0:83245b45ea70 88 _fwd = 0;
osmeest 0:83245b45ea70 89 _rev = 0;
osmeest 0:83245b45ea70 90 _pwm = 0;
osmeest 0:83245b45ea70 91 _sign = 0;
osmeest 0:83245b45ea70 92 }
osmeest 0:83245b45ea70 93
osmeest 0:83245b45ea70 94 float Motor::stop(float duty) {
osmeest 0:83245b45ea70 95 if (_brakeable == 1) {
osmeest 0:83245b45ea70 96 _fwd = 1;
osmeest 0:83245b45ea70 97 _rev = 1;
osmeest 0:83245b45ea70 98 _pwm = duty;
osmeest 0:83245b45ea70 99 _sign = 0;
osmeest 0:83245b45ea70 100 return duty;
osmeest 0:83245b45ea70 101 } else
osmeest 0:83245b45ea70 102 Motor::coast();
osmeest 0:83245b45ea70 103 return -1; // error, can't brake
osmeest 0:83245b45ea70 104 }
osmeest 0:83245b45ea70 105
osmeest 0:83245b45ea70 106 float Motor::state(void) const {
osmeest 0:83245b45ea70 107 int fwd = _fwd.read();
osmeest 0:83245b45ea70 108 int rev = _rev.read();
osmeest 0:83245b45ea70 109 float pwm = _pwm.read();
osmeest 0:83245b45ea70 110
osmeest 0:83245b45ea70 111 if ((fwd == rev) && (pwm > 0)) {
osmeest 0:83245b45ea70 112 return -2;//braking
osmeest 0:83245b45ea70 113 } else if (pwm == 0) {
osmeest 0:83245b45ea70 114 return 2;//coasting
osmeest 0:83245b45ea70 115 } else if ((fwd == 0) && (rev == 1)) {
osmeest 0:83245b45ea70 116 return -pwm;//reversing
osmeest 0:83245b45ea70 117 } else if ((fwd == 1) && (rev == 0)) {
osmeest 0:83245b45ea70 118 return pwm;//fowards
osmeest 0:83245b45ea70 119 } else
osmeest 0:83245b45ea70 120 return -3;//error
osmeest 0:83245b45ea70 121 }
osmeest 0:83245b45ea70 122
osmeest 0:83245b45ea70 123 /*
osmeest 0:83245b45ea70 124 test code, this demonstrates working motor drivers.
osmeest 0:83245b45ea70 125
osmeest 0:83245b45ea70 126 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
osmeest 0:83245b45ea70 127 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
osmeest 0:83245b45ea70 128 int main() {
osmeest 0:83245b45ea70 129 for (float s=-1.0; s < 1.0 ; s += 0.01) {
osmeest 0:83245b45ea70 130 A.speed(s);
osmeest 0:83245b45ea70 131 B.speed(s);
osmeest 0:83245b45ea70 132 wait(0.02);
osmeest 0:83245b45ea70 133 }
osmeest 0:83245b45ea70 134 A.stop();
osmeest 0:83245b45ea70 135 B.stop();
osmeest 0:83245b45ea70 136 wait(1);
osmeest 0:83245b45ea70 137 A.coast();
osmeest 0:83245b45ea70 138 B.coast();
osmeest 0:83245b45ea70 139 }
osmeest 0:83245b45ea70 140 */