Tedd OKANO / StepperMotorUni

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   LAB04_Oppgave1 test_stepper Stepper_Motor_Demo StepperMotorUni_Hello ... more

StepperMotorUni.cpp@0:5d0abdf92786, 2014-01-19 (annotated)

Committer:
okano
Date:
Sun Jan 19 10:59:53 2014 +0000
Revision:
0:5d0abdf92786
Child:
2:6835e719ff96
initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okano 0:5d0abdf92786 1 /** Stepper Motor (Unipolar) control library
okano 0:5d0abdf92786 2 *
okano 0:5d0abdf92786 3 * @class StepperMotorUni
okano 0:5d0abdf92786 4 * @author Tedd OKANO
okano 0:5d0abdf92786 5 * @version 1.0
okano 0:5d0abdf92786 6 * @date 19-Jun-2014
okano 0:5d0abdf92786 7 *
okano 0:5d0abdf92786 8 * Copyright: 2010, 2014 Tedd OKANO
okano 0:5d0abdf92786 9 * Released under the Apache 2 license License
okano 0:5d0abdf92786 10 *
okano 0:5d0abdf92786 11 * The library that controls stepper motor via motor driver chip
okano 0:5d0abdf92786 12 * This is a driver for a unipolar stepper motor.
okano 0:5d0abdf92786 13 *
okano 0:5d0abdf92786 14 * Example:
okano 0:5d0abdf92786 15 * @code
okano 0:5d0abdf92786 16 * #include "mbed.h"
okano 0:5d0abdf92786 17 * #include "StepperMotorUni.h"
okano 0:5d0abdf92786 18 *
okano 0:5d0abdf92786 19 * StepperMotorUni motor( p26, p25, p24, p23 );
okano 0:5d0abdf92786 20 *
okano 0:5d0abdf92786 21 * int main()
okano 0:5d0abdf92786 22 * {
okano 0:5d0abdf92786 23 * motor.set_pps( 50 );
okano 0:5d0abdf92786 24 *
okano 0:5d0abdf92786 25 * while ( 1 ) {
okano 0:5d0abdf92786 26 * motor.move_steps( 24 );
okano 0:5d0abdf92786 27 * wait( 1 );
okano 0:5d0abdf92786 28 *
okano 0:5d0abdf92786 29 * motor.move_steps( -24 );
okano 0:5d0abdf92786 30 * wait( 1 );
okano 0:5d0abdf92786 31 * }
okano 0:5d0abdf92786 32 * }
okano 0:5d0abdf92786 33 * @endcode
okano 0:5d0abdf92786 34 *
okano 0:5d0abdf92786 35 * version 0.51(27-Nov-2010) // initial version (un-published)
okano 0:5d0abdf92786 36 * version 0.6 (15-Jan-2014) // compatible to LPC1768, LPC11U24 and LPC1114 targets
okano 0:5d0abdf92786 37 */
okano 0:5d0abdf92786 38
okano 0:5d0abdf92786 39 #include "mbed.h"
okano 0:5d0abdf92786 40 #include "StepperMotorUni.h"
okano 0:5d0abdf92786 41
okano 0:5d0abdf92786 42 StepperMotorUni::StepperMotorUni(
okano 0:5d0abdf92786 43 PinName out_A,
okano 0:5d0abdf92786 44 PinName out_B,
okano 0:5d0abdf92786 45 PinName out_C,
okano 0:5d0abdf92786 46 PinName out_D,
okano 0:5d0abdf92786 47 PinName position_detect
okano 0:5d0abdf92786 48 ) :
okano 0:5d0abdf92786 49 motor_out( out_A, out_B, out_C, out_D ),
okano 0:5d0abdf92786 50 position_detect_pin( position_detect ),
okano 0:5d0abdf92786 51 rot_mode( SHORTEST ),
okano 0:5d0abdf92786 52 sync_mode( ASYNCHRONOUS ),
okano 0:5d0abdf92786 53 max_pos( 480 ),
okano 0:5d0abdf92786 54 current_pos( 0 ),
okano 0:5d0abdf92786 55 pos_offset( 0 ),
okano 0:5d0abdf92786 56 target_pos( 0 ),
okano 0:5d0abdf92786 57 max_pps( MAX_PPS ),
okano 0:5d0abdf92786 58 init_done( false ),
okano 0:5d0abdf92786 59 pause( false ),
okano 0:5d0abdf92786 60 power_ctrl( false ),
okano 0:5d0abdf92786 61 ramp_init_speed_rate( 1.0 ),
okano 0:5d0abdf92786 62 ramp_control_steps( 0 ),
okano 0:5d0abdf92786 63 ramp_rate( 1.0 ) {
okano 0:5d0abdf92786 64
okano 0:5d0abdf92786 65 pps = max_pps;
okano 0:5d0abdf92786 66 pattern = (unsigned char *)pattern_one_phase;
okano 0:5d0abdf92786 67 pat_index_mask = 0x3;
okano 0:5d0abdf92786 68 t.attach( this, &StepperMotorUni::motor_maintain, 1.0 / (float)pps );
okano 0:5d0abdf92786 69 }
okano 0:5d0abdf92786 70
okano 0:5d0abdf92786 71 float StepperMotorUni::set_pps( float v ) {
okano 0:5d0abdf92786 72 float p;
okano 0:5d0abdf92786 73
okano 0:5d0abdf92786 74 p = pps;
okano 0:5d0abdf92786 75 pps = v;
okano 0:5d0abdf92786 76 t.detach();
okano 0:5d0abdf92786 77 t.attach( this, &StepperMotorUni::motor_maintain, 1.0 / (float)pps );
okano 0:5d0abdf92786 78 return ( p );
okano 0:5d0abdf92786 79 }
okano 0:5d0abdf92786 80
okano 0:5d0abdf92786 81 void StepperMotorUni::set_max_pps( float v ) {
okano 0:5d0abdf92786 82 max_pps = v;
okano 0:5d0abdf92786 83 }
okano 0:5d0abdf92786 84
okano 0:5d0abdf92786 85 int StepperMotorUni::find_home_position( PositionDetectPorarity edge ) {
okano 0:5d0abdf92786 86 RotMode prev_rot;
okano 0:5d0abdf92786 87 SyncMode prev_sync;
okano 0:5d0abdf92786 88 float prev_pps;
okano 0:5d0abdf92786 89 int prev_det_pin;
okano 0:5d0abdf92786 90 int direction;
okano 0:5d0abdf92786 91
okano 0:5d0abdf92786 92 if ( position_detect_pin == NC )
okano 0:5d0abdf92786 93 return 0;
okano 0:5d0abdf92786 94
okano 0:5d0abdf92786 95 prev_pps = set_pps( max_pps );
okano 0:5d0abdf92786 96 prev_rot = rot_mode;
okano 0:5d0abdf92786 97 prev_sync = sync_mode;
okano 0:5d0abdf92786 98 set_sync_mode( SYNCHRONOUS );
okano 0:5d0abdf92786 99 set_rot_mode( SHORTEST );
okano 0:5d0abdf92786 100
okano 0:5d0abdf92786 101 prev_det_pin = position_detect_pin;
okano 0:5d0abdf92786 102
okano 0:5d0abdf92786 103 if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
okano 0:5d0abdf92786 104 direction = -1;
okano 0:5d0abdf92786 105 else
okano 0:5d0abdf92786 106 direction = 1;
okano 0:5d0abdf92786 107
okano 0:5d0abdf92786 108 if ( prev_rot == NO_WRAPAROUND ) {
okano 0:5d0abdf92786 109
okano 0:5d0abdf92786 110 for ( int i = 0; i < (max_pos >> 1); i++ ) {
okano 0:5d0abdf92786 111 move_steps( -1 );
okano 0:5d0abdf92786 112 if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
okano 0:5d0abdf92786 113 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
okano 0:5d0abdf92786 114 set_home_position();
okano 0:5d0abdf92786 115 init_done = true;
okano 0:5d0abdf92786 116 break;
okano 0:5d0abdf92786 117 }
okano 0:5d0abdf92786 118 prev_det_pin = position_detect_pin;
okano 0:5d0abdf92786 119 }
okano 0:5d0abdf92786 120 }
okano 0:5d0abdf92786 121
okano 0:5d0abdf92786 122 for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
okano 0:5d0abdf92786 123 move_steps( direction );
okano 0:5d0abdf92786 124 if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
okano 0:5d0abdf92786 125 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
okano 0:5d0abdf92786 126 set_home_position();
okano 0:5d0abdf92786 127 init_done = true;
okano 0:5d0abdf92786 128 break;
okano 0:5d0abdf92786 129 }
okano 0:5d0abdf92786 130 prev_det_pin = position_detect_pin;
okano 0:5d0abdf92786 131 }
okano 0:5d0abdf92786 132
okano 0:5d0abdf92786 133 go_position( 0 );
okano 0:5d0abdf92786 134 set_pps( prev_pps );
okano 0:5d0abdf92786 135 set_rot_mode( prev_rot );
okano 0:5d0abdf92786 136 set_sync_mode( prev_sync );
okano 0:5d0abdf92786 137
okano 0:5d0abdf92786 138 return ( init_done );
okano 0:5d0abdf92786 139 }
okano 0:5d0abdf92786 140
okano 0:5d0abdf92786 141 void StepperMotorUni::set_home_position( void ) {
okano 0:5d0abdf92786 142 set_pause( true );
okano 0:5d0abdf92786 143 pos_offset = current_pos & 0x3;
okano 0:5d0abdf92786 144 current_pos = 0;
okano 0:5d0abdf92786 145 set_target_pos( 0 );
okano 0:5d0abdf92786 146 set_pause( false );
okano 0:5d0abdf92786 147 }
okano 0:5d0abdf92786 148
okano 0:5d0abdf92786 149
okano 0:5d0abdf92786 150 void StepperMotorUni::go_angle( float angle ) {
okano 0:5d0abdf92786 151 go_position( (int)((angle / 360.0) * (float)max_pos) );
okano 0:5d0abdf92786 152 }
okano 0:5d0abdf92786 153
okano 0:5d0abdf92786 154 int StepperMotorUni::move_steps( int s ) {
okano 0:5d0abdf92786 155 set_target_pos( current_pos + s );
okano 0:5d0abdf92786 156 return ( current_pos );
okano 0:5d0abdf92786 157 }
okano 0:5d0abdf92786 158
okano 0:5d0abdf92786 159 int StepperMotorUni::move_rotates( float r ) {
okano 0:5d0abdf92786 160 set_target_pos( current_pos + (int)(r * (float)max_pos) );
okano 0:5d0abdf92786 161 return ( current_pos );
okano 0:5d0abdf92786 162 }
okano 0:5d0abdf92786 163
okano 0:5d0abdf92786 164 void StepperMotorUni::set_operation_phase_mode( OperationPhaseMode v ) {
okano 0:5d0abdf92786 165
okano 0:5d0abdf92786 166 switch ( v ) {
okano 0:5d0abdf92786 167 case StepperMotorUni::ONE_PHASE :
okano 0:5d0abdf92786 168 pattern = pattern_one_phase;
okano 0:5d0abdf92786 169 pat_index_mask = 0x3;
okano 0:5d0abdf92786 170 break;
okano 0:5d0abdf92786 171 case StepperMotorUni::TWO_PHASE :
okano 0:5d0abdf92786 172 pattern = pattern_two_phase;
okano 0:5d0abdf92786 173 pat_index_mask = 0x3;
okano 0:5d0abdf92786 174 break;
okano 0:5d0abdf92786 175 case StepperMotorUni::HALFSTEP :
okano 0:5d0abdf92786 176 pattern = pattern_halfstep;
okano 0:5d0abdf92786 177 pat_index_mask = 0x7;
okano 0:5d0abdf92786 178 break;
okano 0:5d0abdf92786 179 }
okano 0:5d0abdf92786 180
okano 0:5d0abdf92786 181 phase_mode = v;
okano 0:5d0abdf92786 182 }
okano 0:5d0abdf92786 183
okano 0:5d0abdf92786 184 void StepperMotorUni::set_rot_mode( RotMode m ) {
okano 0:5d0abdf92786 185 rot_mode = m;
okano 0:5d0abdf92786 186 }
okano 0:5d0abdf92786 187
okano 0:5d0abdf92786 188 void StepperMotorUni::set_sync_mode( SyncMode m ) {
okano 0:5d0abdf92786 189 sync_mode = m;
okano 0:5d0abdf92786 190 }
okano 0:5d0abdf92786 191
okano 0:5d0abdf92786 192 int StepperMotorUni::distance( void ) {
okano 0:5d0abdf92786 193 return( target_pos - current_pos );
okano 0:5d0abdf92786 194 }
okano 0:5d0abdf92786 195
okano 0:5d0abdf92786 196 void StepperMotorUni::set_pause( int sw ) {
okano 0:5d0abdf92786 197 pause = sw;
okano 0:5d0abdf92786 198 }
okano 0:5d0abdf92786 199
okano 0:5d0abdf92786 200 void StepperMotorUni::brake( void ) {
okano 0:5d0abdf92786 201 brake( SOFT_BRAKE );
okano 0:5d0abdf92786 202 }
okano 0:5d0abdf92786 203
okano 0:5d0abdf92786 204 void StepperMotorUni::brake( BrakeMode mode ) {
okano 0:5d0abdf92786 205 int extra_steps;
okano 0:5d0abdf92786 206
okano 0:5d0abdf92786 207 if ( mode == SOFT_BRAKE ) {
okano 0:5d0abdf92786 208 extra_steps = abs( target_pos - current_pos );
okano 0:5d0abdf92786 209 extra_steps = extra_steps < ramp_control_steps ? extra_steps : ramp_control_steps;
okano 0:5d0abdf92786 210 extra_steps = target_pos < current_pos ? -extra_steps : extra_steps;
okano 0:5d0abdf92786 211 } else {
okano 0:5d0abdf92786 212 extra_steps = 0;
okano 0:5d0abdf92786 213 }
okano 0:5d0abdf92786 214
okano 0:5d0abdf92786 215 target_pos = current_pos + extra_steps;
okano 0:5d0abdf92786 216 }
okano 0:5d0abdf92786 217
okano 0:5d0abdf92786 218 void StepperMotorUni::set_target_pos( int p ) {
okano 0:5d0abdf92786 219 target_pos = p;
okano 0:5d0abdf92786 220
okano 0:5d0abdf92786 221 if (sync_mode == SYNCHRONOUS)
okano 0:5d0abdf92786 222 while ( distance() )
okano 0:5d0abdf92786 223 wait( 0 );
okano 0:5d0abdf92786 224 }
okano 0:5d0abdf92786 225
okano 0:5d0abdf92786 226 void StepperMotorUni::set_power_ctrl( int sw ) {
okano 0:5d0abdf92786 227 power_ctrl = sw;
okano 0:5d0abdf92786 228 }
okano 0:5d0abdf92786 229
okano 0:5d0abdf92786 230 void StepperMotorUni::set_steps_per_rotate( int steps ) {
okano 0:5d0abdf92786 231 max_pos = steps;
okano 0:5d0abdf92786 232 }
okano 0:5d0abdf92786 233
okano 0:5d0abdf92786 234 void StepperMotorUni::set_ramp_control( float initial_speed_rate, int ramp_steps ) {
okano 0:5d0abdf92786 235 ramp_init_speed_rate = initial_speed_rate;
okano 0:5d0abdf92786 236 ramp_control_steps = ramp_steps;
okano 0:5d0abdf92786 237 ramp_rate = powf( ramp_init_speed_rate, 1.0 / (float)ramp_control_steps );
okano 0:5d0abdf92786 238 }
okano 0:5d0abdf92786 239
okano 0:5d0abdf92786 240 void StepperMotorUni::motor_maintain( void ) {
okano 0:5d0abdf92786 241 int distance;
okano 0:5d0abdf92786 242 static int moved_steps = 0;
okano 0:5d0abdf92786 243 static int ramp_steps = 0;
okano 0:5d0abdf92786 244 static float speed_rate = 1.0;
okano 0:5d0abdf92786 245
okano 0:5d0abdf92786 246 if ( pause )
okano 0:5d0abdf92786 247 return;
okano 0:5d0abdf92786 248
okano 0:5d0abdf92786 249 distance = target_pos - current_pos;
okano 0:5d0abdf92786 250 distance = (distance < 0)? -distance : distance;
okano 0:5d0abdf92786 251
okano 0:5d0abdf92786 252 if ( !distance ) {
okano 0:5d0abdf92786 253 motor_out = power_ctrl ? 0 : 1;
okano 0:5d0abdf92786 254 moved_steps = 0;
okano 0:5d0abdf92786 255 return;
okano 0:5d0abdf92786 256 }
okano 0:5d0abdf92786 257
okano 0:5d0abdf92786 258 if ( !moved_steps ) { // ramp control
okano 0:5d0abdf92786 259 speed_rate = ramp_init_speed_rate;
okano 0:5d0abdf92786 260 ramp_steps = ((ramp_control_steps * 2) - distance) / 2;
okano 0:5d0abdf92786 261 ramp_steps = (0 < ramp_steps) ? ramp_control_steps - ramp_steps : ramp_control_steps;
okano 0:5d0abdf92786 262 // printf( "ramp_steps=%d, speed_rate=%f\r\n", ramp_steps, speed_rate );
okano 0:5d0abdf92786 263 // printf( "diff=%d, speed_rate=%f\r\n", diff, speed_rate );
okano 0:5d0abdf92786 264 } else if ( moved_steps < ramp_steps ) {
okano 0:5d0abdf92786 265 speed_rate /= ramp_rate;
okano 0:5d0abdf92786 266 } else if ( distance <= ramp_steps ) {
okano 0:5d0abdf92786 267 speed_rate *= ramp_rate;
okano 0:5d0abdf92786 268 } else {
okano 0:5d0abdf92786 269 speed_rate = 1.0;
okano 0:5d0abdf92786 270 }
okano 0:5d0abdf92786 271
okano 0:5d0abdf92786 272 moved_steps++;
okano 0:5d0abdf92786 273 current_pos = current_pos + ( (target_pos < current_pos) ? -1 : 1 );
okano 0:5d0abdf92786 274
okano 0:5d0abdf92786 275 motor_out = pattern[ (current_pos + pos_offset) & pat_index_mask ];
okano 0:5d0abdf92786 276
okano 0:5d0abdf92786 277 //printf( "%d>>>%d\r\n", current_pos, target_pos );
okano 0:5d0abdf92786 278
okano 0:5d0abdf92786 279 if ( speed_rate != 1.0 ) {
okano 0:5d0abdf92786 280 // t.detach();
okano 0:5d0abdf92786 281 // t.attach( this, &StepperMotorUni::motor_maintain, ((1.0 / (float)pps)) / speed_rate );
okano 0:5d0abdf92786 282 }
okano 0:5d0abdf92786 283
okano 0:5d0abdf92786 284 if ( target_pos == current_pos ) {
okano 0:5d0abdf92786 285 current_pos = current_pos % max_pos;
okano 0:5d0abdf92786 286 target_pos = target_pos % max_pos;
okano 0:5d0abdf92786 287 moved_steps = 0;
okano 0:5d0abdf92786 288 }
okano 0:5d0abdf92786 289 };
okano 0:5d0abdf92786 290
okano 0:5d0abdf92786 291 int StepperMotorUni::go_position( int target_pos ) {
okano 0:5d0abdf92786 292
okano 0:5d0abdf92786 293 current_pos = current_pos % max_pos;
okano 0:5d0abdf92786 294 current_pos = (current_pos < 0) ? current_pos + max_pos : current_pos;
okano 0:5d0abdf92786 295
okano 0:5d0abdf92786 296 target_pos = target_pos % max_pos;
okano 0:5d0abdf92786 297 target_pos = (target_pos < 0) ? target_pos + max_pos : target_pos;
okano 0:5d0abdf92786 298
okano 0:5d0abdf92786 299 #define CW 0
okano 0:5d0abdf92786 300 #define CCW 1
okano 0:5d0abdf92786 301
okano 0:5d0abdf92786 302 int direction;
okano 0:5d0abdf92786 303
okano 0:5d0abdf92786 304 switch ( rot_mode ) {
okano 0:5d0abdf92786 305 case NO_WRAPAROUND :
okano 0:5d0abdf92786 306 direction = (current_pos < target_pos) ? CW : CCW;
okano 0:5d0abdf92786 307 break;
okano 0:5d0abdf92786 308 case CLOCKWISE_ONLY :
okano 0:5d0abdf92786 309 direction = CW;
okano 0:5d0abdf92786 310 break;
okano 0:5d0abdf92786 311 case COUNTER_CLOCKWISE_ONLY :
okano 0:5d0abdf92786 312 direction = CCW;
okano 0:5d0abdf92786 313 break;
okano 0:5d0abdf92786 314 default : // SHORTEST :
okano 0:5d0abdf92786 315 direction = ( (max_pos >> 1) < ((target_pos + max_pos - current_pos) % max_pos) ) ? CCW : CW;
okano 0:5d0abdf92786 316 break;
okano 0:5d0abdf92786 317 }
okano 0:5d0abdf92786 318
okano 0:5d0abdf92786 319 switch ( direction ) {
okano 0:5d0abdf92786 320 case CW :
okano 0:5d0abdf92786 321 current_pos = (target_pos < current_pos) ? current_pos - max_pos : current_pos;
okano 0:5d0abdf92786 322 break;
okano 0:5d0abdf92786 323 case CCW :
okano 0:5d0abdf92786 324 target_pos = (current_pos < target_pos) ? target_pos - max_pos : target_pos;
okano 0:5d0abdf92786 325 break;
okano 0:5d0abdf92786 326 }
okano 0:5d0abdf92786 327
okano 0:5d0abdf92786 328 set_target_pos( target_pos );
okano 0:5d0abdf92786 329 return ( current_pos );
okano 0:5d0abdf92786 330 }
okano 0:5d0abdf92786 331
okano 0:5d0abdf92786 332 #if 0
okano 0:5d0abdf92786 333 unsigned char StepperMotorUni::pattern_one_phase[ 4 ] = { 0x8, 0x4, 0x2, 0x1 };
okano 0:5d0abdf92786 334 unsigned char StepperMotorUni::pattern_two_phase[ 4 ] = { 0x9, 0xC, 0x6, 0x3 };
okano 0:5d0abdf92786 335 unsigned char StepperMotorUni::pattern_halfstep[ 8 ] = { 0x9, 0x8, 0xC, 0x4, 0x6, 0x2, 0x3, 0x1 };
okano 0:5d0abdf92786 336 #else
okano 0:5d0abdf92786 337 unsigned char StepperMotorUni::pattern_one_phase[ 4 ] = { 0x1, 0x2, 0x4, 0x8 };
okano 0:5d0abdf92786 338 unsigned char StepperMotorUni::pattern_two_phase[ 4 ] = { 0x3, 0x6, 0xC, 0x9 };
okano 0:5d0abdf92786 339 unsigned char StepperMotorUni::pattern_halfstep[ 8 ] = { 0x1, 0x3, 0x2, 0x6, 0x4, 0xC, 0x8, 0x9 };
okano 0:5d0abdf92786 340 #endif