Stepper motor control class library
Dependents: StepperMotor_HelloWorld
Components pages
Components pages are available for bipolar
and unipolar
motor libraries
StepperMotor.cpp@0:4beb37ae37ce, 2010-11-25 (annotated)
- Committer:
- okano
- Date:
- Thu Nov 25 11:11:51 2010 +0000
- Revision:
- 0:4beb37ae37ce
- Child:
- 1:dc6cf8f8bcb7
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
okano | 0:4beb37ae37ce | 1 | #include "mbed.h" |
okano | 0:4beb37ae37ce | 2 | #include "StepperMotor.h" |
okano | 0:4beb37ae37ce | 3 | |
okano | 0:4beb37ae37ce | 4 | StepperMotor::StepperMotor( |
okano | 0:4beb37ae37ce | 5 | PinName out_A, |
okano | 0:4beb37ae37ce | 6 | PinName out_B, |
okano | 0:4beb37ae37ce | 7 | PinName out_PWR, |
okano | 0:4beb37ae37ce | 8 | PinName position_detect |
okano | 0:4beb37ae37ce | 9 | ) : |
okano | 0:4beb37ae37ce | 10 | motor_out( out_A, out_B ), |
okano | 0:4beb37ae37ce | 11 | pwr_out( out_PWR ), |
okano | 0:4beb37ae37ce | 12 | position_detect_pin( position_detect ), |
okano | 0:4beb37ae37ce | 13 | rot_mode( SHORTEST ), |
okano | 0:4beb37ae37ce | 14 | sync_mode( ASYNCHRONOUS ), |
okano | 0:4beb37ae37ce | 15 | max_pos( 480 ), |
okano | 0:4beb37ae37ce | 16 | current_pos( 0 ), |
okano | 0:4beb37ae37ce | 17 | pos_offset( 0 ), |
okano | 0:4beb37ae37ce | 18 | target_pos( 0 ), |
okano | 0:4beb37ae37ce | 19 | max_pps( MAX_PPS ), |
okano | 0:4beb37ae37ce | 20 | init_done( false ), |
okano | 0:4beb37ae37ce | 21 | pause( false ), |
okano | 0:4beb37ae37ce | 22 | power_ctrl( false ) { |
okano | 0:4beb37ae37ce | 23 | |
okano | 0:4beb37ae37ce | 24 | pps = max_pps; |
okano | 0:4beb37ae37ce | 25 | t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps ); |
okano | 0:4beb37ae37ce | 26 | } |
okano | 0:4beb37ae37ce | 27 | |
okano | 0:4beb37ae37ce | 28 | int StepperMotor::set_pps( int v ) { |
okano | 0:4beb37ae37ce | 29 | int p; |
okano | 0:4beb37ae37ce | 30 | |
okano | 0:4beb37ae37ce | 31 | p = pps; |
okano | 0:4beb37ae37ce | 32 | pps = v; |
okano | 0:4beb37ae37ce | 33 | t.detach(); |
okano | 0:4beb37ae37ce | 34 | t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps ); |
okano | 0:4beb37ae37ce | 35 | return ( p ); |
okano | 0:4beb37ae37ce | 36 | } |
okano | 0:4beb37ae37ce | 37 | |
okano | 0:4beb37ae37ce | 38 | void StepperMotor::set_max_pps( int v ) { |
okano | 0:4beb37ae37ce | 39 | max_pps = v; |
okano | 0:4beb37ae37ce | 40 | } |
okano | 0:4beb37ae37ce | 41 | |
okano | 0:4beb37ae37ce | 42 | int StepperMotor::find_home_position( PositionDetectPorarity edge ) { |
okano | 0:4beb37ae37ce | 43 | RotMode prev_rot; |
okano | 0:4beb37ae37ce | 44 | SyncMode prev_sync; |
okano | 0:4beb37ae37ce | 45 | int prev_pps; |
okano | 0:4beb37ae37ce | 46 | int prev_det_pin; |
okano | 0:4beb37ae37ce | 47 | int direction; |
okano | 0:4beb37ae37ce | 48 | |
okano | 0:4beb37ae37ce | 49 | prev_pps = set_pps( max_pps ); |
okano | 0:4beb37ae37ce | 50 | prev_rot = rot_mode; |
okano | 0:4beb37ae37ce | 51 | prev_sync = sync_mode; |
okano | 0:4beb37ae37ce | 52 | set_sync_mode( SYNCHRONOUS ); |
okano | 0:4beb37ae37ce | 53 | set_rot_mode( SHORTEST ); |
okano | 0:4beb37ae37ce | 54 | |
okano | 0:4beb37ae37ce | 55 | prev_det_pin = position_detect_pin; |
okano | 0:4beb37ae37ce | 56 | |
okano | 0:4beb37ae37ce | 57 | if ( prev_rot == COUNTER_CLOCKWISE_ONLY ) |
okano | 0:4beb37ae37ce | 58 | direction = -1; |
okano | 0:4beb37ae37ce | 59 | else |
okano | 0:4beb37ae37ce | 60 | direction = 1; |
okano | 0:4beb37ae37ce | 61 | |
okano | 0:4beb37ae37ce | 62 | if ( prev_rot == NO_WRAPAROUND ) { |
okano | 0:4beb37ae37ce | 63 | |
okano | 0:4beb37ae37ce | 64 | for ( int i = 0; i < (max_pos >> 1); i++ ) { |
okano | 0:4beb37ae37ce | 65 | move_steps( -1 ); |
okano | 0:4beb37ae37ce | 66 | if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin) |
okano | 0:4beb37ae37ce | 67 | || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) { |
okano | 0:4beb37ae37ce | 68 | set_home_position(); |
okano | 0:4beb37ae37ce | 69 | init_done = true; |
okano | 0:4beb37ae37ce | 70 | break; |
okano | 0:4beb37ae37ce | 71 | } |
okano | 0:4beb37ae37ce | 72 | prev_det_pin = position_detect_pin; |
okano | 0:4beb37ae37ce | 73 | } |
okano | 0:4beb37ae37ce | 74 | } |
okano | 0:4beb37ae37ce | 75 | |
okano | 0:4beb37ae37ce | 76 | for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) { |
okano | 0:4beb37ae37ce | 77 | move_steps( direction ); |
okano | 0:4beb37ae37ce | 78 | if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin) |
okano | 0:4beb37ae37ce | 79 | || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) { |
okano | 0:4beb37ae37ce | 80 | set_home_position(); |
okano | 0:4beb37ae37ce | 81 | init_done = true; |
okano | 0:4beb37ae37ce | 82 | break; |
okano | 0:4beb37ae37ce | 83 | } |
okano | 0:4beb37ae37ce | 84 | prev_det_pin = position_detect_pin; |
okano | 0:4beb37ae37ce | 85 | } |
okano | 0:4beb37ae37ce | 86 | |
okano | 0:4beb37ae37ce | 87 | go_position( 0 ); |
okano | 0:4beb37ae37ce | 88 | set_pps( prev_pps ); |
okano | 0:4beb37ae37ce | 89 | set_rot_mode( prev_rot ); |
okano | 0:4beb37ae37ce | 90 | set_sync_mode( prev_sync ); |
okano | 0:4beb37ae37ce | 91 | |
okano | 0:4beb37ae37ce | 92 | return ( init_done ); |
okano | 0:4beb37ae37ce | 93 | } |
okano | 0:4beb37ae37ce | 94 | |
okano | 0:4beb37ae37ce | 95 | void StepperMotor::set_home_position( void ) { |
okano | 0:4beb37ae37ce | 96 | set_pause( true ); |
okano | 0:4beb37ae37ce | 97 | pos_offset = current_pos & 0x3; |
okano | 0:4beb37ae37ce | 98 | current_pos = 0; |
okano | 0:4beb37ae37ce | 99 | set_target_pos( 0 ); |
okano | 0:4beb37ae37ce | 100 | set_pause( false ); |
okano | 0:4beb37ae37ce | 101 | } |
okano | 0:4beb37ae37ce | 102 | |
okano | 0:4beb37ae37ce | 103 | int StepperMotor::go_position( int v ) { |
okano | 0:4beb37ae37ce | 104 | set_target_pos( v ); |
okano | 0:4beb37ae37ce | 105 | return ( current_pos ); |
okano | 0:4beb37ae37ce | 106 | } |
okano | 0:4beb37ae37ce | 107 | |
okano | 0:4beb37ae37ce | 108 | void StepperMotor::go_angle( float angle ) { |
okano | 0:4beb37ae37ce | 109 | go_position( (int)((angle / 360.0) * (float)max_pos) ); |
okano | 0:4beb37ae37ce | 110 | } |
okano | 0:4beb37ae37ce | 111 | |
okano | 0:4beb37ae37ce | 112 | int StepperMotor::move_steps( int s ) { |
okano | 0:4beb37ae37ce | 113 | set_target_pos( current_pos + s ); |
okano | 0:4beb37ae37ce | 114 | return ( current_pos ); |
okano | 0:4beb37ae37ce | 115 | } |
okano | 0:4beb37ae37ce | 116 | |
okano | 0:4beb37ae37ce | 117 | void StepperMotor::set_rot_mode( RotMode m ) { |
okano | 0:4beb37ae37ce | 118 | rot_mode = m; |
okano | 0:4beb37ae37ce | 119 | } |
okano | 0:4beb37ae37ce | 120 | |
okano | 0:4beb37ae37ce | 121 | void StepperMotor::set_sync_mode( SyncMode m ) { |
okano | 0:4beb37ae37ce | 122 | sync_mode = m; |
okano | 0:4beb37ae37ce | 123 | } |
okano | 0:4beb37ae37ce | 124 | |
okano | 0:4beb37ae37ce | 125 | int StepperMotor::distance( void ) { |
okano | 0:4beb37ae37ce | 126 | return( target_pos - current_pos ); |
okano | 0:4beb37ae37ce | 127 | } |
okano | 0:4beb37ae37ce | 128 | |
okano | 0:4beb37ae37ce | 129 | void StepperMotor::set_pause( int sw ) { |
okano | 0:4beb37ae37ce | 130 | pause = sw; |
okano | 0:4beb37ae37ce | 131 | } |
okano | 0:4beb37ae37ce | 132 | |
okano | 0:4beb37ae37ce | 133 | void StepperMotor::set_target_pos( int p ) { |
okano | 0:4beb37ae37ce | 134 | target_pos = (p + max_pos) % max_pos; |
okano | 0:4beb37ae37ce | 135 | |
okano | 0:4beb37ae37ce | 136 | if (sync_mode == SYNCHRONOUS) |
okano | 0:4beb37ae37ce | 137 | while ( distance() ) |
okano | 0:4beb37ae37ce | 138 | wait( 0 ); |
okano | 0:4beb37ae37ce | 139 | } |
okano | 0:4beb37ae37ce | 140 | |
okano | 0:4beb37ae37ce | 141 | void StepperMotor::set_power_ctrl( int sw ) { |
okano | 0:4beb37ae37ce | 142 | power_ctrl = sw; |
okano | 0:4beb37ae37ce | 143 | } |
okano | 0:4beb37ae37ce | 144 | |
okano | 0:4beb37ae37ce | 145 | void StepperMotor::set_steps_per_rotate( int steps ) { |
okano | 0:4beb37ae37ce | 146 | max_pos = steps; |
okano | 0:4beb37ae37ce | 147 | } |
okano | 0:4beb37ae37ce | 148 | |
okano | 0:4beb37ae37ce | 149 | void StepperMotor::motor_maintain( void ) { |
okano | 0:4beb37ae37ce | 150 | |
okano | 0:4beb37ae37ce | 151 | int diff; |
okano | 0:4beb37ae37ce | 152 | int direction; |
okano | 0:4beb37ae37ce | 153 | |
okano | 0:4beb37ae37ce | 154 | if ( pause ) |
okano | 0:4beb37ae37ce | 155 | return; |
okano | 0:4beb37ae37ce | 156 | |
okano | 0:4beb37ae37ce | 157 | diff = target_pos - current_pos; |
okano | 0:4beb37ae37ce | 158 | |
okano | 0:4beb37ae37ce | 159 | if ( !diff ) { |
okano | 0:4beb37ae37ce | 160 | pwr_out = power_ctrl ? 0 : 1; |
okano | 0:4beb37ae37ce | 161 | return; |
okano | 0:4beb37ae37ce | 162 | } |
okano | 0:4beb37ae37ce | 163 | |
okano | 0:4beb37ae37ce | 164 | pwr_out = 1; |
okano | 0:4beb37ae37ce | 165 | |
okano | 0:4beb37ae37ce | 166 | diff = (diff + max_pos) % max_pos; |
okano | 0:4beb37ae37ce | 167 | |
okano | 0:4beb37ae37ce | 168 | if ( diff > (max_pos >> 1) ) |
okano | 0:4beb37ae37ce | 169 | diff -= max_pos; |
okano | 0:4beb37ae37ce | 170 | |
okano | 0:4beb37ae37ce | 171 | switch ( rot_mode ) { |
okano | 0:4beb37ae37ce | 172 | case NO_WRAPAROUND : |
okano | 0:4beb37ae37ce | 173 | direction = ( (target_pos - current_pos) < 0 ) ? -1 : 1; |
okano | 0:4beb37ae37ce | 174 | break; |
okano | 0:4beb37ae37ce | 175 | case CLOCKWISE_ONLY : |
okano | 0:4beb37ae37ce | 176 | direction = 1; |
okano | 0:4beb37ae37ce | 177 | break; |
okano | 0:4beb37ae37ce | 178 | case COUNTER_CLOCKWISE_ONLY : |
okano | 0:4beb37ae37ce | 179 | direction = -1; |
okano | 0:4beb37ae37ce | 180 | break; |
okano | 0:4beb37ae37ce | 181 | default : // SHORTEST : |
okano | 0:4beb37ae37ce | 182 | direction = ( diff < 0 ) ? -1 : 1; |
okano | 0:4beb37ae37ce | 183 | break; |
okano | 0:4beb37ae37ce | 184 | } |
okano | 0:4beb37ae37ce | 185 | |
okano | 0:4beb37ae37ce | 186 | |
okano | 0:4beb37ae37ce | 187 | current_pos = ((current_pos + max_pos) + direction) % max_pos; |
okano | 0:4beb37ae37ce | 188 | |
okano | 0:4beb37ae37ce | 189 | // printf( "pos=%d ofst=%d puls=%d\r\n", current_pos, pos_offset, (current_pos + pos_offset) ); |
okano | 0:4beb37ae37ce | 190 | motor_out = pat[ (current_pos + pos_offset) & 0x3 ]; |
okano | 0:4beb37ae37ce | 191 | }; |
okano | 0:4beb37ae37ce | 192 | |
okano | 0:4beb37ae37ce | 193 | const unsigned char StepperMotor::pat[ 4 ] = { 0, 1, 3, 2 }; |