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