Modified phase table in stepper.cpp to enable stepping a bipolar motor using a twin H-bridge driver.
Fork of stepper by
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@2:fd11d89b8ce0, 2014-12-18 (annotated)
- Committer:
- gregeric
- Date:
- Thu Dec 18 17:52:40 2014 +0000
- Revision:
- 2:fd11d89b8ce0
- Parent:
- 0:7b0c724fa658
- Child:
- 3:96bfb8b476f8
Phase sequence modified for bipolar stepper type, requiring one h-bridge driver for each coil.
Who changed what in which revision?
User | Revision | Line number | New 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" |
kenjiArai | 0:7b0c724fa658 | 19 | #include "stepper.h" |
gregeric | 2:fd11d89b8ce0 | 20 | /* |
gregeric | 2:fd11d89b8ce0 | 21 | //Firing sequence for uni-polar (5+ wires) stepper (4x open-collector drivers required). |
kenjiArai | 0:7b0c724fa658 | 22 | const uint8_t pase_cw[4][4] = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}}; |
kenjiArai | 0:7b0c724fa658 | 23 | const uint8_t pase_ccw[4][4] = {{1, 1, 0, 0}, {1, 0, 0, 1}, {0, 0, 1, 1}, {0, 1, 1, 0}}; |
gregeric | 2:fd11d89b8ce0 | 24 | */ |
gregeric | 2:fd11d89b8ce0 | 25 | //Firing sequence for bi-polar (4 wires) stepper (2x H-Bridge driver required eg L298 |
gregeric | 2:fd11d89b8ce0 | 26 | //A+, B+, A-, B-) |
gregeric | 2:fd11d89b8ce0 | 27 | const uint8_t pase_cw[4][4] = {{1, 0, 0, 0}, {0, 0, 1, 0}, {0, 1, 0, 0}, {0, 0, 0, 1}}; |
gregeric | 2:fd11d89b8ce0 | 28 | const uint8_t pase_ccw[4][4] = {{1, 0, 0, 0}, {0, 0, 0, 1}, {0, 1, 0, 0}, {0, 0, 1, 0}}; |
gregeric | 2:fd11d89b8ce0 | 29 | |
kenjiArai | 0:7b0c724fa658 | 30 | extern uint8_t pls_width[]; |
kenjiArai | 0:7b0c724fa658 | 31 | |
kenjiArai | 0:7b0c724fa658 | 32 | STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn): |
kenjiArai | 0:7b0c724fa658 | 33 | _xp(xp), _xn(xn), _yp(yp), _yn(yn) { |
kenjiArai | 0:7b0c724fa658 | 34 | init(); |
kenjiArai | 0:7b0c724fa658 | 35 | } |
kenjiArai | 0:7b0c724fa658 | 36 | |
kenjiArai | 0:7b0c724fa658 | 37 | void STEPPER::move (int32_t steps){ |
kenjiArai | 0:7b0c724fa658 | 38 | if (steps < 0){ |
kenjiArai | 0:7b0c724fa658 | 39 | inf.direction = D_CCW; |
kenjiArai | 0:7b0c724fa658 | 40 | steps = -steps; |
kenjiArai | 0:7b0c724fa658 | 41 | } else { |
kenjiArai | 0:7b0c724fa658 | 42 | inf.direction = D_CW; |
kenjiArai | 0:7b0c724fa658 | 43 | } |
kenjiArai | 0:7b0c724fa658 | 44 | inf.total_step = steps; |
kenjiArai | 0:7b0c724fa658 | 45 | setup_mtr_drv_dt(&inf, &cntl); |
kenjiArai | 0:7b0c724fa658 | 46 | } |
kenjiArai | 0:7b0c724fa658 | 47 | |
kenjiArai | 0:7b0c724fa658 | 48 | void STEPPER::set_max_speed (uint32_t time_base_us){ |
kenjiArai | 0:7b0c724fa658 | 49 | if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)){ |
kenjiArai | 0:7b0c724fa658 | 50 | time_base_us = (MT_PLS_WIDTH_MIN * 1000); |
kenjiArai | 0:7b0c724fa658 | 51 | } |
kenjiArai | 0:7b0c724fa658 | 52 | _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us); |
kenjiArai | 0:7b0c724fa658 | 53 | } |
kenjiArai | 0:7b0c724fa658 | 54 | |
kenjiArai | 0:7b0c724fa658 | 55 | uint8_t STEPPER::status (void){ |
kenjiArai | 0:7b0c724fa658 | 56 | return cntl.state; |
kenjiArai | 0:7b0c724fa658 | 57 | } |
kenjiArai | 0:7b0c724fa658 | 58 | |
kenjiArai | 0:7b0c724fa658 | 59 | void STEPPER::init(void){ |
kenjiArai | 0:7b0c724fa658 | 60 | busy_sm_drv = 0; |
kenjiArai | 0:7b0c724fa658 | 61 | _xp = 0; |
kenjiArai | 0:7b0c724fa658 | 62 | _xn = 0; |
kenjiArai | 0:7b0c724fa658 | 63 | _yp = 0; |
kenjiArai | 0:7b0c724fa658 | 64 | _yn = 0; |
kenjiArai | 0:7b0c724fa658 | 65 | } |
kenjiArai | 0:7b0c724fa658 | 66 | |
kenjiArai | 0:7b0c724fa658 | 67 | void STEPPER::set4ports (uint32_t step, int8_t dir){ |
kenjiArai | 0:7b0c724fa658 | 68 | uint8_t i; |
kenjiArai | 0:7b0c724fa658 | 69 | |
kenjiArai | 0:7b0c724fa658 | 70 | i = (uint8_t)(step % 4); |
kenjiArai | 0:7b0c724fa658 | 71 | if (dir == D_CW) { |
kenjiArai | 0:7b0c724fa658 | 72 | _xp = pase_cw[i][0]; |
kenjiArai | 0:7b0c724fa658 | 73 | _xn = pase_cw[i][1]; |
kenjiArai | 0:7b0c724fa658 | 74 | _yp = pase_cw[i][2]; |
kenjiArai | 0:7b0c724fa658 | 75 | _yn = pase_cw[i][3]; |
kenjiArai | 0:7b0c724fa658 | 76 | } else { |
kenjiArai | 0:7b0c724fa658 | 77 | _xp = pase_ccw[i][0]; |
kenjiArai | 0:7b0c724fa658 | 78 | _xn = pase_ccw[i][1]; |
kenjiArai | 0:7b0c724fa658 | 79 | _yp = pase_ccw[i][2]; |
kenjiArai | 0:7b0c724fa658 | 80 | _yn = pase_ccw[i][3]; |
kenjiArai | 0:7b0c724fa658 | 81 | } |
kenjiArai | 0:7b0c724fa658 | 82 | } |
kenjiArai | 0:7b0c724fa658 | 83 | |
kenjiArai | 0:7b0c724fa658 | 84 | void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt){ |
kenjiArai | 0:7b0c724fa658 | 85 | busy_sm_drv = 1; |
kenjiArai | 0:7b0c724fa658 | 86 | if (mi->total_step == 0){ |
kenjiArai | 0:7b0c724fa658 | 87 | if (mt->state != M_STOP){ |
kenjiArai | 0:7b0c724fa658 | 88 | mt->state = M_CHANGE; |
kenjiArai | 0:7b0c724fa658 | 89 | mt->change_cnt = 5; |
kenjiArai | 0:7b0c724fa658 | 90 | mt->pls_width = 0; |
kenjiArai | 0:7b0c724fa658 | 91 | mt->ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 92 | mt->up_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 93 | mt->up_cnt_keep = 0; |
kenjiArai | 0:7b0c724fa658 | 94 | mt->down_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 95 | mt->continue_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 96 | } |
kenjiArai | 0:7b0c724fa658 | 97 | busy_sm_drv = 0; |
kenjiArai | 0:7b0c724fa658 | 98 | return; |
kenjiArai | 0:7b0c724fa658 | 99 | } |
kenjiArai | 0:7b0c724fa658 | 100 | if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)){ |
kenjiArai | 0:7b0c724fa658 | 101 | if (mi->total_step < MT_SLOP_STEP){ |
kenjiArai | 0:7b0c724fa658 | 102 | mt->up_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 103 | mt->up_cnt_keep = 0; |
kenjiArai | 0:7b0c724fa658 | 104 | mt->down_cnt = mi->total_step; |
kenjiArai | 0:7b0c724fa658 | 105 | mt->continue_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 106 | mt->state = M_DOWN; |
kenjiArai | 0:7b0c724fa658 | 107 | mt->ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 108 | } else { |
kenjiArai | 0:7b0c724fa658 | 109 | mt->up_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 110 | mt->up_cnt_keep = 0; |
kenjiArai | 0:7b0c724fa658 | 111 | mt->down_cnt = MT_SLOP_STEP -1; |
kenjiArai | 0:7b0c724fa658 | 112 | mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1); |
kenjiArai | 0:7b0c724fa658 | 113 | } |
kenjiArai | 0:7b0c724fa658 | 114 | } else { |
kenjiArai | 0:7b0c724fa658 | 115 | if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)){ |
kenjiArai | 0:7b0c724fa658 | 116 | mt->state = M_CHANGE; |
kenjiArai | 0:7b0c724fa658 | 117 | mt->change_cnt = 5; |
kenjiArai | 0:7b0c724fa658 | 118 | } else { |
kenjiArai | 0:7b0c724fa658 | 119 | mt->motor_step = 0; |
kenjiArai | 0:7b0c724fa658 | 120 | mt->state = M_UP; |
kenjiArai | 0:7b0c724fa658 | 121 | } |
kenjiArai | 0:7b0c724fa658 | 122 | mt->pls_width = 0; |
kenjiArai | 0:7b0c724fa658 | 123 | mt->ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 124 | mt->direction = mi->direction; |
kenjiArai | 0:7b0c724fa658 | 125 | if (mi->total_step < MT_MIN_STEP){ |
kenjiArai | 0:7b0c724fa658 | 126 | if (mi->total_step == MT_MIN_STEP - 1){ |
kenjiArai | 0:7b0c724fa658 | 127 | mt->up_cnt = MT_SLOP_STEP; |
kenjiArai | 0:7b0c724fa658 | 128 | } else { |
kenjiArai | 0:7b0c724fa658 | 129 | mt->up_cnt = mi->total_step / 2; |
kenjiArai | 0:7b0c724fa658 | 130 | } |
kenjiArai | 0:7b0c724fa658 | 131 | mt->up_cnt_keep = mt->up_cnt; |
kenjiArai | 0:7b0c724fa658 | 132 | mt->down_cnt = mi->total_step - mt->up_cnt; |
kenjiArai | 0:7b0c724fa658 | 133 | mt->continue_cnt = 0; |
kenjiArai | 0:7b0c724fa658 | 134 | } else { |
kenjiArai | 0:7b0c724fa658 | 135 | mt->up_cnt = MT_SLOP_STEP; |
kenjiArai | 0:7b0c724fa658 | 136 | mt->up_cnt_keep = mt->up_cnt; |
kenjiArai | 0:7b0c724fa658 | 137 | mt->down_cnt = MT_SLOP_STEP -1 ; |
kenjiArai | 0:7b0c724fa658 | 138 | mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1); |
kenjiArai | 0:7b0c724fa658 | 139 | } |
kenjiArai | 0:7b0c724fa658 | 140 | } |
kenjiArai | 0:7b0c724fa658 | 141 | busy_sm_drv = 0; |
kenjiArai | 0:7b0c724fa658 | 142 | } |
kenjiArai | 0:7b0c724fa658 | 143 | |
kenjiArai | 0:7b0c724fa658 | 144 | void STEPPER::millisec_inteval() { |
kenjiArai | 0:7b0c724fa658 | 145 | if (busy_sm_drv == 1){ return;} |
kenjiArai | 0:7b0c724fa658 | 146 | switch (cntl.state){ |
kenjiArai | 0:7b0c724fa658 | 147 | case M_STOP: |
kenjiArai | 0:7b0c724fa658 | 148 | _xp = 0; |
kenjiArai | 0:7b0c724fa658 | 149 | _xn = 0; |
kenjiArai | 0:7b0c724fa658 | 150 | _yp = 0; |
kenjiArai | 0:7b0c724fa658 | 151 | _yn = 0; |
kenjiArai | 0:7b0c724fa658 | 152 | break; |
kenjiArai | 0:7b0c724fa658 | 153 | case M_UP: |
kenjiArai | 0:7b0c724fa658 | 154 | if (cntl.ongoing){ |
kenjiArai | 0:7b0c724fa658 | 155 | if (--cntl.pls_width == 0){ |
kenjiArai | 0:7b0c724fa658 | 156 | if (--cntl.up_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 157 | cntl.ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 158 | if (cntl.continue_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 159 | cntl.state = M_DOWN; |
kenjiArai | 0:7b0c724fa658 | 160 | } else { |
kenjiArai | 0:7b0c724fa658 | 161 | cntl.state = M_CONTINUE; |
kenjiArai | 0:7b0c724fa658 | 162 | } |
kenjiArai | 0:7b0c724fa658 | 163 | } else { |
kenjiArai | 0:7b0c724fa658 | 164 | set4ports(cntl.motor_step, cntl.direction); |
kenjiArai | 0:7b0c724fa658 | 165 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 166 | cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt]; |
kenjiArai | 0:7b0c724fa658 | 167 | } |
kenjiArai | 0:7b0c724fa658 | 168 | } else { |
kenjiArai | 0:7b0c724fa658 | 169 | break; |
kenjiArai | 0:7b0c724fa658 | 170 | } |
kenjiArai | 0:7b0c724fa658 | 171 | } else { // 1st entry from M_STOP |
kenjiArai | 0:7b0c724fa658 | 172 | set4ports(cntl.motor_step, cntl.direction); |
kenjiArai | 0:7b0c724fa658 | 173 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 174 | cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt]; |
kenjiArai | 0:7b0c724fa658 | 175 | cntl.ongoing = 1; |
kenjiArai | 0:7b0c724fa658 | 176 | } |
kenjiArai | 0:7b0c724fa658 | 177 | break; |
kenjiArai | 0:7b0c724fa658 | 178 | case M_CONTINUE: |
kenjiArai | 0:7b0c724fa658 | 179 | set4ports(cntl.motor_step, cntl.direction); |
kenjiArai | 0:7b0c724fa658 | 180 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 181 | if (--cntl.continue_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 182 | cntl.ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 183 | cntl.state = M_DOWN; |
kenjiArai | 0:7b0c724fa658 | 184 | } |
kenjiArai | 0:7b0c724fa658 | 185 | break; |
kenjiArai | 0:7b0c724fa658 | 186 | case M_DOWN: |
kenjiArai | 0:7b0c724fa658 | 187 | if (cntl.ongoing){ |
kenjiArai | 0:7b0c724fa658 | 188 | if (--cntl.pls_width == 0){ |
kenjiArai | 0:7b0c724fa658 | 189 | if (--cntl.down_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 190 | cntl.ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 191 | cntl.state = M_STOP; |
kenjiArai | 0:7b0c724fa658 | 192 | }else { |
kenjiArai | 0:7b0c724fa658 | 193 | set4ports(cntl.motor_step, cntl.direction); |
kenjiArai | 0:7b0c724fa658 | 194 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 195 | cntl.pls_width = pls_width[cntl.down_cnt]; |
kenjiArai | 0:7b0c724fa658 | 196 | } |
kenjiArai | 0:7b0c724fa658 | 197 | } else { |
kenjiArai | 0:7b0c724fa658 | 198 | break; |
kenjiArai | 0:7b0c724fa658 | 199 | } |
kenjiArai | 0:7b0c724fa658 | 200 | } else { // 1st entry from M_UP or M_CONTINUE |
kenjiArai | 0:7b0c724fa658 | 201 | set4ports(cntl.motor_step, cntl.direction); |
kenjiArai | 0:7b0c724fa658 | 202 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 203 | cntl.pls_width = pls_width[cntl.down_cnt]; |
kenjiArai | 0:7b0c724fa658 | 204 | cntl.ongoing = 1; |
kenjiArai | 0:7b0c724fa658 | 205 | } |
kenjiArai | 0:7b0c724fa658 | 206 | break; |
kenjiArai | 0:7b0c724fa658 | 207 | case M_CHANGE: |
kenjiArai | 0:7b0c724fa658 | 208 | if (cntl.ongoing){ |
kenjiArai | 0:7b0c724fa658 | 209 | if (--cntl.pls_width == 0){ |
kenjiArai | 0:7b0c724fa658 | 210 | if (--cntl.change_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 211 | cntl.ongoing = 0; |
kenjiArai | 0:7b0c724fa658 | 212 | if (cntl.up_cnt == 0){ |
kenjiArai | 0:7b0c724fa658 | 213 | cntl.state = M_STOP; |
kenjiArai | 0:7b0c724fa658 | 214 | } else { |
kenjiArai | 0:7b0c724fa658 | 215 | cntl.state = M_UP; |
kenjiArai | 0:7b0c724fa658 | 216 | } |
kenjiArai | 0:7b0c724fa658 | 217 | }else { |
kenjiArai | 0:7b0c724fa658 | 218 | if (cntl.direction == D_CW){ |
kenjiArai | 0:7b0c724fa658 | 219 | set4ports(cntl.motor_step, D_CCW); |
kenjiArai | 0:7b0c724fa658 | 220 | } else { |
kenjiArai | 0:7b0c724fa658 | 221 | set4ports(cntl.motor_step, D_CW); |
kenjiArai | 0:7b0c724fa658 | 222 | } |
kenjiArai | 0:7b0c724fa658 | 223 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 224 | cntl.pls_width = pls_width[cntl.change_cnt]; |
kenjiArai | 0:7b0c724fa658 | 225 | } |
kenjiArai | 0:7b0c724fa658 | 226 | } else { |
kenjiArai | 0:7b0c724fa658 | 227 | break; |
kenjiArai | 0:7b0c724fa658 | 228 | } |
kenjiArai | 0:7b0c724fa658 | 229 | } else { // 1st entry |
kenjiArai | 0:7b0c724fa658 | 230 | if (cntl.direction == D_CW){ |
kenjiArai | 0:7b0c724fa658 | 231 | set4ports(cntl.motor_step, D_CCW); |
kenjiArai | 0:7b0c724fa658 | 232 | } else { |
kenjiArai | 0:7b0c724fa658 | 233 | set4ports(cntl.motor_step, D_CW); |
kenjiArai | 0:7b0c724fa658 | 234 | } |
kenjiArai | 0:7b0c724fa658 | 235 | cntl.motor_step++; |
kenjiArai | 0:7b0c724fa658 | 236 | cntl.pls_width = pls_width[cntl.change_cnt]; |
kenjiArai | 0:7b0c724fa658 | 237 | cntl.ongoing = 1; |
kenjiArai | 0:7b0c724fa658 | 238 | } |
kenjiArai | 0:7b0c724fa658 | 239 | break; |
kenjiArai | 0:7b0c724fa658 | 240 | default : |
kenjiArai | 0:7b0c724fa658 | 241 | cntl.state = M_STOP; |
kenjiArai | 0:7b0c724fa658 | 242 | } |
kenjiArai | 0:7b0c724fa658 | 243 | } |