Semi-Autonomous-Robot / Semi Autonomous Robot - ECE 4180

Code for controlling mbed hardware (LED's, motors), as well as code for the Raspberry Pi to run a Support Vector Machine that identifies objects using the Pi camera

Dependencies:   mbed Motordriver mbed-rtos PololuLedStrip

mbed/Motordriver/motordriver.cpp@3:a3ed7ff99772, 2019-12-06 (annotated)

Committer:
arogliero3
Date:
Fri Dec 06 00:58:02 2019 -0500
Revision:
3:a3ed7ff99772
Parent:
0:e0dbd261724a 
update img6

Who changed what in which revision?

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