suu pen / BD6211_Motordriver

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

Fork of Motordriver by Christopher Hasler

motordriver.cpp@5:3110b9209d3c, 2010-11-25 (annotated)

Committer:
littlexc
Date:
Thu Nov 25 13:34:15 2010 +0000
Revision:
5:3110b9209d3c
Parent:
4:5fb1296c0d60 
set the stop function to coast if not able to dynamicaly brake.

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
littlexc 0:edc152f119b7 29 #include "motordriver.h"
littlexc 1:3da7302dc9ae 30
littlexc 0:edc152f119b7 31 #include "mbed.h"
littlexc 1:3da7302dc9ae 32
littlexc 0:edc152f119b7 33 Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable):
littlexc 0:edc152f119b7 34 _pwm(pwm), _fwd(fwd), _rev(rev) {
littlexc 0:edc152f119b7 35
littlexc 0:edc152f119b7 36 // Set initial condition of PWM
littlexc 0:edc152f119b7 37 _pwm.period(0.001);
littlexc 0:edc152f119b7 38 _pwm = 0;
littlexc 0:edc152f119b7 39
littlexc 0:edc152f119b7 40 // Initial condition of output enables
littlexc 0:edc152f119b7 41 _fwd = 0;
littlexc 0:edc152f119b7 42 _rev = 0;
littlexc 1:3da7302dc9ae 43
littlexc 0:edc152f119b7 44 //set if the motor dirver is capable of braking. (addition)
littlexc 0:edc152f119b7 45 Brakeable= brakeable;
littlexc 1:3da7302dc9ae 46 sign = 0;//i.e nothing.
littlexc 0:edc152f119b7 47 }
littlexc 1:3da7302dc9ae 48
littlexc 2:2dc873322032 49 float Motor::speed(float speed) {
littlexc 2:2dc873322032 50 float temp = 0;
littlexc 1:3da7302dc9ae 51 if (sign == 0) {
littlexc 1:3da7302dc9ae 52 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 53 _rev = (speed < 0.0);
littlexc 2:2dc873322032 54 temp = abs(speed);
littlexc 2:2dc873322032 55 _pwm = temp;
littlexc 1:3da7302dc9ae 56 } else if (sign == 1) {
littlexc 1:3da7302dc9ae 57 if (speed < 0) {
littlexc 1:3da7302dc9ae 58 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 59 _rev = (speed < 0.0);
littlexc 1:3da7302dc9ae 60 _pwm = 0;
littlexc 2:2dc873322032 61 temp = 0;
littlexc 4:5fb1296c0d60 62 } else {
littlexc 1:3da7302dc9ae 63 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 64 _rev = (speed < 0.0);
littlexc 2:2dc873322032 65 temp = abs(speed);
littlexc 2:2dc873322032 66 _pwm = temp;
littlexc 1:3da7302dc9ae 67 }
littlexc 1:3da7302dc9ae 68 } else if (sign == -1) {
littlexc 1:3da7302dc9ae 69 if (speed > 0) {
littlexc 1:3da7302dc9ae 70 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 71 _rev = (speed < 0.0);
littlexc 1:3da7302dc9ae 72 _pwm = 0;
littlexc 2:2dc873322032 73 temp = 0;
littlexc 1:3da7302dc9ae 74 } else {
littlexc 1:3da7302dc9ae 75 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 76 _rev = (speed < 0.0);
littlexc 2:2dc873322032 77 temp = abs(speed);
littlexc 2:2dc873322032 78 _pwm = temp;
littlexc 1:3da7302dc9ae 79 }
littlexc 1:3da7302dc9ae 80 }
littlexc 1:3da7302dc9ae 81 if (speed > 0)
littlexc 1:3da7302dc9ae 82 sign = 1;
littlexc 1:3da7302dc9ae 83 else if (speed < 0) {
littlexc 1:3da7302dc9ae 84 sign = -1;
littlexc 1:3da7302dc9ae 85 } else if (speed == 0) {
littlexc 1:3da7302dc9ae 86 sign = 0;
littlexc 1:3da7302dc9ae 87 }
littlexc 2:2dc873322032 88 return temp;
littlexc 0:edc152f119b7 89 }
littlexc 0:edc152f119b7 90 // (additions)
littlexc 0:edc152f119b7 91 void Motor::coast(void) {
littlexc 0:edc152f119b7 92 _fwd = 0;
littlexc 0:edc152f119b7 93 _rev = 0;
littlexc 0:edc152f119b7 94 _pwm = 0;
littlexc 1:3da7302dc9ae 95 sign = 0;
littlexc 0:edc152f119b7 96 }
littlexc 0:edc152f119b7 97
littlexc 2:2dc873322032 98 float Motor::stop(float duty) {
littlexc 0:edc152f119b7 99 if (Brakeable == 1) {
littlexc 0:edc152f119b7 100 _fwd = 1;
littlexc 0:edc152f119b7 101 _rev = 1;
littlexc 2:2dc873322032 102 _pwm = duty;
littlexc 1:3da7302dc9ae 103 sign = 0;
littlexc 2:2dc873322032 104 return duty;
littlexc 1:3da7302dc9ae 105 } else
littlexc 5:3110b9209d3c 106 Motor::coast();
littlexc 3:8822f4955035 107 return -1;
littlexc 0:edc152f119b7 108 }
littlexc 1:3da7302dc9ae 109
littlexc 4:5fb1296c0d60 110 float Motor::state(void) {
littlexc 4:5fb1296c0d60 111 if ((_fwd == _rev) && (_pwm > 0)) {
littlexc 4:5fb1296c0d60 112 return -2;//braking
littlexc 4:5fb1296c0d60 113 } else if (_pwm == 0) {
littlexc 4:5fb1296c0d60 114 return 2;//coasting
littlexc 4:5fb1296c0d60 115 } else if ((_fwd == 0) && (_rev == 1)) {
littlexc 4:5fb1296c0d60 116 return -(_pwm);//reversing
littlexc 4:5fb1296c0d60 117 } else if ((_fwd == 1) && (_rev == 0)) {
littlexc 4:5fb1296c0d60 118 return _pwm;//fowards
littlexc 4:5fb1296c0d60 119 } else
littlexc 4:5fb1296c0d60 120 return -3;//error
littlexc 4:5fb1296c0d60 121 }
littlexc 4:5fb1296c0d60 122
littlexc 0:edc152f119b7 123 /*
littlexc 0:edc152f119b7 124 test code, this demonstrates working motor drivers.
littlexc 1:3da7302dc9ae 125
littlexc 1:3da7302dc9ae 126 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
littlexc 0:edc152f119b7 127 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
littlexc 0:edc152f119b7 128 int main() {
littlexc 0:edc152f119b7 129 for (float s=-1.0; s < 1.0 ; s += 0.01) {
littlexc 1:3da7302dc9ae 130 A.speed(s);
littlexc 1:3da7302dc9ae 131 B.speed(s);
littlexc 0:edc152f119b7 132 wait(0.02);
littlexc 0:edc152f119b7 133 }
littlexc 0:edc152f119b7 134 A.stop();
littlexc 0:edc152f119b7 135 B.stop();
littlexc 0:edc152f119b7 136 wait(1);
littlexc 0:edc152f119b7 137 A.coast();
littlexc 0:edc152f119b7 138 B.coast();
littlexc 0:edc152f119b7 139 }
littlexc 0:edc152f119b7 140 */