Stepper motor control class library
Dependents: StepperMotor_HelloWorld
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 };
Generated on Wed Jul 13 2022 21:47:06 by
