StepperMotorUni.cpp

00001 /** Stepper Motor (Unipolar) control library
00002  *
00003  *  @class   StepperMotorUni
00004  *  @author  Tedd OKANO
00005  *  @version 1.1.3
00006  *  @date    13-Sep-2017
00007  *
00008  *  Copyright: 2010, 2014, 2015, 2017 Tedd OKANO
00009  *  Released under the Apache 2 license License
00010  *
00011  *  The library that controls stepper motor via motor driver chip
00012  *  This is a driver for a unipolar stepper motor.
00013  *
00014  *  Example:
00015  *  @code
00016  *  #include "mbed.h"
00017  *  #include "StepperMotorUni.h"
00018  *
00019  *  StepperMotorUni motor( p26, p25, p24, p23 );
00020  *
00021  *  int main()
00022  *  {
00023  *      motor.set_pps( 50 );
00024  *
00025  *      while ( 1 ) {
00026  *          motor.move_steps( 24 );
00027  *          wait( 1 );
00028  *
00029  *          motor.move_steps( -24 );
00030  *          wait( 1 );
00031  *      }
00032  *  }
00033  *  @endcode
00034  *
00035  *  version 0.51  (27-Nov-2010) //  initial version (un-published)
00036  *  version 0.6   (15-Jan-2014) //  compatible to LPC1768, LPC11U24 and LPC1114 targets
00037  *  version 1.0   (19-Jun-2014) //  version 1.0 release
00038  *  version 1.0.1 (14-Apr-2015) //  API document correction
00039  *  version 1.1   (21-Apr-2015) //  ramp control function enabled
00040  *  version 1.1.1 (22-Apr-2015) //  fixed: find_home_position compatibility with ramp control feature
00041  *  version 1.1.2 (27-Apr-2015) //  fixed: init_done behavior 
00042  *  version 1.1.3 (13-Sep-2017) //  fixed: keeping stopped position while power_ctrl is set false
00043  */
00044 
00045 #include "mbed.h"
00046 #include "StepperMotorUni.h"
00047 
00048 StepperMotorUni::StepperMotorUni(
00049     PinName out_A,
00050     PinName out_B,
00051     PinName out_C,
00052     PinName out_D,
00053     PinName position_detect
00054 ) :
00055     init_done( false ),
00056     motor_out( out_A, out_B, out_C, out_D ),
00057     position_detect_pin( position_detect ),
00058     rot_mode( SHORTEST ),
00059     sync_mode( ASYNCHRONOUS ),
00060     max_pos( 480 ),
00061     current_pos( 0 ),
00062     pos_offset( 0 ),
00063     target_pos( 0 ),
00064     max_pps( MAX_PPS ),
00065     pause( false ),
00066     power_ctrl( false ),
00067     ramp_init_speed_rate( 1.0 ),
00068     ramp_control_steps( 0 ),
00069     ramp_rate( 1.0 )
00070 {
00071 
00072     pps     = max_pps;
00073     pattern = (unsigned char *)pattern_one_phase;
00074     pat_index_mask  = 0x3;
00075     t.attach( this, &StepperMotorUni::motor_maintain, 1.0 / (float)pps );
00076 }
00077 
00078 float StepperMotorUni::set_pps( float v )
00079 {
00080     float   p;
00081 
00082     p   = pps;
00083     pps = v;
00084     t.detach();
00085     t.attach( this, &StepperMotorUni::motor_maintain, 1.0 / (float)pps );
00086     return ( p );
00087 }
00088 
00089 void StepperMotorUni::set_max_pps( float v )
00090 {
00091     max_pps     = v;
00092 }
00093 
00094 int StepperMotorUni::find_home_position( PositionDetectPorarity edge )
00095 {
00096     RotMode     prev_rot;
00097     SyncMode    prev_sync;
00098     float       prev_pps;
00099     float       prev_i_rate;
00100     int         prev_rcs;
00101     int         prev_det_pin;
00102     int         direction;
00103 
00104     if ( position_detect_pin == NC )
00105         return 0;
00106 
00107     prev_pps    = set_pps( max_pps );
00108     prev_i_rate = ramp_init_speed_rate;
00109     prev_rcs    = ramp_control_steps;
00110     prev_rot    = rot_mode;
00111     prev_sync   = sync_mode;
00112     set_sync_mode( SYNCHRONOUS );
00113     set_rot_mode( SHORTEST );
00114 
00115     prev_det_pin        = position_detect_pin;
00116     set_ramp_control( 1.0, 0 );
00117 
00118     if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
00119         direction   = -1;
00120     else
00121         direction   = 1;
00122 
00123     init_done   = false;
00124 
00125     if ( prev_rot == NO_WRAPAROUND ) {
00126 
00127         for ( int i = 0; i < (max_pos >> 1); i++ ) {
00128             move_steps( -1 );
00129             if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
00130                     || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
00131                 set_home_position();
00132                 init_done   = true;
00133                 break;
00134             }
00135             prev_det_pin    = position_detect_pin;
00136         }
00137     }
00138 
00139     for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
00140         move_steps( direction );
00141         if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
00142                 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
00143             set_home_position();
00144             init_done   = true;
00145             break;
00146         }
00147         prev_det_pin    = position_detect_pin;
00148     }
00149 
00150     go_position( 0 );
00151     set_pps( prev_pps );
00152     set_rot_mode( prev_rot );
00153     set_sync_mode( prev_sync );
00154     set_ramp_control( prev_i_rate, prev_rcs );
00155 
00156     return ( init_done );
00157 }
00158 
00159 void StepperMotorUni::set_home_position( void )
00160 {
00161     set_pause( true );
00162     pos_offset  = current_pos & 0x3;
00163     current_pos = 0;
00164     set_target_pos( 0 );
00165     set_pause( false );
00166 }
00167 
00168 
00169 void StepperMotorUni::go_angle( float angle )
00170 {
00171     go_position( (int)((angle / 360.0) * (float)max_pos) );
00172 }
00173 
00174 int StepperMotorUni::move_steps( int s )
00175 {
00176     set_target_pos( current_pos + s );
00177     return ( current_pos );
00178 }
00179 
00180 int StepperMotorUni::move_rotates( float r )
00181 {
00182     set_target_pos( current_pos + (int)(r * (float)max_pos) );
00183     return ( current_pos );
00184 }
00185 
00186 void StepperMotorUni::set_operation_phase_mode( OperationPhaseMode v )
00187 {
00188 
00189     switch ( v ) {
00190         case StepperMotorUni::ONE_PHASE :
00191             pattern = pattern_one_phase;
00192             pat_index_mask  = 0x3;
00193             break;
00194         case StepperMotorUni::TWO_PHASE :
00195             pattern = pattern_two_phase;
00196             pat_index_mask  = 0x3;
00197             break;
00198         case StepperMotorUni::HALFSTEP :
00199             pattern = pattern_halfstep;
00200             pat_index_mask  = 0x7;
00201             break;
00202     }
00203 
00204     phase_mode  = v;
00205 }
00206 
00207 void StepperMotorUni::set_rot_mode( RotMode m )
00208 {
00209     rot_mode    = m;
00210 }
00211 
00212 void StepperMotorUni::set_sync_mode( SyncMode m )
00213 {
00214     sync_mode    = m;
00215 }
00216 
00217 int StepperMotorUni::distance( void )
00218 {
00219     return( target_pos - current_pos );
00220 }
00221 
00222 void StepperMotorUni::set_pause( int sw )
00223 {
00224     pause   = sw;
00225 }
00226 
00227 void StepperMotorUni::brake( void )
00228 {
00229     brake( SOFT_BRAKE );
00230 }
00231 
00232 void StepperMotorUni::brake( BrakeMode mode )
00233 {
00234     int extra_steps;
00235 
00236     if ( mode == SOFT_BRAKE ) {
00237         extra_steps = abs( target_pos - current_pos );
00238         extra_steps = extra_steps < ramp_control_steps ? extra_steps : ramp_control_steps;
00239         extra_steps = target_pos < current_pos ? -extra_steps : extra_steps;
00240     } else {
00241         extra_steps = 0;
00242     }
00243 
00244     target_pos  = current_pos + extra_steps;
00245 }
00246 
00247 void StepperMotorUni::set_target_pos( int p )
00248 {
00249     target_pos  = p;
00250 
00251     if (sync_mode == SYNCHRONOUS)
00252         while ( distance() )
00253             wait( 0 );
00254 }
00255 
00256 void StepperMotorUni::set_power_ctrl( int sw )
00257 {
00258     power_ctrl  = sw;
00259 }
00260 
00261 void StepperMotorUni::set_steps_per_rotate( int steps )
00262 {
00263     max_pos = steps;
00264 }
00265 
00266 void StepperMotorUni::set_ramp_control( float initial_speed_rate, int ramp_steps )
00267 {
00268     ramp_init_speed_rate    = initial_speed_rate;
00269     ramp_control_steps      = ramp_steps;
00270 
00271     if ( ramp_steps == 0 )
00272         ramp_rate   = 1.0;
00273     else {
00274         ramp_rate   = powf( ramp_init_speed_rate, 1.0 / (float)ramp_control_steps );
00275     }
00276 }
00277 
00278 void StepperMotorUni::motor_maintain( void )
00279 {
00280     int             distance;
00281     static int      moved_steps     = 0;
00282     static int      ramp_steps      = 0;
00283     static float    speed_rate      = 1.0;
00284 
00285     if ( pause )
00286         return;
00287 
00288     distance    = target_pos - current_pos;
00289     distance    = (distance < 0)? -distance : distance;
00290 
00291     if ( !distance ) {
00292         if ( power_ctrl )
00293             motor_out   = 0;
00294             
00295         moved_steps = 0;
00296         return;
00297     }
00298 
00299     if ( !moved_steps ) { //  ramp control
00300         speed_rate  = ramp_init_speed_rate;
00301         ramp_steps  = ((ramp_control_steps * 2) - distance) / 2;
00302         ramp_steps  = (0 < ramp_steps) ? ramp_control_steps - ramp_steps : ramp_control_steps;
00303 //        printf( "ramp_steps=%d, speed_rate=%f\r\n", ramp_steps, speed_rate );
00304 //        printf( "diff=%d, speed_rate=%f\r\n", diff, speed_rate );
00305     } else if ( moved_steps < ramp_steps ) {
00306         speed_rate  /= ramp_rate;
00307     } else if ( distance <= ramp_steps ) {
00308         speed_rate  *= ramp_rate;
00309     } else {
00310         speed_rate  = 1.0;
00311     }
00312 
00313     moved_steps++;
00314     current_pos = current_pos + ( (target_pos < current_pos) ? -1 : 1 );
00315 
00316     motor_out   = pattern[ (current_pos + pos_offset) & pat_index_mask ];
00317 
00318 //printf( "%d>>>%d\r\n", current_pos, target_pos );
00319 
00320     if ( speed_rate != 1.0 ) {
00321         t.detach();
00322         t.attach( this, &StepperMotorUni::motor_maintain, ((1.0 / (float)pps)) / speed_rate );
00323     }
00324 
00325     if ( target_pos == current_pos ) {
00326         current_pos = current_pos % max_pos;
00327         target_pos  = target_pos  % max_pos;
00328         moved_steps = 0;
00329     }
00330 };
00331 
00332 int StepperMotorUni::go_position( int target_pos )
00333 {
00334 
00335     current_pos = current_pos % max_pos;
00336     current_pos = (current_pos < 0) ? current_pos + max_pos : current_pos;
00337 
00338     target_pos  = target_pos  % max_pos;
00339     target_pos  = (target_pos  < 0) ? target_pos  + max_pos : target_pos;
00340 
00341 #define CW      0
00342 #define CCW     1
00343 
00344     int     direction;
00345 
00346     switch ( rot_mode ) {
00347         case NO_WRAPAROUND :
00348             direction   = (current_pos < target_pos) ? CW : CCW;
00349             break;
00350         case CLOCKWISE_ONLY :
00351             direction   = CW;
00352             break;
00353         case COUNTER_CLOCKWISE_ONLY :
00354             direction   = CCW;
00355             break;
00356         default : // SHORTEST :
00357             direction   = ( (max_pos >> 1) < ((target_pos + max_pos - current_pos) % max_pos) ) ? CCW : CW;
00358             break;
00359     }
00360 
00361     switch ( direction ) {
00362         case CW :
00363             current_pos   = (target_pos < current_pos) ? current_pos - max_pos : current_pos;
00364             break;
00365         case CCW :
00366             target_pos   = (current_pos < target_pos) ? target_pos - max_pos : target_pos;
00367             break;
00368     }
00369 
00370     set_target_pos( target_pos );
00371     return ( current_pos );
00372 }
00373 
00374 #if 0
00375 unsigned char StepperMotorUni::pattern_one_phase[ 4 ]  = { 0x8, 0x4, 0x2, 0x1 };
00376 unsigned char StepperMotorUni::pattern_two_phase[ 4 ]  = { 0x9, 0xC, 0x6, 0x3 };
00377 unsigned char StepperMotorUni::pattern_halfstep[ 8 ]  = { 0x9, 0x8, 0xC, 0x4, 0x6, 0x2, 0x3, 0x1 };
00378 #else
00379 unsigned char StepperMotorUni::pattern_one_phase[ 4 ]  = { 0x1, 0x2, 0x4, 0x8 };
00380 unsigned char StepperMotorUni::pattern_two_phase[ 4 ]  = { 0x3, 0x6, 0xC, 0x9 };
00381 unsigned char StepperMotorUni::pattern_halfstep[ 8 ]  = { 0x1, 0x3, 0x2, 0x6, 0x4, 0xC, 0x8, 0x9 };
00382 #endif