File content as of revision 1:94f55ebfe2db:

/*
 * mbed library program 
 *      Stepping Motor
 *
 * Copyright (c) 2014 Kenji Arai / JH1PJL
 *  http://www.page.sannet.ne.jp/kenjia/index.html
 *  http://mbed.org/users/kenjiArai/
 *      Created: August    20th, 2014
 *      Revised: August    23rd, 2014
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include    "mbed.h"
#include    "stepper.h"

const uint8_t pase_cw[4][4]  = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}};
const uint8_t pase_ccw[4][4] = {{1, 1, 0, 0}, {1, 0, 0, 1}, {0, 0, 1, 1}, {0, 1, 1, 0}};
extern uint8_t pls_width[];

STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn):
                   _xp(xp), _xn(xn), _yp(yp), _yn(yn) {
    init();
}

void STEPPER::move (int32_t steps){
    if (steps < 0){
        inf.direction = D_CCW;
        steps = -steps;
    } else {
        inf.direction = D_CW;
    }   
    inf.total_step = steps;
    setup_mtr_drv_dt(&inf, &cntl);
}

void STEPPER::set_max_speed (uint32_t time_base_us){
    if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)){
        time_base_us = (MT_PLS_WIDTH_MIN * 1000);
    }
    _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us);
}

uint8_t STEPPER::status (void){
    return cntl.state;
}

void STEPPER::init(void){
    busy_sm_drv = 0;
    _xp = 0;
    _xn = 0;
    _yp = 0;
    _yn = 0;
}

void STEPPER::set4ports (uint32_t step, int8_t dir){
uint8_t i;

    i = (uint8_t)(step % 4);
    if (dir == D_CW) {
        _xp = pase_cw[i][0];
        _xn = pase_cw[i][1];
        _yp = pase_cw[i][2];
        _yn = pase_cw[i][3];
    } else {
        _xp = pase_ccw[i][0];
        _xn = pase_ccw[i][1];
        _yp = pase_ccw[i][2];
        _yn = pase_ccw[i][3];
    }
}

void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt){
    busy_sm_drv = 1;
    if (mi->total_step == 0){
        if (mt->state != M_STOP){
            mt->state = M_CHANGE;
            mt->change_cnt = 5;
            mt->pls_width = 0;
            mt->ongoing = 0;
            mt->up_cnt = 0;
            mt->up_cnt_keep = 0;
            mt->down_cnt = 0;
            mt->continue_cnt = 0;
        }
        busy_sm_drv = 0;
        return;
    }
    if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)){
        if (mi->total_step < MT_SLOP_STEP){
            mt->up_cnt = 0;
            mt->up_cnt_keep = 0;
            mt->down_cnt = mi->total_step;
            mt->continue_cnt = 0;
            mt->state = M_DOWN;
            mt->ongoing = 0;
        } else {
            mt->up_cnt = 0;
            mt->up_cnt_keep = 0;
            mt->down_cnt = MT_SLOP_STEP -1;
            mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1);
        }
    } else {
        if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)){
            mt->state = M_CHANGE;
            mt->change_cnt = 5;
        } else {
            mt->motor_step = 0;
            mt->state = M_UP;        
        }
        mt->pls_width = 0;
        mt->ongoing = 0; 
        mt->direction = mi->direction;
        if (mi->total_step < MT_MIN_STEP){
            if (mi->total_step == MT_MIN_STEP - 1){
                mt->up_cnt = MT_SLOP_STEP;
            } else {
                mt->up_cnt = mi->total_step / 2;
            }
            mt->up_cnt_keep = mt->up_cnt;
            mt->down_cnt = mi->total_step - mt->up_cnt;
            mt->continue_cnt = 0;
        } else {
            mt->up_cnt = MT_SLOP_STEP;
            mt->up_cnt_keep = mt->up_cnt;
            mt->down_cnt = MT_SLOP_STEP -1 ;
            mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1);
        }
    }
    busy_sm_drv = 0;
}

void STEPPER::millisec_inteval() {
    if (busy_sm_drv == 1){ return;}
    switch (cntl.state){
    case M_STOP:
        _xp = 0;
        _xn = 0;
        _yp = 0;
        _yn = 0;
        break;
    case M_UP:
        if (cntl.ongoing){
            if (--cntl.pls_width == 0){
                if (--cntl.up_cnt == 0){
                    cntl.ongoing = 0;
                    if (cntl.continue_cnt == 0){
                        cntl.state = M_DOWN;
                    } else {
                        cntl.state = M_CONTINUE;
                    }
                } else {
                    set4ports(cntl.motor_step, cntl.direction);
                    cntl.motor_step++;
                    cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
                }
            } else {
                break;
            }
        } else {    // 1st entry from M_STOP
            set4ports(cntl.motor_step, cntl.direction);
            cntl.motor_step++;
            cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
            cntl.ongoing = 1;
        }
        break;
    case M_CONTINUE:
        set4ports(cntl.motor_step, cntl.direction);
        cntl.motor_step++;
        if (--cntl.continue_cnt == 0){
            cntl.ongoing = 0;
            cntl.state = M_DOWN;
        }
        break;
    case M_DOWN:
        if (cntl.ongoing){
            if (--cntl.pls_width == 0){
                if (--cntl.down_cnt == 0){
                    cntl.ongoing = 0;
                    cntl.state = M_STOP;
                }else {
                    set4ports(cntl.motor_step, cntl.direction);
                    cntl.motor_step++;
                    cntl.pls_width = pls_width[cntl.down_cnt];
                }
            } else {
                break;
            }
        } else {    // 1st entry from M_UP or M_CONTINUE
            set4ports(cntl.motor_step, cntl.direction);
            cntl.motor_step++;
            cntl.pls_width = pls_width[cntl.down_cnt];
            cntl.ongoing = 1;
        }
        break;
    case M_CHANGE:
        if (cntl.ongoing){
            if (--cntl.pls_width == 0){
                if (--cntl.change_cnt == 0){
                    cntl.ongoing = 0;
                    if (cntl.up_cnt == 0){
                        cntl.state = M_STOP;
                    } else {
                        cntl.state = M_UP;
                    }
                }else {
                    if (cntl.direction == D_CW){
                        set4ports(cntl.motor_step, D_CCW);
                    } else {
                        set4ports(cntl.motor_step, D_CW);
                    }
                    cntl.motor_step++;
                    cntl.pls_width = pls_width[cntl.change_cnt];
                }
            } else {
                break;
            }
        } else {    // 1st entry
            if (cntl.direction == D_CW){
                set4ports(cntl.motor_step, D_CCW);
            } else {
                set4ports(cntl.motor_step, D_CW);
            }
            cntl.motor_step++;
            cntl.pls_width = pls_width[cntl.change_cnt];
            cntl.ongoing = 1;
        }
        break;
    default :
        cntl.state = M_STOP;
    }
}