130901現在開発中 BD6211 motor driver Library FIN,RIN にPWM信号を入れて速度制御する
Fork of Motordriver by
BD6211_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 "BD6211_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 Motor::Motor(PinName fwd, PinName rev, int brakeable): 00036 _pwmfwd(fwd), _pwmrev(rev) 00037 { 00038 00039 // Set initial condition of PWM 00040 //_pwm.period(0.001); 00041 //_pwm = 0; 00042 _pwmfwd.period(0.0001); 00043 _pwmfwd = 0; 00044 00045 _pwmrev.period(0.0001); 00046 _pwmrev = 0; 00047 00048 // Initial condition of output enables 00049 // _fwd = 0; 00050 // _rev = 0; 00051 00052 //set if the motor dirver is capable of braking. (addition) 00053 Brakeable= brakeable; 00054 sign = 0;//i.e nothing. 00055 } 00056 00057 float Motor::speed(float speed) 00058 { 00059 float temp = 0; 00060 if (sign == 0) { 00061 // _fwd = (speed > 0.0); 00062 // _rev = (speed < 0.0); 00063 // temp = abs(speed); 00064 // _pwm = temp; 00065 temp = abs(speed); 00066 if(speed > 0.0) { 00067 _pwmfwd = temp; 00068 _pwmrev = 0; 00069 } else if(speed < 0.0) { 00070 _pwmfwd = 0; 00071 _pwmrev = temp; 00072 } else { 00073 _pwmfwd = 0; 00074 _pwmrev = 0; 00075 temp = 0; 00076 } 00077 } else if (sign == 1) { 00078 // if (speed < 0) { 00079 // _fwd = (speed > 0.0); 00080 // _rev = (speed < 0.0); 00081 // _pwm = 0; 00082 // temp = 0; 00083 // } else { 00084 // _fwd = (speed > 0.0); 00085 // _rev = (speed < 0.0); 00086 // temp = abs(speed); 00087 // _pwm = temp; 00088 temp = abs(speed); 00089 if(speed > 0.0) { 00090 _pwmfwd = temp; 00091 _pwmrev = 0; 00092 } else if(speed < 0.0) { 00093 _pwmfwd = 0; 00094 _pwmrev = 0; 00095 temp = 0; 00096 } else { 00097 _pwmfwd = 0; 00098 _pwmrev = 0; 00099 temp = 0; 00100 } 00101 00102 } else if (sign == -1) { 00103 // if (speed > 0) { 00104 // _fwd = (speed > 0.0); 00105 // _rev = (speed < 0.0); 00106 // _pwm = 0; 00107 // temp = 0; 00108 // } else { 00109 // _fwd = (speed > 0.0); 00110 // _rev = (speed < 0.0); 00111 // temp = abs(speed); 00112 // _pwm = temp; 00113 // } 00114 temp = abs(speed); 00115 if(speed > 0.0) { 00116 _pwmfwd = 0; 00117 _pwmrev = 0; 00118 temp = 0; 00119 } else if(speed < 0.0) { 00120 _pwmfwd = 0; 00121 _pwmrev = temp; 00122 } else { 00123 _pwmfwd = 0; 00124 _pwmrev = 0; 00125 temp = 0; 00126 } 00127 } 00128 if (speed > 0) 00129 sign = 1; 00130 else if (speed < 0) { 00131 sign = -1; 00132 } else if (speed == 0) { 00133 sign = 0; 00134 } 00135 return temp; 00136 } 00137 // (additions) 00138 void Motor::coast(void) 00139 { 00140 // _fwd = 0; 00141 // _rev = 0; 00142 // _pwm = 0; 00143 // sign = 0; 00144 _pwmfwd = 0; 00145 _pwmrev = 0; 00146 sign = 0; 00147 } 00148 00149 float Motor::stop(float duty) 00150 { 00151 if (Brakeable == 1) { 00152 // _fwd = 1; 00153 // _rev = 1; 00154 // _pwm = duty; 00155 // sign = 0; 00156 // return duty; 00157 _pwmfwd = 1; 00158 _pwmrev = 1; 00159 sign = 0; 00160 return duty; 00161 } else 00162 Motor::coast(); 00163 return -1; 00164 } 00165 00166 float Motor::state(void) 00167 { 00168 // if ((_fwd == _rev) && (_pwm > 0)) { 00169 // return -2;//braking 00170 // } else if (_pwm == 0) { 00171 // return 2;//coasting 00172 // } else if ((_fwd == 0) && (_rev == 1)) { 00173 // return -(_pwm);//reversing 00174 // } else if ((_fwd == 1) && (_rev == 0)) { 00175 // return _pwm;//fowards 00176 // } else 00177 // return -3;//error 00178 if ((_pwmfwd == _pwmrev) && (_pwmfwd > 0)) { 00179 return -2;//braking 00180 } else if ((_pwmfwd == 0) && (_pwmrev == 0)) { 00181 return 2;//coasting 00182 } else if ((_pwmfwd == 0) && (_pwmrev != 0)) { 00183 return -(_pwmrev);//reversing 00184 } else if ((_pwmfwd != 0) && (_pwmrev == 0)) { 00185 return _pwmfwd;//fowards 00186 } else 00187 return -3;//error 00188 } 00189 00190 /* 00191 test code, this demonstrates working motor drivers. 00192 00193 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break 00194 Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break 00195 int main() { 00196 for (float s=-1.0; s < 1.0 ; s += 0.01) { 00197 A.speed(s); 00198 B.speed(s); 00199 wait(0.02); 00200 } 00201 A.stop(); 00202 B.stop(); 00203 wait(1); 00204 A.coast(); 00205 B.coast(); 00206 } 00207 */
Generated on Fri Jul 22 2022 03:58:08 by 1.7.2