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 //uncomment one option
00022 //in truth, both uni- & bi- are equivalent, and just represent a different wiring of mcu GPIOs to H-Bridge
00023 //TODO: adopt one most-prevalent wiring scheme only, run-time selectable wiring & step pattern?
00024 
00025 //#define BIPOLAR_STEPPER_1PH
00026 //#define BIPOLAR_STEPPER_2PH
00027 #define BIPOLAR_STEPPER_12S
00028 //#define UNIPOLAR_STEPPER_1PH
00029 //#define UNIPOLAR_STEPPER_2PH
00030 //#define UNIPOLAR_STEPPER_12S
00031 /*
00032  * Firing sequence for bi-polar (4 wires) stepper (2x H-Bridge driver required eg L298)
00033  */
00034 #ifdef BIPOLAR_STEPPER_1PH
00035 //single phase A+, B+, A-, B-
00036 //mcu wiring to H-bridge:        A A'  B B'
00037 const uint8_t pase_cw[4][4]  = {{1, 0, 0, 0}, {0, 0, 1, 0}, {0, 1, 0, 0}, {0, 0, 0, 1}};
00038 #endif
00039 
00040 #ifdef BIPOLAR_STEPPER_2PH
00041 //double phase A+B+, A-B+, A-B-, A+B-
00042 const uint8_t pase_cw[4][4]  = {{1, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 1}, {1, 0, 0, 1}};
00043 #endif
00044 
00045 #ifdef BIPOLAR_STEPPER_12S
00046 //half step A+B+, A+, A+B-, B-, A-B-, A-, A-B+, B+
00047 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}};
00048 #endif
00049 
00050 /*
00051  * Firing sequence for uni-polar (5+ wires) stepper (4x open-collector drivers required).
00052  */
00053 #ifdef UNIPOLAR_STEPPER_1PH
00054 //mcu wiring to Driver/FETs      A+ B+ A- B-
00055 const uint8_t pase_cw[4][4]  = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
00056 #endif
00057 
00058 #ifdef UNIPOLAR_STEPPER_2PH
00059 //mcu wiring to Driver/FETs      A+ B+ A- B-
00060 const uint8_t pase_cw[4][4]  = {{1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, {1, 0, 0, 1}};
00061 #endif
00062 
00063 #ifdef UNIPOLAR_STEPPER_12S
00064 //mcu wiring to Driver/FETs      A+ B+ A- B-
00065 const uint8_t pase_cw[8][4]  = {{1, 1, 0, 0}, {1, 0, 0, 0}, {1, 0, 0, 1}, {0, 0, 0, 1}, {0, 0, 1, 1}, {0, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 0}};
00066 #endif
00067 
00068 //declare as pointer, not array, so that we can swap to different acc/deceleration arrays on the fly.
00069 extern uint8_t *pls_width;
00070 
00071 STEPPER::STEPPER (PinName xp, PinName xn, PinName yp, PinName yn):
00072     _xp(xp), _xn(xn), _yp(yp), _yn(yn)
00073 {
00074     stop();
00075 }
00076 
00077 void STEPPER::move (int32_t steps)
00078 {
00079     if (steps < 0) {
00080         inf.direction = D_CCW;
00081         steps = -steps;
00082     }
00083     // test for +ve, don't flip direction for stop command steps==0
00084     else if (steps > 0) {
00085         inf.direction = D_CW;
00086     }
00087     inf.total_step = steps;
00088     setup_mtr_drv_dt(&inf, &cntl);
00089 }
00090 
00091 void STEPPER::set_max_speed (uint32_t time_base_us)
00092 {
00093     if (time_base_us < (MT_PLS_WIDTH_MIN * 1000)) {
00094         time_base_us = (MT_PLS_WIDTH_MIN * 1000);
00095     }
00096     _smdrv.detach();
00097     _smdrv.attach_us(this, &STEPPER::millisec_inteval, time_base_us);
00098 }
00099 
00100 uint8_t STEPPER::status (void)
00101 {
00102     return cntl.state;
00103 }
00104 
00105 void STEPPER::stop(void)
00106 {
00107     busy_sm_drv = 0;
00108     _xp = 0;
00109     _xn = 0;
00110     _yp = 0;
00111     _yn = 0;
00112 }
00113 
00114 void STEPPER::set4ports (void)
00115 {
00116     uint8_t i;
00117     cntl.motor_step+=inf.direction;
00118     i = (uint8_t)(cntl.motor_step % (sizeof(pase_cw)/4) );
00119     _xp = pase_cw[i][0];
00120     _xn = pase_cw[i][1];
00121     _yp = pase_cw[i][2];
00122     _yn = pase_cw[i][3];
00123 }
00124 
00125 void STEPPER::setup_mtr_drv_dt(Motor_Inf *mi, Motor_Control *mt)
00126 {
00127     busy_sm_drv = 1;
00128 
00129     //0-steps command: if moving, bring to controlled stop & hold position
00130     if (mi->total_step == 0) {
00131         if (mt->state != M_STOP) {
00132             mt->state = M_CHANGE;
00133             mt->change_cnt = 5;
00134             mt->pls_width = 0;
00135             mt->ongoing = 0;
00136             mt->up_cnt = 0;
00137             mt->up_cnt_keep = 0;
00138             mt->down_cnt = 0;
00139             mt->continue_cnt = 0;
00140         } //else init(); // already stopped, interpret as: release HOLD currents, power down coils, motor freewheeling. init() now public, renamed stop()
00141         busy_sm_drv = 0;
00142         return;
00143     }
00144 
00145     //already travelling full speed, new step in same direction
00146     if ((mt->state == M_CONTINUE) && ( mt->direction == mi->direction)) {
00147         if (mi->total_step < MT_SLOP_STEP) { //step len shorter than ramp down ledn
00148             mt->up_cnt = 0;
00149             mt->up_cnt_keep = 0;
00150             mt->down_cnt = mi->total_step;
00151             mt->continue_cnt = 0;
00152             mt->state = M_DOWN;
00153             mt->ongoing = 0;
00154         } else {
00155             mt->up_cnt = 0;
00156             mt->up_cnt_keep = 0;
00157             mt->down_cnt = MT_SLOP_STEP -1;
00158             mt->continue_cnt = mi->total_step - (MT_SLOP_STEP - 1);
00159         }
00160     } else {
00161         //already moving, reverse direction required
00162         if ((mt->state == M_CONTINUE) && ( mt->direction != mi->direction)) {
00163             mt->state = M_CHANGE;
00164             mt->change_cnt = 5;
00165         } else {//motor was at rest?
00166             //mt->motor_step = 0; // don't destroy knowledge of current phase
00167             mt->state = M_UP;
00168         }
00169         mt->pls_width = 0;
00170         mt->ongoing = 0;
00171         mt->direction = mi->direction;
00172         if (mi->total_step < MT_MIN_STEP) {
00173             if (mi->total_step == MT_MIN_STEP - 1) {
00174                 mt->up_cnt = MT_SLOP_STEP;
00175             } else {
00176                 mt->up_cnt = mi->total_step / 2;
00177                 if (mt->up_cnt==0) mt->up_cnt=1;
00178             }
00179             mt->up_cnt_keep = mt->up_cnt;
00180             mt->down_cnt = mi->total_step - mt->up_cnt;
00181             mt->continue_cnt = 0;
00182         } else {
00183             mt->up_cnt = MT_SLOP_STEP;
00184             mt->up_cnt_keep = mt->up_cnt;
00185             mt->down_cnt = MT_SLOP_STEP -1 ;
00186             mt->continue_cnt = mi->total_step - MT_SLOP_STEP - (MT_SLOP_STEP - 1);
00187         }
00188     }
00189     busy_sm_drv = 0;
00190 }
00191 
00192 void STEPPER::millisec_inteval()
00193 {
00194     if (busy_sm_drv == 1) {
00195         return;
00196     }
00197     switch (cntl.state) {
00198         case M_STOP:
00199             /*
00200              * no move, but continue to energize coils: hold current position
00201             */
00202             break;
00203         case M_UP:
00204             if (cntl.ongoing) {
00205                 if (--cntl.pls_width == 0) {
00206                     if (--cntl.up_cnt == 0) {
00207                         cntl.ongoing = 0;
00208                         if (cntl.continue_cnt == 0) {
00209                             cntl.state = M_DOWN;
00210                             if (cntl.down_cnt==0) cntl.state=M_STOP;
00211                         } else {
00212                             cntl.state = M_CONTINUE;
00213                         }
00214                     } else {
00215                         set4ports();
00216                         cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
00217                     }
00218                 } else {
00219                     break;
00220                 }
00221             } else {    // 1st entry from M_STOP
00222                 set4ports();
00223                 cntl.pls_width = pls_width[cntl.up_cnt_keep - cntl.up_cnt];
00224                 cntl.ongoing = 1;
00225             }
00226             break;
00227         case M_CONTINUE:
00228             set4ports();
00229             if (--cntl.continue_cnt == 0) {
00230                 cntl.ongoing = 0;
00231                 cntl.state = M_DOWN;
00232             }
00233             break;
00234         case M_DOWN:
00235             if (cntl.ongoing) {
00236                 if (--cntl.pls_width == 0) {
00237                     if (--cntl.down_cnt == 0) {
00238                         cntl.ongoing = 0;
00239                         cntl.state = M_STOP;
00240                     } else {
00241                         set4ports();
00242                         cntl.pls_width = pls_width[cntl.down_cnt];
00243                     }
00244                 } else {
00245                     break;
00246                 }
00247             } else {    // 1st entry from M_UP or M_CONTINUE
00248                 set4ports();
00249                 cntl.pls_width = pls_width[cntl.down_cnt];
00250                 cntl.ongoing = 1;
00251             }
00252             break;
00253         case M_CHANGE:
00254             if (cntl.ongoing) {
00255                 if (--cntl.pls_width == 0) {
00256                     if (--cntl.change_cnt == 0) {
00257                         cntl.ongoing = 0;
00258                         if (cntl.up_cnt == 0) {
00259                             cntl.state = M_STOP;
00260                         } else {
00261                             cntl.state = M_UP;
00262                         }
00263                     } else {
00264                         set4ports();
00265                         cntl.pls_width = pls_width[cntl.change_cnt];
00266                     }
00267                 } else {
00268                     break;
00269                 }
00270             } else {    // 1st entry
00271                 set4ports();
00272                 cntl.pls_width = pls_width[cntl.change_cnt];
00273                 cntl.ongoing = 1;
00274             }
00275             break;
00276         default :
00277             cntl.state = M_STOP;
00278     }
00279 }