130901現在開発中 BD6211 motor driver Library FIN,RIN にPWM信号を入れて速度制御する

Fork of Motordriver by Christopher Hasler

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?

UserRevisionLine numberNew 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 */