130901現在開発中 BD6211 motor driver Library FIN,RIN にPWM信号を入れて速度制御する
Fork of Motordriver by
BD6211_motordriver.cpp@6:70f4fa29f10e, 2013-09-01 (annotated)
- Committer:
- suupen
- Date:
- Sun Sep 01 10:17:10 2013 +0000
- Revision:
- 6:70f4fa29f10e
- Parent:
- motordriver.cpp@5:3110b9209d3c
BD6211 motor driver ?FIN,RIN???PWM?????????????Library
; 130901?????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
littlexc | 1:3da7302dc9ae | 1 | /*motor driver libary modified from the following libary, |
littlexc | 1:3da7302dc9ae | 2 | * |
littlexc | 1:3da7302dc9ae | 3 | * mbed simple H-bridge motor controller |
littlexc | 1:3da7302dc9ae | 4 | * Copyright (c) 2007-2010, sford |
littlexc | 1:3da7302dc9ae | 5 | * |
littlexc | 1:3da7302dc9ae | 6 | * by Christopher Hasler. |
littlexc | 1:3da7302dc9ae | 7 | * |
littlexc | 1:3da7302dc9ae | 8 | * from sford's libary, |
littlexc | 1:3da7302dc9ae | 9 | * |
littlexc | 1:3da7302dc9ae | 10 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
littlexc | 1:3da7302dc9ae | 11 | * of this software and associated documentation files (the "Software"), to deal |
littlexc | 1:3da7302dc9ae | 12 | * in the Software without restriction, including without limitation the rights |
littlexc | 1:3da7302dc9ae | 13 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
littlexc | 1:3da7302dc9ae | 14 | * copies of the Software, and to permit persons to whom the Software is |
littlexc | 1:3da7302dc9ae | 15 | * furnished to do so, subject to the following conditions: |
littlexc | 1:3da7302dc9ae | 16 | * |
littlexc | 1:3da7302dc9ae | 17 | * The above copyright notice and this permission notice shall be included in |
littlexc | 1:3da7302dc9ae | 18 | * all copies or substantial portions of the Software. |
littlexc | 1:3da7302dc9ae | 19 | * |
littlexc | 1:3da7302dc9ae | 20 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
littlexc | 1:3da7302dc9ae | 21 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
littlexc | 1:3da7302dc9ae | 22 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
littlexc | 1:3da7302dc9ae | 23 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
littlexc | 1:3da7302dc9ae | 24 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
littlexc | 1:3da7302dc9ae | 25 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
littlexc | 1:3da7302dc9ae | 26 | * THE SOFTWARE. |
littlexc | 1:3da7302dc9ae | 27 | */ |
littlexc | 0:edc152f119b7 | 28 | |
suupen | 6:70f4fa29f10e | 29 | #include "BD6211_motordriver.h" |
littlexc | 1:3da7302dc9ae | 30 | |
littlexc | 0:edc152f119b7 | 31 | #include "mbed.h" |
littlexc | 1:3da7302dc9ae | 32 | |
suupen | 6:70f4fa29f10e | 33 | //Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable): |
suupen | 6:70f4fa29f10e | 34 | // _pwm(pwm), _fwd(fwd), _rev(rev) { |
suupen | 6:70f4fa29f10e | 35 | Motor::Motor(PinName fwd, PinName rev, int brakeable): |
suupen | 6:70f4fa29f10e | 36 | _pwmfwd(fwd), _pwmrev(rev) |
suupen | 6:70f4fa29f10e | 37 | { |
littlexc | 0:edc152f119b7 | 38 | |
littlexc | 0:edc152f119b7 | 39 | // Set initial condition of PWM |
suupen | 6:70f4fa29f10e | 40 | //_pwm.period(0.001); |
suupen | 6:70f4fa29f10e | 41 | //_pwm = 0; |
suupen | 6:70f4fa29f10e | 42 | _pwmfwd.period(0.0001); |
suupen | 6:70f4fa29f10e | 43 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 44 | |
suupen | 6:70f4fa29f10e | 45 | _pwmrev.period(0.0001); |
suupen | 6:70f4fa29f10e | 46 | _pwmrev = 0; |
littlexc | 0:edc152f119b7 | 47 | |
littlexc | 0:edc152f119b7 | 48 | // Initial condition of output enables |
suupen | 6:70f4fa29f10e | 49 | // _fwd = 0; |
suupen | 6:70f4fa29f10e | 50 | // _rev = 0; |
littlexc | 1:3da7302dc9ae | 51 | |
littlexc | 0:edc152f119b7 | 52 | //set if the motor dirver is capable of braking. (addition) |
littlexc | 0:edc152f119b7 | 53 | Brakeable= brakeable; |
littlexc | 1:3da7302dc9ae | 54 | sign = 0;//i.e nothing. |
littlexc | 0:edc152f119b7 | 55 | } |
littlexc | 1:3da7302dc9ae | 56 | |
suupen | 6:70f4fa29f10e | 57 | float Motor::speed(float speed) |
suupen | 6:70f4fa29f10e | 58 | { |
littlexc | 2:2dc873322032 | 59 | float temp = 0; |
littlexc | 1:3da7302dc9ae | 60 | if (sign == 0) { |
suupen | 6:70f4fa29f10e | 61 | // _fwd = (speed > 0.0); |
suupen | 6:70f4fa29f10e | 62 | // _rev = (speed < 0.0); |
suupen | 6:70f4fa29f10e | 63 | // temp = abs(speed); |
suupen | 6:70f4fa29f10e | 64 | // _pwm = temp; |
littlexc | 2:2dc873322032 | 65 | temp = abs(speed); |
suupen | 6:70f4fa29f10e | 66 | if(speed > 0.0) { |
suupen | 6:70f4fa29f10e | 67 | _pwmfwd = temp; |
suupen | 6:70f4fa29f10e | 68 | _pwmrev = 0; |
suupen | 6:70f4fa29f10e | 69 | } else if(speed < 0.0) { |
suupen | 6:70f4fa29f10e | 70 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 71 | _pwmrev = temp; |
suupen | 6:70f4fa29f10e | 72 | } else { |
suupen | 6:70f4fa29f10e | 73 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 74 | _pwmrev = 0; |
littlexc | 2:2dc873322032 | 75 | temp = 0; |
littlexc | 1:3da7302dc9ae | 76 | } |
suupen | 6:70f4fa29f10e | 77 | } else if (sign == 1) { |
suupen | 6:70f4fa29f10e | 78 | // if (speed < 0) { |
suupen | 6:70f4fa29f10e | 79 | // _fwd = (speed > 0.0); |
suupen | 6:70f4fa29f10e | 80 | // _rev = (speed < 0.0); |
suupen | 6:70f4fa29f10e | 81 | // _pwm = 0; |
suupen | 6:70f4fa29f10e | 82 | // temp = 0; |
suupen | 6:70f4fa29f10e | 83 | // } else { |
suupen | 6:70f4fa29f10e | 84 | // _fwd = (speed > 0.0); |
suupen | 6:70f4fa29f10e | 85 | // _rev = (speed < 0.0); |
suupen | 6:70f4fa29f10e | 86 | // temp = abs(speed); |
suupen | 6:70f4fa29f10e | 87 | // _pwm = temp; |
suupen | 6:70f4fa29f10e | 88 | temp = abs(speed); |
suupen | 6:70f4fa29f10e | 89 | if(speed > 0.0) { |
suupen | 6:70f4fa29f10e | 90 | _pwmfwd = temp; |
suupen | 6:70f4fa29f10e | 91 | _pwmrev = 0; |
suupen | 6:70f4fa29f10e | 92 | } else if(speed < 0.0) { |
suupen | 6:70f4fa29f10e | 93 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 94 | _pwmrev = 0; |
littlexc | 2:2dc873322032 | 95 | temp = 0; |
littlexc | 1:3da7302dc9ae | 96 | } else { |
suupen | 6:70f4fa29f10e | 97 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 98 | _pwmrev = 0; |
suupen | 6:70f4fa29f10e | 99 | temp = 0; |
suupen | 6:70f4fa29f10e | 100 | } |
suupen | 6:70f4fa29f10e | 101 | |
suupen | 6:70f4fa29f10e | 102 | } else if (sign == -1) { |
suupen | 6:70f4fa29f10e | 103 | // if (speed > 0) { |
suupen | 6:70f4fa29f10e | 104 | // _fwd = (speed > 0.0); |
suupen | 6:70f4fa29f10e | 105 | // _rev = (speed < 0.0); |
suupen | 6:70f4fa29f10e | 106 | // _pwm = 0; |
suupen | 6:70f4fa29f10e | 107 | // temp = 0; |
suupen | 6:70f4fa29f10e | 108 | // } else { |
suupen | 6:70f4fa29f10e | 109 | // _fwd = (speed > 0.0); |
suupen | 6:70f4fa29f10e | 110 | // _rev = (speed < 0.0); |
suupen | 6:70f4fa29f10e | 111 | // temp = abs(speed); |
suupen | 6:70f4fa29f10e | 112 | // _pwm = temp; |
suupen | 6:70f4fa29f10e | 113 | // } |
suupen | 6:70f4fa29f10e | 114 | temp = abs(speed); |
suupen | 6:70f4fa29f10e | 115 | if(speed > 0.0) { |
suupen | 6:70f4fa29f10e | 116 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 117 | _pwmrev = 0; |
suupen | 6:70f4fa29f10e | 118 | temp = 0; |
suupen | 6:70f4fa29f10e | 119 | } else if(speed < 0.0) { |
suupen | 6:70f4fa29f10e | 120 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 121 | _pwmrev = temp; |
suupen | 6:70f4fa29f10e | 122 | } else { |
suupen | 6:70f4fa29f10e | 123 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 124 | _pwmrev = 0; |
suupen | 6:70f4fa29f10e | 125 | temp = 0; |
littlexc | 1:3da7302dc9ae | 126 | } |
littlexc | 1:3da7302dc9ae | 127 | } |
littlexc | 1:3da7302dc9ae | 128 | if (speed > 0) |
littlexc | 1:3da7302dc9ae | 129 | sign = 1; |
littlexc | 1:3da7302dc9ae | 130 | else if (speed < 0) { |
littlexc | 1:3da7302dc9ae | 131 | sign = -1; |
littlexc | 1:3da7302dc9ae | 132 | } else if (speed == 0) { |
littlexc | 1:3da7302dc9ae | 133 | sign = 0; |
littlexc | 1:3da7302dc9ae | 134 | } |
littlexc | 2:2dc873322032 | 135 | return temp; |
littlexc | 0:edc152f119b7 | 136 | } |
littlexc | 0:edc152f119b7 | 137 | // (additions) |
suupen | 6:70f4fa29f10e | 138 | void Motor::coast(void) |
suupen | 6:70f4fa29f10e | 139 | { |
suupen | 6:70f4fa29f10e | 140 | // _fwd = 0; |
suupen | 6:70f4fa29f10e | 141 | // _rev = 0; |
suupen | 6:70f4fa29f10e | 142 | // _pwm = 0; |
suupen | 6:70f4fa29f10e | 143 | // sign = 0; |
suupen | 6:70f4fa29f10e | 144 | _pwmfwd = 0; |
suupen | 6:70f4fa29f10e | 145 | _pwmrev = 0; |
littlexc | 1:3da7302dc9ae | 146 | sign = 0; |
littlexc | 0:edc152f119b7 | 147 | } |
littlexc | 0:edc152f119b7 | 148 | |
suupen | 6:70f4fa29f10e | 149 | float Motor::stop(float duty) |
suupen | 6:70f4fa29f10e | 150 | { |
littlexc | 0:edc152f119b7 | 151 | if (Brakeable == 1) { |
suupen | 6:70f4fa29f10e | 152 | // _fwd = 1; |
suupen | 6:70f4fa29f10e | 153 | // _rev = 1; |
suupen | 6:70f4fa29f10e | 154 | // _pwm = duty; |
suupen | 6:70f4fa29f10e | 155 | // sign = 0; |
suupen | 6:70f4fa29f10e | 156 | // return duty; |
suupen | 6:70f4fa29f10e | 157 | _pwmfwd = 1; |
suupen | 6:70f4fa29f10e | 158 | _pwmrev = 1; |
littlexc | 1:3da7302dc9ae | 159 | sign = 0; |
littlexc | 2:2dc873322032 | 160 | return duty; |
littlexc | 1:3da7302dc9ae | 161 | } else |
littlexc | 5:3110b9209d3c | 162 | Motor::coast(); |
suupen | 6:70f4fa29f10e | 163 | return -1; |
littlexc | 0:edc152f119b7 | 164 | } |
littlexc | 1:3da7302dc9ae | 165 | |
suupen | 6:70f4fa29f10e | 166 | float Motor::state(void) |
suupen | 6:70f4fa29f10e | 167 | { |
suupen | 6:70f4fa29f10e | 168 | // if ((_fwd == _rev) && (_pwm > 0)) { |
suupen | 6:70f4fa29f10e | 169 | // return -2;//braking |
suupen | 6:70f4fa29f10e | 170 | // } else if (_pwm == 0) { |
suupen | 6:70f4fa29f10e | 171 | // return 2;//coasting |
suupen | 6:70f4fa29f10e | 172 | // } else if ((_fwd == 0) && (_rev == 1)) { |
suupen | 6:70f4fa29f10e | 173 | // return -(_pwm);//reversing |
suupen | 6:70f4fa29f10e | 174 | // } else if ((_fwd == 1) && (_rev == 0)) { |
suupen | 6:70f4fa29f10e | 175 | // return _pwm;//fowards |
suupen | 6:70f4fa29f10e | 176 | // } else |
suupen | 6:70f4fa29f10e | 177 | // return -3;//error |
suupen | 6:70f4fa29f10e | 178 | if ((_pwmfwd == _pwmrev) && (_pwmfwd > 0)) { |
littlexc | 4:5fb1296c0d60 | 179 | return -2;//braking |
suupen | 6:70f4fa29f10e | 180 | } else if ((_pwmfwd == 0) && (_pwmrev == 0)) { |
littlexc | 4:5fb1296c0d60 | 181 | return 2;//coasting |
suupen | 6:70f4fa29f10e | 182 | } else if ((_pwmfwd == 0) && (_pwmrev != 0)) { |
suupen | 6:70f4fa29f10e | 183 | return -(_pwmrev);//reversing |
suupen | 6:70f4fa29f10e | 184 | } else if ((_pwmfwd != 0) && (_pwmrev == 0)) { |
suupen | 6:70f4fa29f10e | 185 | return _pwmfwd;//fowards |
littlexc | 4:5fb1296c0d60 | 186 | } else |
littlexc | 4:5fb1296c0d60 | 187 | return -3;//error |
littlexc | 4:5fb1296c0d60 | 188 | } |
littlexc | 4:5fb1296c0d60 | 189 | |
littlexc | 0:edc152f119b7 | 190 | /* |
littlexc | 0:edc152f119b7 | 191 | test code, this demonstrates working motor drivers. |
littlexc | 1:3da7302dc9ae | 192 | |
littlexc | 1:3da7302dc9ae | 193 | Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break |
littlexc | 0:edc152f119b7 | 194 | Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break |
littlexc | 0:edc152f119b7 | 195 | int main() { |
littlexc | 0:edc152f119b7 | 196 | for (float s=-1.0; s < 1.0 ; s += 0.01) { |
littlexc | 1:3da7302dc9ae | 197 | A.speed(s); |
littlexc | 1:3da7302dc9ae | 198 | B.speed(s); |
littlexc | 0:edc152f119b7 | 199 | wait(0.02); |
littlexc | 0:edc152f119b7 | 200 | } |
littlexc | 0:edc152f119b7 | 201 | A.stop(); |
littlexc | 0:edc152f119b7 | 202 | B.stop(); |
littlexc | 0:edc152f119b7 | 203 | wait(1); |
littlexc | 0:edc152f119b7 | 204 | A.coast(); |
littlexc | 0:edc152f119b7 | 205 | B.coast(); |
littlexc | 0:edc152f119b7 | 206 | } |
littlexc | 0:edc152f119b7 | 207 | */ |