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@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?
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" |
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 | } |