stepper.cpp

00001 /*
00002  * mbed library program 
00003  *      Stepping Motor
00004  *
00005  * Copyright (c) 2014 Kenji Arai / JH1PJL
00006  *  http://www.page.sannet.ne.jp/kenjia/index.html
00007  *  http://mbed.org/users/kenjiArai/
00008  *      Created: August    20th, 2014
00009  *      Revised: August    23rd, 2014
00010  *
00011  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
00012  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
00013  * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00014  * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00015  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00016  */
00017 
00018 #include    "mbed.h"
00019 #include    "stepper.h"
00020 
00021 const uint8_t pase_cw[4][4]  = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}};
00022 const uint8_t pase_ccw[4][4] = {{1, 1, 0, 0}, {1, 0, 0, 1}, {0, 0, 1, 1}, {0, 1, 1, 0}};
00023 extern uint8_t pls_width[];
00024 
00025 STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn):
00026                    _xp(xp), _xn(xn), _yp(yp), _yn(yn) {
00027     init();
00028 }
00029 
00030 void STEPPER::move (int32_t steps){
00031     if (steps < 0){
00032         inf.direction = D_CCW;
00033         steps = -steps;
00034     } else {
00035         inf.direction = D_CW;
00036     }   
00037     inf.total_step = steps;
00038     setup_mtr_drv_dt(&inf, &cntl);
00039 }
00040 
00041 void STEPPER::set_max_speed (uint32_t time_base_us){
00042     if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)){
00043         time_base_us = (MT_PLS_WIDTH_MIN * 1000);
00044     }
00045     _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us);
00046 }
00047 
00048 uint8_t STEPPER::status (void){
00049     return cntl.state;
00050 }
00051 
00052 void STEPPER::init(void){
00053     busy_sm_drv = 0;
00054     _xp = 0;
00055     _xn = 0;
00056     _yp = 0;
00057     _yn = 0;
00058 }
00059 
00060 void STEPPER::set4ports (uint32_t step, int8_t dir){
00061 uint8_t i;
00062 
00063     i = (uint8_t)(step % 4);
00064     if (dir == D_CW) {
00065         _xp = pase_cw[i][0];
00066         _xn = pase_cw[i][1];
00067         _yp = pase_cw[i][2];
00068         _yn = pase_cw[i][3];
00069     } else {
00070         _xp = pase_ccw[i][0];
00071         _xn = pase_ccw[i][1];
00072         _yp = pase_ccw[i][2];
00073         _yn = pase_ccw[i][3];
00074     }
00075 }
00076 
00077 void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt){
00078     busy_sm_drv = 1;
00079     if (mi->total_step == 0){
00080         if (mt->state != M_STOP){
00081             mt->state = M_CHANGE;
00082             mt->change_cnt = 5;
00083             mt->pls_width = 0;
00084             mt->ongoing = 0;
00085             mt->up_cnt = 0;
00086             mt->up_cnt_keep = 0;
00087             mt->down_cnt = 0;
00088             mt->continue_cnt = 0;
00089         }
00090         busy_sm_drv = 0;
00091         return;
00092     }
00093     if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)){
00094         if (mi->total_step < MT_SLOP_STEP){
00095             mt->up_cnt = 0;
00096             mt->up_cnt_keep = 0;
00097             mt->down_cnt = mi->total_step;
00098             mt->continue_cnt = 0;
00099             mt->state = M_DOWN;
00100             mt->ongoing = 0;
00101         } else {
00102             mt->up_cnt = 0;
00103             mt->up_cnt_keep = 0;
00104             mt->down_cnt = MT_SLOP_STEP -1;
00105             mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1);
00106         }
00107     } else {
00108         if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)){
00109             mt->state = M_CHANGE;
00110             mt->change_cnt = 5;
00111         } else {
00112             mt->motor_step = 0;
00113             mt->state = M_UP;        
00114         }
00115         mt->pls_width = 0;
00116         mt->ongoing = 0; 
00117         mt->direction = mi->direction;
00118         if (mi->total_step < MT_MIN_STEP){
00119             if (mi->total_step == MT_MIN_STEP - 1){
00120                 mt->up_cnt = MT_SLOP_STEP;
00121             } else {
00122                 mt->up_cnt = mi->total_step / 2;
00123             }
00124             mt->up_cnt_keep = mt->up_cnt;
00125             mt->down_cnt = mi->total_step - mt->up_cnt;
00126             mt->continue_cnt = 0;
00127         } else {
00128             mt->up_cnt = MT_SLOP_STEP;
00129             mt->up_cnt_keep = mt->up_cnt;
00130             mt->down_cnt = MT_SLOP_STEP -1 ;
00131             mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1);
00132         }
00133     }
00134     busy_sm_drv = 0;
00135 }
00136 
00137 void STEPPER::millisec_inteval() {
00138     if (busy_sm_drv == 1){ return;}
00139     switch (cntl.state){
00140     case M_STOP:
00141         _xp = 0;
00142         _xn = 0;
00143         _yp = 0;
00144         _yn = 0;
00145         break;
00146     case M_UP:
00147         if (cntl.ongoing){
00148             if (--cntl.pls_width == 0){
00149                 if (--cntl.up_cnt == 0){
00150                     cntl.ongoing = 0;
00151                     if (cntl.continue_cnt == 0){
00152                         cntl.state = M_DOWN;
00153                     } else {
00154                         cntl.state = M_CONTINUE;
00155                     }
00156                 } else {
00157                     set4ports(cntl.motor_step, cntl.direction);
00158                     cntl.motor_step++;
00159                     cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
00160                 }
00161             } else {
00162                 break;
00163             }
00164         } else {    // 1st entry from M_STOP
00165             set4ports(cntl.motor_step, cntl.direction);
00166             cntl.motor_step++;
00167             cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
00168             cntl.ongoing = 1;
00169         }
00170         break;
00171     case M_CONTINUE:
00172         set4ports(cntl.motor_step, cntl.direction);
00173         cntl.motor_step++;
00174         if (--cntl.continue_cnt == 0){
00175             cntl.ongoing = 0;
00176             cntl.state = M_DOWN;
00177         }
00178         break;
00179     case M_DOWN:
00180         if (cntl.ongoing){
00181             if (--cntl.pls_width == 0){
00182                 if (--cntl.down_cnt == 0){
00183                     cntl.ongoing = 0;
00184                     cntl.state = M_STOP;
00185                 }else {
00186                     set4ports(cntl.motor_step, cntl.direction);
00187                     cntl.motor_step++;
00188                     cntl.pls_width = pls_width[cntl.down_cnt];
00189                 }
00190             } else {
00191                 break;
00192             }
00193         } else {    // 1st entry from M_UP or M_CONTINUE
00194             set4ports(cntl.motor_step, cntl.direction);
00195             cntl.motor_step++;
00196             cntl.pls_width = pls_width[cntl.down_cnt];
00197             cntl.ongoing = 1;
00198         }
00199         break;
00200     case M_CHANGE:
00201         if (cntl.ongoing){
00202             if (--cntl.pls_width == 0){
00203                 if (--cntl.change_cnt == 0){
00204                     cntl.ongoing = 0;
00205                     if (cntl.up_cnt == 0){
00206                         cntl.state = M_STOP;
00207                     } else {
00208                         cntl.state = M_UP;
00209                     }
00210                 }else {
00211                     if (cntl.direction == D_CW){
00212                         set4ports(cntl.motor_step, D_CCW);
00213                     } else {
00214                         set4ports(cntl.motor_step, D_CW);
00215                     }
00216                     cntl.motor_step++;
00217                     cntl.pls_width = pls_width[cntl.change_cnt];
00218                 }
00219             } else {
00220                 break;
00221             }
00222         } else {    // 1st entry
00223             if (cntl.direction == D_CW){
00224                 set4ports(cntl.motor_step, D_CCW);
00225             } else {
00226                 set4ports(cntl.motor_step, D_CW);
00227             }
00228             cntl.motor_step++;
00229             cntl.pls_width = pls_width[cntl.change_cnt];
00230             cntl.ongoing = 1;
00231         }
00232         break;
00233     default :
00234         cntl.state = M_STOP;
00235     }
00236 }