StepperMotor.cpp

00001 /** Stepper Motor control library
00002  *  
00003  *  @class   StepperMotor
00004  *  @author  Tedd OKANO
00005  *  @version 0.6 (13-Sep-2017)
00006  *  
00007  *  Copyright: 2010 Tedd OKANO, Tsukimidai Communications Syndicate - Crawl Design
00008  *  The library that controls stepper motor via motor driver chip: TA7774
00009  *  The TA7774 is a driver for a bipolar stepper motor. 
00010  *  With this library, mbed will generate 2 phase pulses to operate the motor. 
00011  *  
00012  *  version 0.51  (27-Nov-2010)
00013  *  version 0.6   (13-Sep-2017) //  fixed to keep last position while power-control disabled
00014  */
00015  
00016 #include "mbed.h"
00017 #include "StepperMotor.h"
00018 
00019 StepperMotor::StepperMotor(
00020     PinName out_A,
00021     PinName out_B,
00022     PinName out_PWR,
00023     PinName position_detect
00024 ) :
00025         motor_out( out_A, out_B ),
00026         pwr_out( out_PWR ),
00027         position_detect_pin( position_detect ),
00028         rot_mode( SHORTEST ),
00029         sync_mode( ASYNCHRONOUS ),
00030         max_pos( 480 ),
00031         current_pos( 0 ),
00032         pos_offset( 0 ),
00033         target_pos( 0 ),
00034         max_pps( MAX_PPS ),
00035         init_done( false ),
00036         pause( false ),
00037         power_ctrl( false ) {
00038 
00039     pps     = max_pps;
00040     t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
00041 }
00042 
00043 float StepperMotor::set_pps( float v ) {
00044     float   p;
00045 
00046     p   = pps;
00047     pps = v;
00048     t.detach();
00049     t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
00050     return ( p );
00051 }
00052 
00053 void StepperMotor::set_max_pps( float v ) {
00054     max_pps     = v;
00055 }
00056 
00057 int StepperMotor::find_home_position( PositionDetectPorarity edge ) {
00058     RotMode     prev_rot;
00059     SyncMode    prev_sync;
00060     float       prev_pps;
00061     int         prev_det_pin;
00062     int         direction;
00063 
00064     prev_pps    = set_pps( max_pps );
00065     prev_rot    = rot_mode;
00066     prev_sync   = sync_mode;
00067     set_sync_mode( SYNCHRONOUS );
00068     set_rot_mode( SHORTEST );
00069 
00070     prev_det_pin    = position_detect_pin;
00071 
00072     if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
00073         direction   = -1;
00074     else
00075         direction   = 1;
00076 
00077     if ( prev_rot == NO_WRAPAROUND ) {
00078 
00079         for ( int i = 0; i < (max_pos >> 1); i++ ) {
00080             move_steps( -1 );
00081             if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
00082                     || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
00083                 set_home_position();
00084                 init_done   = true;
00085                 break;
00086             }
00087             prev_det_pin    = position_detect_pin;
00088         }
00089     }
00090 
00091     for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
00092         move_steps( direction );
00093         if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
00094                 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
00095             set_home_position();
00096             init_done   = true;
00097             break;
00098         }
00099         prev_det_pin    = position_detect_pin;
00100     }
00101 
00102     go_position( 0 );
00103     set_pps( prev_pps );
00104     set_rot_mode( prev_rot );
00105     set_sync_mode( prev_sync );
00106 
00107     return ( init_done );
00108 }
00109 
00110 void StepperMotor::set_home_position( void ) {
00111     set_pause( true );
00112     pos_offset  = current_pos & 0x3;
00113     current_pos = 0;
00114     set_target_pos( 0 );
00115     set_pause( false );
00116 }
00117 
00118 int StepperMotor::go_position( int v ) {
00119     set_target_pos( v );
00120     return ( current_pos );
00121 }
00122 
00123 void StepperMotor::go_angle( float angle ) {
00124     go_position( (int)((angle / 360.0) * (float)max_pos) );
00125 }
00126 
00127 int StepperMotor::move_steps( int s ) {
00128     set_target_pos( current_pos + s );
00129     return ( current_pos );
00130 }
00131 
00132 void StepperMotor::set_rot_mode( RotMode m ) {
00133     rot_mode    = m;
00134 }
00135 
00136 void StepperMotor::set_sync_mode( SyncMode m ) {
00137     sync_mode    = m;
00138 }
00139 
00140 int StepperMotor::distance( void ) {
00141     return( target_pos - current_pos );
00142 }
00143 
00144 void StepperMotor::set_pause( int sw ) {
00145     pause   = sw;
00146 }
00147 
00148 void StepperMotor::set_target_pos( int p ) {
00149     p           = p % max_pos;
00150     target_pos  = (p < 0) ? (p + max_pos) : p;
00151      
00152     if (sync_mode == SYNCHRONOUS)
00153         while ( distance() )
00154             wait( 0 );
00155 }
00156 
00157 void StepperMotor::set_power_ctrl( int sw ) {
00158     power_ctrl  = sw;
00159 }
00160 
00161 void StepperMotor::set_steps_per_rotate( int steps ) {
00162     max_pos = steps;
00163 }
00164 
00165 void StepperMotor::motor_maintain( void ) {
00166 
00167     int     diff;
00168     int     direction;
00169 
00170     if ( pause )
00171         return;
00172 
00173     diff    = target_pos - current_pos;
00174 
00175     if ( !diff ) {
00176         if ( power_ctrl )
00177             pwr_out = 0;
00178             
00179         return;
00180     }
00181 
00182     pwr_out = 1;
00183 
00184     diff = (diff + max_pos) % max_pos;
00185 
00186     if ( diff > (max_pos >> 1) )
00187         diff -= max_pos;
00188 
00189     switch ( rot_mode ) {
00190         case NO_WRAPAROUND :
00191             direction   = ( (target_pos - current_pos) < 0 ) ? -1 : 1;
00192             break;
00193         case CLOCKWISE_ONLY :
00194             direction   = 1;
00195             break;
00196         case COUNTER_CLOCKWISE_ONLY :
00197             direction   = -1;
00198             break;
00199         default : // SHORTEST :
00200             direction   = ( diff < 0 ) ? -1 : 1;
00201             break;
00202     }
00203 
00204 
00205     current_pos = ((current_pos + max_pos) + direction) % max_pos;
00206 
00207 //    printf( "pos=%d  ofst=%d  puls=%d\r\n", current_pos, pos_offset, (current_pos + pos_offset) );
00208     motor_out   = pat[ (current_pos + pos_offset) & 0x3 ];
00209 };
00210 
00211 const unsigned char StepperMotor::pat[ 4 ]  = { 0, 1, 3, 2 };