David Lowe / stepper

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of stepper by Kenji Arai

stepper.cpp@6:ee6c96064559, 2014-12-22 (annotated)

Committer:
gregeric
Date:
Mon Dec 22 21:49:05 2014 +0000
Revision:
6:ee6c96064559
Parent:
5:f2bbcd06019e
Child:
7:9fc4b1be489c
Fix the fix

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 6:ee6c96064559 67 else 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 }