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@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