Kenji Arai / stepper

Stepper Motor Driver library for Uni-polar type

Dependents:   NucleoL152_stepper_w_lib

Please refer my notebook.
http://mbed.org/users/kenjiArai/notebook/stepping-motor-control-unipolar-type/
I have tested the program following mbed boards.

  • mbed ST Nucleo L152RE
  • mbed ST Nucleo F401RE
  • mbed LPC1114FN28

stepper.cpp@1:94f55ebfe2db, 2014-08-23 (annotated)

Committer:
kenjiArai
Date:
Sat Aug 23 06:45:41 2014 +0000
Revision:
1:94f55ebfe2db
Parent:
0:7b0c724fa658 
modified comments and cosmetic parts

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