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@3:0eec6148e739, 2015-04-21 (annotated)

Committer:
okano
Date:
Tue Apr 21 14:37:52 2015 +0000
Revision:
3:0eec6148e739
Parent:
2:6835e719ff96
Child:
4:6909efe4c8ac
ramp control enabled

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