StepperMotor.cpp

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