cansat-d_2018 / Mbed 2 deprecated cansat_main_new_3

otamesi

Dependencies:   mbed

motordriver.cpp@41:7c537a922510, 2019-02-16 (annotated)

Committer:
seangshim
Date:
Sat Feb 16 05:40:24 2019 +0000
Revision:
41:7c537a922510
Parent:
0:a01fda36fde8
Child:
7:1c1b782263cf
nei

Who changed what in which revision?

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