Tedd OKANO / StepperMotorUni

Unipolar stepper motor operation library

Dependents:   LAB04_Oppgave1 test_stepper Stepper_Motor_Demo StepperMotorUni_Hello ... more

Unipolar stepper motor library

This library generates pulses on 4 digital output pins of the mbed. The pulses are generated by mbed's ticker function.

The mbed pins cannot drive the stepper motor directly. So it requires driver stage for the motor. The circuit may be like following diagram.
The driver stage should be chosen by requirement for the stepper motor.

/media/uploads/okano/unipolar-steppermotor-sample.png

The mbed generates pulses on 4 output pins for external driver stage.
This library can generate 3 types of pulses.

1 phase drive (wave drive) /media/uploads/okano/1phase_drive.gif

2 phase drive /media/uploads/okano/2phase_drive.gif

1-2 phase (half step) drive /media/uploads/okano/halfstep_drive.gif

Components pages

Components pages are available for bipolar and unipolar motor libraries

A bipolar stepper motor driving pulse generator

A unipolar stepper motor driving pulse generator

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