Kazuya Natsusako / Motordriver

Extend Motordriver library

Fork of Motordriver by Christopher Hasler

motordriver.cpp@6:8dc1d797f383, 2014-12-03 (annotated)

Committer:
Nasupl
Date:
Wed Dec 03 07:11:52 2014 +0000
Revision:
6:8dc1d797f383
Parent:
5:3110b9209d3c 
I have added a function to change the PWM frequency.

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
Nasupl 6:8dc1d797f383 49 void Motor::setPwmFreq(float sec) {
Nasupl 6:8dc1d797f383 50 _pwm.period(sec);
Nasupl 6:8dc1d797f383 51 }
Nasupl 6:8dc1d797f383 52
littlexc 2:2dc873322032 53 float Motor::speed(float speed) {
littlexc 2:2dc873322032 54 float temp = 0;
littlexc 1:3da7302dc9ae 55 if (sign == 0) {
littlexc 1:3da7302dc9ae 56 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 57 _rev = (speed < 0.0);
littlexc 2:2dc873322032 58 temp = abs(speed);
littlexc 2:2dc873322032 59 _pwm = temp;
littlexc 1:3da7302dc9ae 60 } else if (sign == 1) {
littlexc 1:3da7302dc9ae 61 if (speed < 0) {
littlexc 1:3da7302dc9ae 62 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 63 _rev = (speed < 0.0);
littlexc 1:3da7302dc9ae 64 _pwm = 0;
littlexc 2:2dc873322032 65 temp = 0;
littlexc 4:5fb1296c0d60 66 } else {
littlexc 1:3da7302dc9ae 67 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 68 _rev = (speed < 0.0);
littlexc 2:2dc873322032 69 temp = abs(speed);
littlexc 2:2dc873322032 70 _pwm = temp;
littlexc 1:3da7302dc9ae 71 }
littlexc 1:3da7302dc9ae 72 } else if (sign == -1) {
littlexc 1:3da7302dc9ae 73 if (speed > 0) {
littlexc 1:3da7302dc9ae 74 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 75 _rev = (speed < 0.0);
littlexc 1:3da7302dc9ae 76 _pwm = 0;
littlexc 2:2dc873322032 77 temp = 0;
littlexc 1:3da7302dc9ae 78 } else {
littlexc 1:3da7302dc9ae 79 _fwd = (speed > 0.0);
littlexc 1:3da7302dc9ae 80 _rev = (speed < 0.0);
littlexc 2:2dc873322032 81 temp = abs(speed);
littlexc 2:2dc873322032 82 _pwm = temp;
littlexc 1:3da7302dc9ae 83 }
littlexc 1:3da7302dc9ae 84 }
littlexc 1:3da7302dc9ae 85 if (speed > 0)
littlexc 1:3da7302dc9ae 86 sign = 1;
littlexc 1:3da7302dc9ae 87 else if (speed < 0) {
littlexc 1:3da7302dc9ae 88 sign = -1;
littlexc 1:3da7302dc9ae 89 } else if (speed == 0) {
littlexc 1:3da7302dc9ae 90 sign = 0;
littlexc 1:3da7302dc9ae 91 }
littlexc 2:2dc873322032 92 return temp;
littlexc 0:edc152f119b7 93 }
littlexc 0:edc152f119b7 94 // (additions)
littlexc 0:edc152f119b7 95 void Motor::coast(void) {
littlexc 0:edc152f119b7 96 _fwd = 0;
littlexc 0:edc152f119b7 97 _rev = 0;
littlexc 0:edc152f119b7 98 _pwm = 0;
littlexc 1:3da7302dc9ae 99 sign = 0;
littlexc 0:edc152f119b7 100 }
littlexc 0:edc152f119b7 101
littlexc 2:2dc873322032 102 float Motor::stop(float duty) {
littlexc 0:edc152f119b7 103 if (Brakeable == 1) {
littlexc 0:edc152f119b7 104 _fwd = 1;
littlexc 0:edc152f119b7 105 _rev = 1;
littlexc 2:2dc873322032 106 _pwm = duty;
littlexc 1:3da7302dc9ae 107 sign = 0;
littlexc 2:2dc873322032 108 return duty;
littlexc 1:3da7302dc9ae 109 } else
littlexc 5:3110b9209d3c 110 Motor::coast();
littlexc 3:8822f4955035 111 return -1;
littlexc 0:edc152f119b7 112 }
littlexc 1:3da7302dc9ae 113
littlexc 4:5fb1296c0d60 114 float Motor::state(void) {
littlexc 4:5fb1296c0d60 115 if ((_fwd == _rev) && (_pwm > 0)) {
littlexc 4:5fb1296c0d60 116 return -2;//braking
littlexc 4:5fb1296c0d60 117 } else if (_pwm == 0) {
littlexc 4:5fb1296c0d60 118 return 2;//coasting
littlexc 4:5fb1296c0d60 119 } else if ((_fwd == 0) && (_rev == 1)) {
littlexc 4:5fb1296c0d60 120 return -(_pwm);//reversing
littlexc 4:5fb1296c0d60 121 } else if ((_fwd == 1) && (_rev == 0)) {
littlexc 4:5fb1296c0d60 122 return _pwm;//fowards
littlexc 4:5fb1296c0d60 123 } else
littlexc 4:5fb1296c0d60 124 return -3;//error
littlexc 4:5fb1296c0d60 125 }
littlexc 4:5fb1296c0d60 126
littlexc 0:edc152f119b7 127 /*
littlexc 0:edc152f119b7 128 test code, this demonstrates working motor drivers.
littlexc 1:3da7302dc9ae 129
littlexc 1:3da7302dc9ae 130 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
littlexc 0:edc152f119b7 131 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
littlexc 0:edc152f119b7 132 int main() {
littlexc 0:edc152f119b7 133 for (float s=-1.0; s < 1.0 ; s += 0.01) {
littlexc 1:3da7302dc9ae 134 A.speed(s);
littlexc 1:3da7302dc9ae 135 B.speed(s);
littlexc 0:edc152f119b7 136 wait(0.02);
littlexc 0:edc152f119b7 137 }
littlexc 0:edc152f119b7 138 A.stop();
littlexc 0:edc152f119b7 139 B.stop();
littlexc 0:edc152f119b7 140 wait(1);
littlexc 0:edc152f119b7 141 A.coast();
littlexc 0:edc152f119b7 142 B.coast();
littlexc 0:edc152f119b7 143 }
littlexc 0:edc152f119b7 144 */