David Lowe / stepper

Modified phase table in stepper.cpp to enable stepping a bipolar motor using a twin H-bridge driver.

Fork of stepper by Kenji Arai

Corrected single-stepping, now walking up or down just one phase table. Compile-time options for driving bipolar motor in any of single-phase, two-phase, or half-stepping. Coils remain engaged at end of specifed movement command - de-energize coils by issuing a motor.move(0) while already stopped.

stepper.cpp@5:f2bbcd06019e, 2014-12-22 (annotated)

Committer:
gregeric
Date:
Mon Dec 22 21:15:55 2014 +0000
Revision:
5:f2bbcd06019e
Parent:
4:5b596b405573
Child:
6:ee6c96064559
Fix bug with direction change to CW for deceleration phase after issuing move(0)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:7b0c724fa658 1 /*
kenjiArai 0:7b0c724fa658 2 * mbed library program
kenjiArai 0:7b0c724fa658 3 * Stepping Motor
kenjiArai 0:7b0c724fa658 4 *
kenjiArai 0:7b0c724fa658 5 * Copyright (c) 2014 Kenji Arai / JH1PJL
kenjiArai 0:7b0c724fa658 6 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 0:7b0c724fa658 7 * http://mbed.org/users/kenjiArai/
kenjiArai 0:7b0c724fa658 8 * Created: August 20th, 2014
kenjiArai 0:7b0c724fa658 9 * Revised: August 23rd, 2014
kenjiArai 0:7b0c724fa658 10 *
kenjiArai 0:7b0c724fa658 11 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
kenjiArai 0:7b0c724fa658 12 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
kenjiArai 0:7b0c724fa658 13 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
kenjiArai 0:7b0c724fa658 14 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kenjiArai 0:7b0c724fa658 15 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
kenjiArai 0:7b0c724fa658 16 */
kenjiArai 0:7b0c724fa658 17
kenjiArai 0:7b0c724fa658 18 #include "mbed.h"
gregeric 4:5b596b405573 19 #include "stepper.h"
gregeric 3:96bfb8b476f8 20
gregeric 4:5b596b405573 21 //uncomment one option
gregeric 4:5b596b405573 22 //#define BIPOLAR_STEPPER_1PH
gregeric 4:5b596b405573 23 //#define BIPOLAR_STEPPER_2PH
gregeric 4:5b596b405573 24 #define BIPOLAR_STEPPER_12S
gregeric 4:5b596b405573 25 //#define UNIPOLAR_STEPPER_2PH
gregeric 3:96bfb8b476f8 26
gregeric 2:fd11d89b8ce0 27 /*
gregeric 4:5b596b405573 28 * Firing sequence for bi-polar (4 wires) stepper (2x H-Bridge driver required eg L298)
gregeric 3:96bfb8b476f8 29 */
gregeric 4:5b596b405573 30 #ifdef BIPOLAR_STEPPER_1PH
gregeric 4:5b596b405573 31 //single phase A+, B+, A-, B-
gregeric 4:5b596b405573 32 //motor wiring on H-bridge: A A' B B'
gregeric 3:96bfb8b476f8 33 const uint8_t pase_cw[4][4] = {{1, 0, 0, 0}, {0, 0, 1, 0}, {0, 1, 0, 0}, {0, 0, 0, 1}};
gregeric 4:5b596b405573 34 #endif
gregeric 4:5b596b405573 35
gregeric 4:5b596b405573 36 #ifdef BIPOLAR_STEPPER_2PH
gregeric 4:5b596b405573 37 //double phase A+B+, A-B+, A-B-, A+B-
gregeric 4:5b596b405573 38 const uint8_t pase_cw[4][4] = {{1, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 1}, {1, 0, 0, 1}};
gregeric 4:5b596b405573 39 #endif
gregeric 4:5b596b405573 40
gregeric 4:5b596b405573 41 #ifdef BIPOLAR_STEPPER_12S
gregeric 4:5b596b405573 42 //half step A+B+, A+, A+B-, B-, A-B-, A-, A-B+, B+
gregeric 4:5b596b405573 43 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}};
gregeric 4:5b596b405573 44 #endif
gregeric 4:5b596b405573 45
gregeric 3:96bfb8b476f8 46 /*
gregeric 3:96bfb8b476f8 47 * Firing sequence for uni-polar (5+ wires) stepper (4x open-collector drivers required).
gregeric 3:96bfb8b476f8 48 */
gregeric 4:5b596b405573 49 #ifdef UNIPOLAR_STEPPER_2PH
gregeric 5:f2bbcd06019e 50 //motor wiring A+ B+ A- B-
kenjiArai 0:7b0c724fa658 51 const uint8_t pase_cw[4][4] = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}};
gregeric 3:96bfb8b476f8 52 #endif
gregeric 2:fd11d89b8ce0 53
kenjiArai 0:7b0c724fa658 54 extern uint8_t pls_width[];
kenjiArai 0:7b0c724fa658 55
kenjiArai 0:7b0c724fa658 56 STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn):
kenjiArai 0:7b0c724fa658 57 _xp(xp), _xn(xn), _yp(yp), _yn(yn) {
kenjiArai 0:7b0c724fa658 58 init();
kenjiArai 0:7b0c724fa658 59 }
kenjiArai 0:7b0c724fa658 60
kenjiArai 0:7b0c724fa658 61 void STEPPER::move (int32_t steps){
kenjiArai 0:7b0c724fa658 62 if (steps < 0){
kenjiArai 0:7b0c724fa658 63 inf.direction = D_CCW;
kenjiArai 0:7b0c724fa658 64 steps = -steps;
gregeric 5:f2bbcd06019e 65 }
gregeric 5:f2bbcd06019e 66 // test for +ve, don't flip direction for stop command steps==0
gregeric 5:f2bbcd06019e 67 if (steps > 0){
gregeric 5:f2bbcd06019e 68 inf.direction = D_CW;
gregeric 5:f2bbcd06019e 69 }
kenjiArai 0:7b0c724fa658 70 inf.total_step = steps;
kenjiArai 0:7b0c724fa658 71 setup_mtr_drv_dt(&inf, &cntl);
kenjiArai 0:7b0c724fa658 72 }
kenjiArai 0:7b0c724fa658 73
kenjiArai 0:7b0c724fa658 74 void STEPPER::set_max_speed (uint32_t time_base_us){
kenjiArai 0:7b0c724fa658 75 if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)){
kenjiArai 0:7b0c724fa658 76 time_base_us = (MT_PLS_WIDTH_MIN * 1000);
kenjiArai 0:7b0c724fa658 77 }
kenjiArai 0:7b0c724fa658 78 _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us);
kenjiArai 0:7b0c724fa658 79 }
kenjiArai 0:7b0c724fa658 80
kenjiArai 0:7b0c724fa658 81 uint8_t STEPPER::status (void){
kenjiArai 0:7b0c724fa658 82 return cntl.state;
kenjiArai 0:7b0c724fa658 83 }
kenjiArai 0:7b0c724fa658 84
kenjiArai 0:7b0c724fa658 85 void STEPPER::init(void){
kenjiArai 0:7b0c724fa658 86 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 87 _xp = 0;
kenjiArai 0:7b0c724fa658 88 _xn = 0;
kenjiArai 0:7b0c724fa658 89 _yp = 0;
kenjiArai 0:7b0c724fa658 90 _yn = 0;
kenjiArai 0:7b0c724fa658 91 }
kenjiArai 0:7b0c724fa658 92
kenjiArai 0:7b0c724fa658 93 void STEPPER::set4ports (uint32_t step, int8_t dir){
kenjiArai 0:7b0c724fa658 94 uint8_t i;
kenjiArai 0:7b0c724fa658 95
gregeric 4:5b596b405573 96 i = (uint8_t)(step % (sizeof(pase_cw)/4) );
gregeric 4:5b596b405573 97 // if (dir == D_CW) {
kenjiArai 0:7b0c724fa658 98 _xp = pase_cw[i][0];
kenjiArai 0:7b0c724fa658 99 _xn = pase_cw[i][1];
kenjiArai 0:7b0c724fa658 100 _yp = pase_cw[i][2];
kenjiArai 0:7b0c724fa658 101 _yn = pase_cw[i][3];
gregeric 4:5b596b405573 102 /* } else {
kenjiArai 0:7b0c724fa658 103 _xp = pase_ccw[i][0];
kenjiArai 0:7b0c724fa658 104 _xn = pase_ccw[i][1];
kenjiArai 0:7b0c724fa658 105 _yp = pase_ccw[i][2];
kenjiArai 0:7b0c724fa658 106 _yn = pase_ccw[i][3];
kenjiArai 0:7b0c724fa658 107 }
gregeric 4:5b596b405573 108 */
kenjiArai 0:7b0c724fa658 109 }
kenjiArai 0:7b0c724fa658 110
kenjiArai 0:7b0c724fa658 111 void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt){
kenjiArai 0:7b0c724fa658 112 busy_sm_drv = 1;
gregeric 4:5b596b405573 113
gregeric 4:5b596b405573 114 //stop command, 0 step
kenjiArai 0:7b0c724fa658 115 if (mi->total_step == 0){
kenjiArai 0:7b0c724fa658 116 if (mt->state != M_STOP){
kenjiArai 0:7b0c724fa658 117 mt->state = M_CHANGE;
kenjiArai 0:7b0c724fa658 118 mt->change_cnt = 5;
kenjiArai 0:7b0c724fa658 119 mt->pls_width = 0;
kenjiArai 0:7b0c724fa658 120 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 121 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 122 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 123 mt->down_cnt = 0;
kenjiArai 0:7b0c724fa658 124 mt->continue_cnt = 0;
kenjiArai 0:7b0c724fa658 125 }
gregeric 4:5b596b405573 126 else init();// already stopped, interpret as: release HOLD currents, power down coils, motor freewheeling.
kenjiArai 0:7b0c724fa658 127 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 128 return;
kenjiArai 0:7b0c724fa658 129 }
gregeric 4:5b596b405573 130
gregeric 4:5b596b405573 131 //already travelling full speed, new step in same direction
kenjiArai 0:7b0c724fa658 132 if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)){
gregeric 4:5b596b405573 133 if (mi->total_step < MT_SLOP_STEP){ //step len shorter than ramp down ledn
kenjiArai 0:7b0c724fa658 134 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 135 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 136 mt->down_cnt = mi->total_step;
kenjiArai 0:7b0c724fa658 137 mt->continue_cnt = 0;
kenjiArai 0:7b0c724fa658 138 mt->state = M_DOWN;
kenjiArai 0:7b0c724fa658 139 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 140 } else {
kenjiArai 0:7b0c724fa658 141 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 142 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 143 mt->down_cnt = MT_SLOP_STEP -1;
kenjiArai 0:7b0c724fa658 144 mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1);
kenjiArai 0:7b0c724fa658 145 }
gregeric 4:5b596b405573 146 }
gregeric 4:5b596b405573 147 else {
gregeric 4:5b596b405573 148 //already moving, reverse direction required
kenjiArai 0:7b0c724fa658 149 if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)){
kenjiArai 0:7b0c724fa658 150 mt->state = M_CHANGE;
kenjiArai 0:7b0c724fa658 151 mt->change_cnt = 5;
gregeric 4:5b596b405573 152 } else {//motor was at rest?
gregeric 4:5b596b405573 153 //mt->motor_step = 0; // don't destroy knowledge of current phase
kenjiArai 0:7b0c724fa658 154 mt->state = M_UP;
kenjiArai 0:7b0c724fa658 155 }
kenjiArai 0:7b0c724fa658 156 mt->pls_width = 0;
kenjiArai 0:7b0c724fa658 157 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 158 mt->direction = mi->direction;
kenjiArai 0:7b0c724fa658 159 if (mi->total_step < MT_MIN_STEP){
kenjiArai 0:7b0c724fa658 160 if (mi->total_step == MT_MIN_STEP - 1){
kenjiArai 0:7b0c724fa658 161 mt->up_cnt = MT_SLOP_STEP;
kenjiArai 0:7b0c724fa658 162 } else {
kenjiArai 0:7b0c724fa658 163 mt->up_cnt = mi->total_step / 2;
gregeric 4:5b596b405573 164 if (mt->up_cnt==0) mt->up_cnt=1;
kenjiArai 0:7b0c724fa658 165 }
kenjiArai 0:7b0c724fa658 166 mt->up_cnt_keep = mt->up_cnt;
kenjiArai 0:7b0c724fa658 167 mt->down_cnt = mi->total_step - mt->up_cnt;
kenjiArai 0:7b0c724fa658 168 mt->continue_cnt = 0;
kenjiArai 0:7b0c724fa658 169 } else {
kenjiArai 0:7b0c724fa658 170 mt->up_cnt = MT_SLOP_STEP;
kenjiArai 0:7b0c724fa658 171 mt->up_cnt_keep = mt->up_cnt;
kenjiArai 0:7b0c724fa658 172 mt->down_cnt = MT_SLOP_STEP -1 ;
kenjiArai 0:7b0c724fa658 173 mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1);
kenjiArai 0:7b0c724fa658 174 }
kenjiArai 0:7b0c724fa658 175 }
kenjiArai 0:7b0c724fa658 176 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 177 }
kenjiArai 0:7b0c724fa658 178
kenjiArai 0:7b0c724fa658 179 void STEPPER::millisec_inteval() {
kenjiArai 0:7b0c724fa658 180 if (busy_sm_drv == 1){ return;}
kenjiArai 0:7b0c724fa658 181 switch (cntl.state){
kenjiArai 0:7b0c724fa658 182 case M_STOP:
gregeric 4:5b596b405573 183 /*
gregeric 4:5b596b405573 184 * continue to energize coils: hold current position
kenjiArai 0:7b0c724fa658 185 _xp = 0;
kenjiArai 0:7b0c724fa658 186 _xn = 0;
kenjiArai 0:7b0c724fa658 187 _yp = 0;
kenjiArai 0:7b0c724fa658 188 _yn = 0;
gregeric 4:5b596b405573 189 */
kenjiArai 0:7b0c724fa658 190 break;
kenjiArai 0:7b0c724fa658 191 case M_UP:
kenjiArai 0:7b0c724fa658 192 if (cntl.ongoing){
kenjiArai 0:7b0c724fa658 193 if (--cntl.pls_width == 0){
kenjiArai 0:7b0c724fa658 194 if (--cntl.up_cnt == 0){
kenjiArai 0:7b0c724fa658 195 cntl.ongoing = 0;
kenjiArai 0:7b0c724fa658 196 if (cntl.continue_cnt == 0){
kenjiArai 0:7b0c724fa658 197 cntl.state = M_DOWN;
gregeric 4:5b596b405573 198 if (cntl.down_cnt==0) cntl.state=M_STOP;
kenjiArai 0:7b0c724fa658 199 } else {
kenjiArai 0:7b0c724fa658 200 cntl.state = M_CONTINUE;
kenjiArai 0:7b0c724fa658 201 }
kenjiArai 0:7b0c724fa658 202 } else {
gregeric 4:5b596b405573 203 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 204 set4ports(cntl.motor_step, cntl.direction);
kenjiArai 0:7b0c724fa658 205 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
kenjiArai 0:7b0c724fa658 206 }
kenjiArai 0:7b0c724fa658 207 } else {
kenjiArai 0:7b0c724fa658 208 break;
kenjiArai 0:7b0c724fa658 209 }
kenjiArai 0:7b0c724fa658 210 } else { // 1st entry from M_STOP
gregeric 4:5b596b405573 211 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 212 set4ports(cntl.motor_step, cntl.direction);
kenjiArai 0:7b0c724fa658 213 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
kenjiArai 0:7b0c724fa658 214 cntl.ongoing = 1;
kenjiArai 0:7b0c724fa658 215 }
kenjiArai 0:7b0c724fa658 216 break;
kenjiArai 0:7b0c724fa658 217 case M_CONTINUE:
gregeric 4:5b596b405573 218 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 219 set4ports(cntl.motor_step, cntl.direction);
kenjiArai 0:7b0c724fa658 220 if (--cntl.continue_cnt == 0){
kenjiArai 0:7b0c724fa658 221 cntl.ongoing = 0;
kenjiArai 0:7b0c724fa658 222 cntl.state = M_DOWN;
kenjiArai 0:7b0c724fa658 223 }
kenjiArai 0:7b0c724fa658 224 break;
kenjiArai 0:7b0c724fa658 225 case M_DOWN:
kenjiArai 0:7b0c724fa658 226 if (cntl.ongoing){
kenjiArai 0:7b0c724fa658 227 if (--cntl.pls_width == 0){
kenjiArai 0:7b0c724fa658 228 if (--cntl.down_cnt == 0){
kenjiArai 0:7b0c724fa658 229 cntl.ongoing = 0;
kenjiArai 0:7b0c724fa658 230 cntl.state = M_STOP;
kenjiArai 0:7b0c724fa658 231 }else {
gregeric 4:5b596b405573 232 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 233 set4ports(cntl.motor_step, cntl.direction);
kenjiArai 0:7b0c724fa658 234 cntl.pls_width = pls_width[cntl.down_cnt];
kenjiArai 0:7b0c724fa658 235 }
kenjiArai 0:7b0c724fa658 236 } else {
kenjiArai 0:7b0c724fa658 237 break;
kenjiArai 0:7b0c724fa658 238 }
kenjiArai 0:7b0c724fa658 239 } else { // 1st entry from M_UP or M_CONTINUE
gregeric 4:5b596b405573 240 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 241 set4ports(cntl.motor_step, cntl.direction);
kenjiArai 0:7b0c724fa658 242 cntl.pls_width = pls_width[cntl.down_cnt];
kenjiArai 0:7b0c724fa658 243 cntl.ongoing = 1;
kenjiArai 0:7b0c724fa658 244 }
kenjiArai 0:7b0c724fa658 245 break;
kenjiArai 0:7b0c724fa658 246 case M_CHANGE:
kenjiArai 0:7b0c724fa658 247 if (cntl.ongoing){
kenjiArai 0:7b0c724fa658 248 if (--cntl.pls_width == 0){
kenjiArai 0:7b0c724fa658 249 if (--cntl.change_cnt == 0){
kenjiArai 0:7b0c724fa658 250 cntl.ongoing = 0;
kenjiArai 0:7b0c724fa658 251 if (cntl.up_cnt == 0){
kenjiArai 0:7b0c724fa658 252 cntl.state = M_STOP;
kenjiArai 0:7b0c724fa658 253 } else {
kenjiArai 0:7b0c724fa658 254 cntl.state = M_UP;
kenjiArai 0:7b0c724fa658 255 }
kenjiArai 0:7b0c724fa658 256 }else {
gregeric 4:5b596b405573 257 cntl.motor_step+=inf.direction;
gregeric 4:5b596b405573 258 set4ports(cntl.motor_step, D_CW);
kenjiArai 0:7b0c724fa658 259 cntl.pls_width = pls_width[cntl.change_cnt];
kenjiArai 0:7b0c724fa658 260 }
kenjiArai 0:7b0c724fa658 261 } else {
kenjiArai 0:7b0c724fa658 262 break;
kenjiArai 0:7b0c724fa658 263 }
kenjiArai 0:7b0c724fa658 264 } else { // 1st entry
gregeric 5:f2bbcd06019e 265 //needed??? cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 266 cntl.pls_width = pls_width[cntl.change_cnt];
kenjiArai 0:7b0c724fa658 267 cntl.ongoing = 1;
kenjiArai 0:7b0c724fa658 268 }
kenjiArai 0:7b0c724fa658 269 break;
kenjiArai 0:7b0c724fa658 270 default :
kenjiArai 0:7b0c724fa658 271 cntl.state = M_STOP;
kenjiArai 0:7b0c724fa658 272 }
kenjiArai 0:7b0c724fa658 273 }