Modified phase table in stepper.cpp to enable stepping a bipolar motor using a twin H-bridge driver.
Fork of stepper by
stepper.cpp
00001 /* 00002 * mbed library program 00003 * Stepping Motor 00004 * 00005 * Copyright (c) 2014 Kenji Arai / JH1PJL 00006 * http://www.page.sannet.ne.jp/kenjia/index.html 00007 * http://mbed.org/users/kenjiArai/ 00008 * Created: August 20th, 2014 00009 * Revised: August 23rd, 2014 00010 * 00011 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 00012 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE 00013 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 00014 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00015 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 00016 */ 00017 00018 #include "mbed.h" 00019 #include "stepper.h" 00020 00021 //uncomment one option 00022 //in truth, both uni- & bi- are equivalent, and just represent a different wiring of mcu GPIOs to H-Bridge 00023 //TODO: adopt one most-prevalent wiring scheme only, run-time selectable wiring & step pattern? 00024 00025 //#define BIPOLAR_STEPPER_1PH 00026 //#define BIPOLAR_STEPPER_2PH 00027 #define BIPOLAR_STEPPER_12S 00028 //#define UNIPOLAR_STEPPER_1PH 00029 //#define UNIPOLAR_STEPPER_2PH 00030 //#define UNIPOLAR_STEPPER_12S 00031 /* 00032 * Firing sequence for bi-polar (4 wires) stepper (2x H-Bridge driver required eg L298) 00033 */ 00034 #ifdef BIPOLAR_STEPPER_1PH 00035 //single phase A+, B+, A-, B- 00036 //mcu wiring to H-bridge: A A' B B' 00037 const uint8_t pase_cw[4][4] = {{1, 0, 0, 0}, {0, 0, 1, 0}, {0, 1, 0, 0}, {0, 0, 0, 1}}; 00038 #endif 00039 00040 #ifdef BIPOLAR_STEPPER_2PH 00041 //double phase A+B+, A-B+, A-B-, A+B- 00042 const uint8_t pase_cw[4][4] = {{1, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 1}, {1, 0, 0, 1}}; 00043 #endif 00044 00045 #ifdef BIPOLAR_STEPPER_12S 00046 //half step A+B+, A+, A+B-, B-, A-B-, A-, A-B+, B+ 00047 const uint8_t pase_cw[8][4] = {{1, 0, 1, 0}, {1, 0, 0, 0}, {1, 0, 0, 1}, {0, 0, 0, 1}, {0, 1, 0, 1}, {0, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 0}}; 00048 #endif 00049 00050 /* 00051 * Firing sequence for uni-polar (5+ wires) stepper (4x open-collector drivers required). 00052 */ 00053 #ifdef UNIPOLAR_STEPPER_1PH 00054 //mcu wiring to Driver/FETs A+ B+ A- B- 00055 const uint8_t pase_cw[4][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; 00056 #endif 00057 00058 #ifdef UNIPOLAR_STEPPER_2PH 00059 //mcu wiring to Driver/FETs A+ B+ A- B- 00060 const uint8_t pase_cw[4][4] = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}}; 00061 #endif 00062 00063 #ifdef UNIPOLAR_STEPPER_12S 00064 //mcu wiring to Driver/FETs A+ B+ A- B- 00065 const uint8_t pase_cw[8][4] = {{1, 1, 0, 0}, {1, 0, 0, 0}, {1, 0, 0, 1}, {0, 0, 0, 1}, {0, 0, 1, 1}, {0, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 0}}; 00066 #endif 00067 00068 //declare as pointer, not array, so that we can swap to different acc/deceleration arrays on the fly. 00069 extern uint8_t *pls_width; 00070 00071 STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn): 00072 _xp(xp), _xn(xn), _yp(yp), _yn(yn) 00073 { 00074 stop(); 00075 } 00076 00077 void STEPPER::move (int32_t steps) 00078 { 00079 if (steps < 0) { 00080 inf.direction = D_CCW; 00081 steps = -steps; 00082 } 00083 // test for +ve, don't flip direction for stop command steps==0 00084 else if (steps > 0) { 00085 inf.direction = D_CW; 00086 } 00087 inf.total_step = steps; 00088 setup_mtr_drv_dt(&inf, &cntl); 00089 } 00090 00091 void STEPPER::set_max_speed (uint32_t time_base_us) 00092 { 00093 if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)) { 00094 time_base_us = (MT_PLS_WIDTH_MIN * 1000); 00095 } 00096 _smdrv.detach(); 00097 _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us); 00098 } 00099 00100 uint8_t STEPPER::status (void) 00101 { 00102 return cntl.state; 00103 } 00104 00105 void STEPPER::stop(void) 00106 { 00107 busy_sm_drv = 0; 00108 _xp = 0; 00109 _xn = 0; 00110 _yp = 0; 00111 _yn = 0; 00112 } 00113 00114 void STEPPER::set4ports (void) 00115 { 00116 uint8_t i; 00117 cntl.motor_step+=inf.direction; 00118 i = (uint8_t)(cntl.motor_step % (sizeof(pase_cw)/4) ); 00119 _xp = pase_cw[i][0]; 00120 _xn = pase_cw[i][1]; 00121 _yp = pase_cw[i][2]; 00122 _yn = pase_cw[i][3]; 00123 } 00124 00125 void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt) 00126 { 00127 busy_sm_drv = 1; 00128 00129 //0-steps command: if moving, bring to controlled stop & hold position 00130 if (mi->total_step == 0) { 00131 if (mt->state != M_STOP) { 00132 mt->state = M_CHANGE; 00133 mt->change_cnt = 5; 00134 mt->pls_width = 0; 00135 mt->ongoing = 0; 00136 mt->up_cnt = 0; 00137 mt->up_cnt_keep = 0; 00138 mt->down_cnt = 0; 00139 mt->continue_cnt = 0; 00140 } //else init(); // already stopped, interpret as: release HOLD currents, power down coils, motor freewheeling. init() now public, renamed stop() 00141 busy_sm_drv = 0; 00142 return; 00143 } 00144 00145 //already travelling full speed, new step in same direction 00146 if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)) { 00147 if (mi->total_step < MT_SLOP_STEP) { //step len shorter than ramp down ledn 00148 mt->up_cnt = 0; 00149 mt->up_cnt_keep = 0; 00150 mt->down_cnt = mi->total_step; 00151 mt->continue_cnt = 0; 00152 mt->state = M_DOWN; 00153 mt->ongoing = 0; 00154 } else { 00155 mt->up_cnt = 0; 00156 mt->up_cnt_keep = 0; 00157 mt->down_cnt = MT_SLOP_STEP -1; 00158 mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1); 00159 } 00160 } else { 00161 //already moving, reverse direction required 00162 if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)) { 00163 mt->state = M_CHANGE; 00164 mt->change_cnt = 5; 00165 } else {//motor was at rest? 00166 //mt->motor_step = 0; // don't destroy knowledge of current phase 00167 mt->state = M_UP; 00168 } 00169 mt->pls_width = 0; 00170 mt->ongoing = 0; 00171 mt->direction = mi->direction; 00172 if (mi->total_step < MT_MIN_STEP) { 00173 if (mi->total_step == MT_MIN_STEP - 1) { 00174 mt->up_cnt = MT_SLOP_STEP; 00175 } else { 00176 mt->up_cnt = mi->total_step / 2; 00177 if (mt->up_cnt==0) mt->up_cnt=1; 00178 } 00179 mt->up_cnt_keep = mt->up_cnt; 00180 mt->down_cnt = mi->total_step - mt->up_cnt; 00181 mt->continue_cnt = 0; 00182 } else { 00183 mt->up_cnt = MT_SLOP_STEP; 00184 mt->up_cnt_keep = mt->up_cnt; 00185 mt->down_cnt = MT_SLOP_STEP -1 ; 00186 mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1); 00187 } 00188 } 00189 busy_sm_drv = 0; 00190 } 00191 00192 void STEPPER::millisec_inteval() 00193 { 00194 if (busy_sm_drv == 1) { 00195 return; 00196 } 00197 switch (cntl.state) { 00198 case M_STOP: 00199 /* 00200 * no move, but continue to energize coils: hold current position 00201 */ 00202 break; 00203 case M_UP: 00204 if (cntl.ongoing) { 00205 if (--cntl.pls_width == 0) { 00206 if (--cntl.up_cnt == 0) { 00207 cntl.ongoing = 0; 00208 if (cntl.continue_cnt == 0) { 00209 cntl.state = M_DOWN; 00210 if (cntl.down_cnt==0) cntl.state=M_STOP; 00211 } else { 00212 cntl.state = M_CONTINUE; 00213 } 00214 } else { 00215 set4ports(); 00216 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt]; 00217 } 00218 } else { 00219 break; 00220 } 00221 } else { // 1st entry from M_STOP 00222 set4ports(); 00223 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt]; 00224 cntl.ongoing = 1; 00225 } 00226 break; 00227 case M_CONTINUE: 00228 set4ports(); 00229 if (--cntl.continue_cnt == 0) { 00230 cntl.ongoing = 0; 00231 cntl.state = M_DOWN; 00232 } 00233 break; 00234 case M_DOWN: 00235 if (cntl.ongoing) { 00236 if (--cntl.pls_width == 0) { 00237 if (--cntl.down_cnt == 0) { 00238 cntl.ongoing = 0; 00239 cntl.state = M_STOP; 00240 } else { 00241 set4ports(); 00242 cntl.pls_width = pls_width[cntl.down_cnt]; 00243 } 00244 } else { 00245 break; 00246 } 00247 } else { // 1st entry from M_UP or M_CONTINUE 00248 set4ports(); 00249 cntl.pls_width = pls_width[cntl.down_cnt]; 00250 cntl.ongoing = 1; 00251 } 00252 break; 00253 case M_CHANGE: 00254 if (cntl.ongoing) { 00255 if (--cntl.pls_width == 0) { 00256 if (--cntl.change_cnt == 0) { 00257 cntl.ongoing = 0; 00258 if (cntl.up_cnt == 0) { 00259 cntl.state = M_STOP; 00260 } else { 00261 cntl.state = M_UP; 00262 } 00263 } else { 00264 set4ports(); 00265 cntl.pls_width = pls_width[cntl.change_cnt]; 00266 } 00267 } else { 00268 break; 00269 } 00270 } else { // 1st entry 00271 set4ports(); 00272 cntl.pls_width = pls_width[cntl.change_cnt]; 00273 cntl.ongoing = 1; 00274 } 00275 break; 00276 default : 00277 cntl.state = M_STOP; 00278 } 00279 }
Generated on Thu Jul 14 2022 02:17:18 by 1.7.2