Extend Motordriver library
Fork of Motordriver by
Embed:
(wiki syntax)
Show/hide line numbers
motordriver.cpp
00001 /*motor driver libary modified from the following libary, 00002 * 00003 * mbed simple H-bridge motor controller 00004 * Copyright (c) 2007-2010, sford 00005 * 00006 * by Christopher Hasler. 00007 * 00008 * from sford's libary, 00009 * 00010 * Permission is hereby granted, free of charge, to any person obtaining a copy 00011 * of this software and associated documentation files (the "Software"), to deal 00012 * in the Software without restriction, including without limitation the rights 00013 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00014 * copies of the Software, and to permit persons to whom the Software is 00015 * furnished to do so, subject to the following conditions: 00016 * 00017 * The above copyright notice and this permission notice shall be included in 00018 * all copies or substantial portions of the Software. 00019 * 00020 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00021 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00022 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00023 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00024 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00025 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00026 * THE SOFTWARE. 00027 */ 00028 00029 #include "motordriver.h" 00030 00031 #include "mbed.h" 00032 00033 Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable): 00034 _pwm(pwm), _fwd(fwd), _rev(rev) { 00035 00036 // Set initial condition of PWM 00037 _pwm.period(0.001); 00038 _pwm = 0; 00039 00040 // Initial condition of output enables 00041 _fwd = 0; 00042 _rev = 0; 00043 00044 //set if the motor dirver is capable of braking. (addition) 00045 Brakeable= brakeable; 00046 sign = 0;//i.e nothing. 00047 } 00048 00049 void Motor::setPwmFreq(float sec) { 00050 _pwm.period(sec); 00051 } 00052 00053 float Motor::speed(float speed) { 00054 float temp = 0; 00055 if (sign == 0) { 00056 _fwd = (speed > 0.0); 00057 _rev = (speed < 0.0); 00058 temp = abs(speed); 00059 _pwm = temp; 00060 } else if (sign == 1) { 00061 if (speed < 0) { 00062 _fwd = (speed > 0.0); 00063 _rev = (speed < 0.0); 00064 _pwm = 0; 00065 temp = 0; 00066 } else { 00067 _fwd = (speed > 0.0); 00068 _rev = (speed < 0.0); 00069 temp = abs(speed); 00070 _pwm = temp; 00071 } 00072 } else if (sign == -1) { 00073 if (speed > 0) { 00074 _fwd = (speed > 0.0); 00075 _rev = (speed < 0.0); 00076 _pwm = 0; 00077 temp = 0; 00078 } else { 00079 _fwd = (speed > 0.0); 00080 _rev = (speed < 0.0); 00081 temp = abs(speed); 00082 _pwm = temp; 00083 } 00084 } 00085 if (speed > 0) 00086 sign = 1; 00087 else if (speed < 0) { 00088 sign = -1; 00089 } else if (speed == 0) { 00090 sign = 0; 00091 } 00092 return temp; 00093 } 00094 // (additions) 00095 void Motor::coast(void) { 00096 _fwd = 0; 00097 _rev = 0; 00098 _pwm = 0; 00099 sign = 0; 00100 } 00101 00102 float Motor::stop(float duty) { 00103 if (Brakeable == 1) { 00104 _fwd = 1; 00105 _rev = 1; 00106 _pwm = duty; 00107 sign = 0; 00108 return duty; 00109 } else 00110 Motor::coast(); 00111 return -1; 00112 } 00113 00114 float Motor::state(void) { 00115 if ((_fwd == _rev) && (_pwm > 0)) { 00116 return -2;//braking 00117 } else if (_pwm == 0) { 00118 return 2;//coasting 00119 } else if ((_fwd == 0) && (_rev == 1)) { 00120 return -(_pwm);//reversing 00121 } else if ((_fwd == 1) && (_rev == 0)) { 00122 return _pwm;//fowards 00123 } else 00124 return -3;//error 00125 } 00126 00127 /* 00128 test code, this demonstrates working motor drivers. 00129 00130 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break 00131 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break 00132 int main() { 00133 for (float s=-1.0; s < 1.0 ; s += 0.01) { 00134 A.speed(s); 00135 B.speed(s); 00136 wait(0.02); 00137 } 00138 A.stop(); 00139 B.stop(); 00140 wait(1); 00141 A.coast(); 00142 B.coast(); 00143 } 00144 */
Generated on Tue Jul 12 2022 23:00:00 by 1.7.2