Diff: stepper.cpp

Revision:

0:7b0c724fa658

Child:

2:fd11d89b8ce0

diff -r 000000000000 -r 7b0c724fa658 stepper.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper.cpp	Sat Aug 23 06:34:07 2014 +0000
@@ -0,0 +1,236 @@
+/*
+ * 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;
+    }
+}