Christopher Hasler / motordrivercs

motordrivercs.cpp@1:3ece560dd2da, 2010-11-19 (annotated)

Committer:
littlexc
Date:
Fri Nov 19 12:18:21 2010 +0000
Revision:
1:3ece560dd2da
Parent:
0:774d7a3e63ae 
documentation change, no code diffrence.

Who changed what in which revision?

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