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@7:9fc4b1be489c, 2014-12-23 (annotated)

Committer:
gregeric
Date:
Tue Dec 23 10:36:58 2014 +0000
Revision:
7:9fc4b1be489c
Parent:
6:ee6c96064559
Child:
8:75fcbd49d49f
Tidy up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:7b0c724fa658 1 /*
gregeric 7:9fc4b1be489c 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 7:9fc4b1be489c 25 //#define UNIPOLAR_STEPPER_1PH
gregeric 4:5b596b405573 26 //#define UNIPOLAR_STEPPER_2PH
gregeric 7:9fc4b1be489c 27 //#define UNIPOLAR_STEPPER_12S
gregeric 2:fd11d89b8ce0 28 /*
gregeric 4:5b596b405573 29 * Firing sequence for bi-polar (4 wires) stepper (2x H-Bridge driver required eg L298)
gregeric 3:96bfb8b476f8 30 */
gregeric 4:5b596b405573 31 #ifdef BIPOLAR_STEPPER_1PH
gregeric 4:5b596b405573 32 //single phase A+, B+, A-, B-
gregeric 4:5b596b405573 33 //motor wiring on H-bridge: A A' B B'
gregeric 3:96bfb8b476f8 34 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 35 #endif
gregeric 4:5b596b405573 36
gregeric 4:5b596b405573 37 #ifdef BIPOLAR_STEPPER_2PH
gregeric 7:9fc4b1be489c 38 //double phase A+B+, A-B+, A-B-, A+B-
gregeric 4:5b596b405573 39 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 40 #endif
gregeric 4:5b596b405573 41
gregeric 4:5b596b405573 42 #ifdef BIPOLAR_STEPPER_12S
gregeric 4:5b596b405573 43 //half step A+B+, A+, A+B-, B-, A-B-, A-, A-B+, B+
gregeric 4:5b596b405573 44 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 45 #endif
gregeric 4:5b596b405573 46
gregeric 3:96bfb8b476f8 47 /*
gregeric 7:9fc4b1be489c 48 * Firing sequence for uni-polar (5+ wires) stepper (4x open-collector drivers required).
gregeric 3:96bfb8b476f8 49 */
gregeric 7:9fc4b1be489c 50 #ifdef UNIPOLAR_STEPPER_1PH
gregeric 7:9fc4b1be489c 51 //motor wiring A+ B+ A- B-
gregeric 7:9fc4b1be489c 52 const uint8_t pase_cw[4][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
gregeric 7:9fc4b1be489c 53 #endif
gregeric 7:9fc4b1be489c 54
gregeric 4:5b596b405573 55 #ifdef UNIPOLAR_STEPPER_2PH
gregeric 5:f2bbcd06019e 56 //motor wiring A+ B+ A- B-
kenjiArai 0:7b0c724fa658 57 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 58 #endif
gregeric 2:fd11d89b8ce0 59
gregeric 7:9fc4b1be489c 60 #ifdef UNIPOLAR_STEPPER_12S
gregeric 7:9fc4b1be489c 61 //motor wiring A+ B+ A- B-
gregeric 7:9fc4b1be489c 62 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}};
gregeric 7:9fc4b1be489c 63 #endif
gregeric 7:9fc4b1be489c 64
kenjiArai 0:7b0c724fa658 65 extern uint8_t pls_width[];
kenjiArai 0:7b0c724fa658 66
kenjiArai 0:7b0c724fa658 67 STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn):
gregeric 7:9fc4b1be489c 68 _xp(xp), _xn(xn), _yp(yp), _yn(yn)
gregeric 7:9fc4b1be489c 69 {
kenjiArai 0:7b0c724fa658 70 init();
kenjiArai 0:7b0c724fa658 71 }
kenjiArai 0:7b0c724fa658 72
gregeric 7:9fc4b1be489c 73 void STEPPER::move (int32_t steps)
gregeric 7:9fc4b1be489c 74 {
gregeric 7:9fc4b1be489c 75 if (steps < 0) {
kenjiArai 0:7b0c724fa658 76 inf.direction = D_CCW;
kenjiArai 0:7b0c724fa658 77 steps = -steps;
gregeric 5:f2bbcd06019e 78 }
gregeric 5:f2bbcd06019e 79 // test for +ve, don't flip direction for stop command steps==0
gregeric 7:9fc4b1be489c 80 else if (steps > 0) {
gregeric 7:9fc4b1be489c 81 inf.direction = D_CW;
gregeric 5:f2bbcd06019e 82 }
kenjiArai 0:7b0c724fa658 83 inf.total_step = steps;
kenjiArai 0:7b0c724fa658 84 setup_mtr_drv_dt(&inf, &cntl);
kenjiArai 0:7b0c724fa658 85 }
kenjiArai 0:7b0c724fa658 86
gregeric 7:9fc4b1be489c 87 void STEPPER::set_max_speed (uint32_t time_base_us)
gregeric 7:9fc4b1be489c 88 {
gregeric 7:9fc4b1be489c 89 if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)) {
kenjiArai 0:7b0c724fa658 90 time_base_us = (MT_PLS_WIDTH_MIN * 1000);
kenjiArai 0:7b0c724fa658 91 }
kenjiArai 0:7b0c724fa658 92 _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us);
kenjiArai 0:7b0c724fa658 93 }
kenjiArai 0:7b0c724fa658 94
gregeric 7:9fc4b1be489c 95 uint8_t STEPPER::status (void)
gregeric 7:9fc4b1be489c 96 {
kenjiArai 0:7b0c724fa658 97 return cntl.state;
kenjiArai 0:7b0c724fa658 98 }
kenjiArai 0:7b0c724fa658 99
gregeric 7:9fc4b1be489c 100 void STEPPER::init(void)
gregeric 7:9fc4b1be489c 101 {
kenjiArai 0:7b0c724fa658 102 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 103 _xp = 0;
kenjiArai 0:7b0c724fa658 104 _xn = 0;
kenjiArai 0:7b0c724fa658 105 _yp = 0;
kenjiArai 0:7b0c724fa658 106 _yn = 0;
kenjiArai 0:7b0c724fa658 107 }
kenjiArai 0:7b0c724fa658 108
gregeric 7:9fc4b1be489c 109 void STEPPER::set4ports (uint32_t step, int8_t dir)
gregeric 7:9fc4b1be489c 110 {
gregeric 7:9fc4b1be489c 111 uint8_t i;
gregeric 4:5b596b405573 112 i = (uint8_t)(step % (sizeof(pase_cw)/4) );
gregeric 7:9fc4b1be489c 113 _xp = pase_cw[i][0];
gregeric 7:9fc4b1be489c 114 _xn = pase_cw[i][1];
gregeric 7:9fc4b1be489c 115 _yp = pase_cw[i][2];
gregeric 7:9fc4b1be489c 116 _yn = pase_cw[i][3];
kenjiArai 0:7b0c724fa658 117 }
kenjiArai 0:7b0c724fa658 118
gregeric 7:9fc4b1be489c 119 void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt)
gregeric 7:9fc4b1be489c 120 {
kenjiArai 0:7b0c724fa658 121 busy_sm_drv = 1;
gregeric 7:9fc4b1be489c 122
gregeric 4:5b596b405573 123 //stop command, 0 step
gregeric 7:9fc4b1be489c 124 if (mi->total_step == 0) {
gregeric 7:9fc4b1be489c 125 if (mt->state != M_STOP) {
kenjiArai 0:7b0c724fa658 126 mt->state = M_CHANGE;
kenjiArai 0:7b0c724fa658 127 mt->change_cnt = 5;
kenjiArai 0:7b0c724fa658 128 mt->pls_width = 0;
kenjiArai 0:7b0c724fa658 129 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 130 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 131 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 132 mt->down_cnt = 0;
kenjiArai 0:7b0c724fa658 133 mt->continue_cnt = 0;
gregeric 7:9fc4b1be489c 134 } else init(); // already stopped, interpret as: release HOLD currents, power down coils, motor freewheeling.
kenjiArai 0:7b0c724fa658 135 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 136 return;
kenjiArai 0:7b0c724fa658 137 }
gregeric 7:9fc4b1be489c 138
gregeric 4:5b596b405573 139 //already travelling full speed, new step in same direction
gregeric 7:9fc4b1be489c 140 if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)) {
gregeric 7:9fc4b1be489c 141 if (mi->total_step < MT_SLOP_STEP) { //step len shorter than ramp down ledn
kenjiArai 0:7b0c724fa658 142 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 143 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 144 mt->down_cnt = mi->total_step;
kenjiArai 0:7b0c724fa658 145 mt->continue_cnt = 0;
kenjiArai 0:7b0c724fa658 146 mt->state = M_DOWN;
kenjiArai 0:7b0c724fa658 147 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 148 } else {
kenjiArai 0:7b0c724fa658 149 mt->up_cnt = 0;
kenjiArai 0:7b0c724fa658 150 mt->up_cnt_keep = 0;
kenjiArai 0:7b0c724fa658 151 mt->down_cnt = MT_SLOP_STEP -1;
kenjiArai 0:7b0c724fa658 152 mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1);
kenjiArai 0:7b0c724fa658 153 }
gregeric 7:9fc4b1be489c 154 } else {
gregeric 7:9fc4b1be489c 155 //already moving, reverse direction required
gregeric 7:9fc4b1be489c 156 if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)) {
kenjiArai 0:7b0c724fa658 157 mt->state = M_CHANGE;
kenjiArai 0:7b0c724fa658 158 mt->change_cnt = 5;
gregeric 4:5b596b405573 159 } else {//motor was at rest?
gregeric 4:5b596b405573 160 //mt->motor_step = 0; // don't destroy knowledge of current phase
gregeric 7:9fc4b1be489c 161 mt->state = M_UP;
kenjiArai 0:7b0c724fa658 162 }
kenjiArai 0:7b0c724fa658 163 mt->pls_width = 0;
gregeric 7:9fc4b1be489c 164 mt->ongoing = 0;
kenjiArai 0:7b0c724fa658 165 mt->direction = mi->direction;
gregeric 7:9fc4b1be489c 166 if (mi->total_step < MT_MIN_STEP) {
gregeric 7:9fc4b1be489c 167 if (mi->total_step == MT_MIN_STEP - 1) {
kenjiArai 0:7b0c724fa658 168 mt->up_cnt = MT_SLOP_STEP;
kenjiArai 0:7b0c724fa658 169 } else {
kenjiArai 0:7b0c724fa658 170 mt->up_cnt = mi->total_step / 2;
gregeric 4:5b596b405573 171 if (mt->up_cnt==0) mt->up_cnt=1;
kenjiArai 0:7b0c724fa658 172 }
kenjiArai 0:7b0c724fa658 173 mt->up_cnt_keep = mt->up_cnt;
kenjiArai 0:7b0c724fa658 174 mt->down_cnt = mi->total_step - mt->up_cnt;
kenjiArai 0:7b0c724fa658 175 mt->continue_cnt = 0;
kenjiArai 0:7b0c724fa658 176 } else {
kenjiArai 0:7b0c724fa658 177 mt->up_cnt = MT_SLOP_STEP;
kenjiArai 0:7b0c724fa658 178 mt->up_cnt_keep = mt->up_cnt;
kenjiArai 0:7b0c724fa658 179 mt->down_cnt = MT_SLOP_STEP -1 ;
kenjiArai 0:7b0c724fa658 180 mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1);
kenjiArai 0:7b0c724fa658 181 }
kenjiArai 0:7b0c724fa658 182 }
kenjiArai 0:7b0c724fa658 183 busy_sm_drv = 0;
kenjiArai 0:7b0c724fa658 184 }
kenjiArai 0:7b0c724fa658 185
gregeric 7:9fc4b1be489c 186 void STEPPER::millisec_inteval()
gregeric 7:9fc4b1be489c 187 {
gregeric 7:9fc4b1be489c 188 if (busy_sm_drv == 1) {
gregeric 7:9fc4b1be489c 189 return;
gregeric 7:9fc4b1be489c 190 }
gregeric 7:9fc4b1be489c 191 switch (cntl.state) {
gregeric 7:9fc4b1be489c 192 case M_STOP:
gregeric 7:9fc4b1be489c 193 /*
gregeric 7:9fc4b1be489c 194 * continue to energize coils: hold current position
gregeric 7:9fc4b1be489c 195 _xp = 0;
gregeric 7:9fc4b1be489c 196 _xn = 0;
gregeric 7:9fc4b1be489c 197 _yp = 0;
gregeric 7:9fc4b1be489c 198 _yn = 0;
gregeric 7:9fc4b1be489c 199 */
gregeric 7:9fc4b1be489c 200 break;
gregeric 7:9fc4b1be489c 201 case M_UP:
gregeric 7:9fc4b1be489c 202 if (cntl.ongoing) {
gregeric 7:9fc4b1be489c 203 if (--cntl.pls_width == 0) {
gregeric 7:9fc4b1be489c 204 if (--cntl.up_cnt == 0) {
gregeric 7:9fc4b1be489c 205 cntl.ongoing = 0;
gregeric 7:9fc4b1be489c 206 if (cntl.continue_cnt == 0) {
gregeric 7:9fc4b1be489c 207 cntl.state = M_DOWN;
gregeric 7:9fc4b1be489c 208 if (cntl.down_cnt==0) cntl.state=M_STOP;
gregeric 7:9fc4b1be489c 209 } else {
gregeric 7:9fc4b1be489c 210 cntl.state = M_CONTINUE;
gregeric 7:9fc4b1be489c 211 }
kenjiArai 0:7b0c724fa658 212 } else {
gregeric 7:9fc4b1be489c 213 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 214 set4ports(cntl.motor_step, cntl.direction);
gregeric 7:9fc4b1be489c 215 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
kenjiArai 0:7b0c724fa658 216 }
kenjiArai 0:7b0c724fa658 217 } else {
gregeric 7:9fc4b1be489c 218 break;
kenjiArai 0:7b0c724fa658 219 }
gregeric 7:9fc4b1be489c 220 } else { // 1st entry from M_STOP
gregeric 7:9fc4b1be489c 221 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 222 set4ports(cntl.motor_step, cntl.direction);
gregeric 7:9fc4b1be489c 223 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
gregeric 7:9fc4b1be489c 224 cntl.ongoing = 1;
kenjiArai 0:7b0c724fa658 225 }
gregeric 7:9fc4b1be489c 226 break;
gregeric 7:9fc4b1be489c 227 case M_CONTINUE:
gregeric 4:5b596b405573 228 cntl.motor_step+=inf.direction;
kenjiArai 0:7b0c724fa658 229 set4ports(cntl.motor_step, cntl.direction);
gregeric 7:9fc4b1be489c 230 if (--cntl.continue_cnt == 0) {
gregeric 7:9fc4b1be489c 231 cntl.ongoing = 0;
gregeric 7:9fc4b1be489c 232 cntl.state = M_DOWN;
kenjiArai 0:7b0c724fa658 233 }
gregeric 7:9fc4b1be489c 234 break;
gregeric 7:9fc4b1be489c 235 case M_DOWN:
gregeric 7:9fc4b1be489c 236 if (cntl.ongoing) {
gregeric 7:9fc4b1be489c 237 if (--cntl.pls_width == 0) {
gregeric 7:9fc4b1be489c 238 if (--cntl.down_cnt == 0) {
gregeric 7:9fc4b1be489c 239 cntl.ongoing = 0;
kenjiArai 0:7b0c724fa658 240 cntl.state = M_STOP;
kenjiArai 0:7b0c724fa658 241 } else {
gregeric 7:9fc4b1be489c 242 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 243 set4ports(cntl.motor_step, cntl.direction);
gregeric 7:9fc4b1be489c 244 cntl.pls_width = pls_width[cntl.down_cnt];
kenjiArai 0:7b0c724fa658 245 }
gregeric 7:9fc4b1be489c 246 } else {
gregeric 7:9fc4b1be489c 247 break;
kenjiArai 0:7b0c724fa658 248 }
gregeric 7:9fc4b1be489c 249 } else { // 1st entry from M_UP or M_CONTINUE
gregeric 7:9fc4b1be489c 250 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 251 set4ports(cntl.motor_step, cntl.direction);
gregeric 7:9fc4b1be489c 252 cntl.pls_width = pls_width[cntl.down_cnt];
gregeric 7:9fc4b1be489c 253 cntl.ongoing = 1;
kenjiArai 0:7b0c724fa658 254 }
gregeric 7:9fc4b1be489c 255 break;
gregeric 7:9fc4b1be489c 256 case M_CHANGE:
gregeric 7:9fc4b1be489c 257 if (cntl.ongoing) {
gregeric 7:9fc4b1be489c 258 if (--cntl.pls_width == 0) {
gregeric 7:9fc4b1be489c 259 if (--cntl.change_cnt == 0) {
gregeric 7:9fc4b1be489c 260 cntl.ongoing = 0;
gregeric 7:9fc4b1be489c 261 if (cntl.up_cnt == 0) {
gregeric 7:9fc4b1be489c 262 cntl.state = M_STOP;
gregeric 7:9fc4b1be489c 263 } else {
gregeric 7:9fc4b1be489c 264 cntl.state = M_UP;
gregeric 7:9fc4b1be489c 265 }
gregeric 7:9fc4b1be489c 266 } else {
gregeric 7:9fc4b1be489c 267 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 268 set4ports(cntl.motor_step, D_CW);
gregeric 7:9fc4b1be489c 269 cntl.pls_width = pls_width[cntl.change_cnt];
gregeric 7:9fc4b1be489c 270 }
gregeric 7:9fc4b1be489c 271 } else {
gregeric 7:9fc4b1be489c 272 break;
gregeric 7:9fc4b1be489c 273 }
gregeric 7:9fc4b1be489c 274 } else { // 1st entry
gregeric 7:9fc4b1be489c 275 cntl.motor_step+=inf.direction;
gregeric 7:9fc4b1be489c 276 set4ports(cntl.motor_step, D_CW);
gregeric 7:9fc4b1be489c 277 cntl.pls_width = pls_width[cntl.change_cnt];
gregeric 7:9fc4b1be489c 278 cntl.ongoing = 1;
gregeric 7:9fc4b1be489c 279 }
gregeric 7:9fc4b1be489c 280 break;
gregeric 7:9fc4b1be489c 281 default :
gregeric 7:9fc4b1be489c 282 cntl.state = M_STOP;
kenjiArai 0:7b0c724fa658 283 }
kenjiArai 0:7b0c724fa658 284 }