Tedd OKANO / StepperMotor

Stepper motor control class library

Dependents:   StepperMotor_HelloWorld

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

StepperMotor.cpp@1:dc6cf8f8bcb7, 2010-11-26 (annotated)

Committer:
okano
Date:
Fri Nov 26 15:32:25 2010 +0000
Revision:
1:dc6cf8f8bcb7
Parent:
0:4beb37ae37ce
Child:
2:f14b5e7276b0
rev.0.51 : (a) fixed - set angle by negative big number problem fixed. (b) function set_pps() can take float number. (c) comment improved to have default number information. (d) and small improvements..

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okano 1:dc6cf8f8bcb7 1 /** Stepper Motor control library
okano 1:dc6cf8f8bcb7 2 *
okano 1:dc6cf8f8bcb7 3 * @class StepperMotor
okano 1:dc6cf8f8bcb7 4 * @author Tedd OKANO
okano 1:dc6cf8f8bcb7 5 * @version 0.51(27-Nov-2010)
okano 1:dc6cf8f8bcb7 6 *
okano 1:dc6cf8f8bcb7 7 * Copyright: 2010 Tedd OKANO, Tsukimidai Communications Syndicate - Crawl Design
okano 1:dc6cf8f8bcb7 8 * The library that controls stepper motor via motor driver chip: TA7774
okano 1:dc6cf8f8bcb7 9 * The TA7774 is a driver for a bipolar stepper motor.
okano 1:dc6cf8f8bcb7 10 * With this library, mbed will generate 2 phase pulses to operate the motor.
okano 1:dc6cf8f8bcb7 11 */
okano 1:dc6cf8f8bcb7 12
okano 0:4beb37ae37ce 13 #include "mbed.h"
okano 0:4beb37ae37ce 14 #include "StepperMotor.h"
okano 0:4beb37ae37ce 15
okano 0:4beb37ae37ce 16 StepperMotor::StepperMotor(
okano 0:4beb37ae37ce 17 PinName out_A,
okano 0:4beb37ae37ce 18 PinName out_B,
okano 0:4beb37ae37ce 19 PinName out_PWR,
okano 0:4beb37ae37ce 20 PinName position_detect
okano 0:4beb37ae37ce 21 ) :
okano 0:4beb37ae37ce 22 motor_out( out_A, out_B ),
okano 0:4beb37ae37ce 23 pwr_out( out_PWR ),
okano 0:4beb37ae37ce 24 position_detect_pin( position_detect ),
okano 0:4beb37ae37ce 25 rot_mode( SHORTEST ),
okano 0:4beb37ae37ce 26 sync_mode( ASYNCHRONOUS ),
okano 0:4beb37ae37ce 27 max_pos( 480 ),
okano 0:4beb37ae37ce 28 current_pos( 0 ),
okano 0:4beb37ae37ce 29 pos_offset( 0 ),
okano 0:4beb37ae37ce 30 target_pos( 0 ),
okano 0:4beb37ae37ce 31 max_pps( MAX_PPS ),
okano 0:4beb37ae37ce 32 init_done( false ),
okano 0:4beb37ae37ce 33 pause( false ),
okano 0:4beb37ae37ce 34 power_ctrl( false ) {
okano 0:4beb37ae37ce 35
okano 0:4beb37ae37ce 36 pps = max_pps;
okano 0:4beb37ae37ce 37 t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
okano 0:4beb37ae37ce 38 }
okano 0:4beb37ae37ce 39
okano 1:dc6cf8f8bcb7 40 float StepperMotor::set_pps( float v ) {
okano 1:dc6cf8f8bcb7 41 float p;
okano 0:4beb37ae37ce 42
okano 0:4beb37ae37ce 43 p = pps;
okano 0:4beb37ae37ce 44 pps = v;
okano 0:4beb37ae37ce 45 t.detach();
okano 0:4beb37ae37ce 46 t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
okano 0:4beb37ae37ce 47 return ( p );
okano 0:4beb37ae37ce 48 }
okano 0:4beb37ae37ce 49
okano 1:dc6cf8f8bcb7 50 void StepperMotor::set_max_pps( float v ) {
okano 0:4beb37ae37ce 51 max_pps = v;
okano 0:4beb37ae37ce 52 }
okano 0:4beb37ae37ce 53
okano 0:4beb37ae37ce 54 int StepperMotor::find_home_position( PositionDetectPorarity edge ) {
okano 0:4beb37ae37ce 55 RotMode prev_rot;
okano 0:4beb37ae37ce 56 SyncMode prev_sync;
okano 1:dc6cf8f8bcb7 57 float prev_pps;
okano 0:4beb37ae37ce 58 int prev_det_pin;
okano 0:4beb37ae37ce 59 int direction;
okano 0:4beb37ae37ce 60
okano 0:4beb37ae37ce 61 prev_pps = set_pps( max_pps );
okano 0:4beb37ae37ce 62 prev_rot = rot_mode;
okano 0:4beb37ae37ce 63 prev_sync = sync_mode;
okano 0:4beb37ae37ce 64 set_sync_mode( SYNCHRONOUS );
okano 0:4beb37ae37ce 65 set_rot_mode( SHORTEST );
okano 0:4beb37ae37ce 66
okano 0:4beb37ae37ce 67 prev_det_pin = position_detect_pin;
okano 0:4beb37ae37ce 68
okano 0:4beb37ae37ce 69 if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
okano 0:4beb37ae37ce 70 direction = -1;
okano 0:4beb37ae37ce 71 else
okano 0:4beb37ae37ce 72 direction = 1;
okano 0:4beb37ae37ce 73
okano 0:4beb37ae37ce 74 if ( prev_rot == NO_WRAPAROUND ) {
okano 0:4beb37ae37ce 75
okano 0:4beb37ae37ce 76 for ( int i = 0; i < (max_pos >> 1); i++ ) {
okano 0:4beb37ae37ce 77 move_steps( -1 );
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
okano 0:4beb37ae37ce 88 for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
okano 0:4beb37ae37ce 89 move_steps( direction );
okano 0:4beb37ae37ce 90 if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
okano 0:4beb37ae37ce 91 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
okano 0:4beb37ae37ce 92 set_home_position();
okano 0:4beb37ae37ce 93 init_done = true;
okano 0:4beb37ae37ce 94 break;
okano 0:4beb37ae37ce 95 }
okano 0:4beb37ae37ce 96 prev_det_pin = position_detect_pin;
okano 0:4beb37ae37ce 97 }
okano 0:4beb37ae37ce 98
okano 0:4beb37ae37ce 99 go_position( 0 );
okano 0:4beb37ae37ce 100 set_pps( prev_pps );
okano 0:4beb37ae37ce 101 set_rot_mode( prev_rot );
okano 0:4beb37ae37ce 102 set_sync_mode( prev_sync );
okano 0:4beb37ae37ce 103
okano 0:4beb37ae37ce 104 return ( init_done );
okano 0:4beb37ae37ce 105 }
okano 0:4beb37ae37ce 106
okano 0:4beb37ae37ce 107 void StepperMotor::set_home_position( void ) {
okano 0:4beb37ae37ce 108 set_pause( true );
okano 0:4beb37ae37ce 109 pos_offset = current_pos & 0x3;
okano 0:4beb37ae37ce 110 current_pos = 0;
okano 0:4beb37ae37ce 111 set_target_pos( 0 );
okano 0:4beb37ae37ce 112 set_pause( false );
okano 0:4beb37ae37ce 113 }
okano 0:4beb37ae37ce 114
okano 0:4beb37ae37ce 115 int StepperMotor::go_position( int v ) {
okano 0:4beb37ae37ce 116 set_target_pos( v );
okano 0:4beb37ae37ce 117 return ( current_pos );
okano 0:4beb37ae37ce 118 }
okano 0:4beb37ae37ce 119
okano 0:4beb37ae37ce 120 void StepperMotor::go_angle( float angle ) {
okano 0:4beb37ae37ce 121 go_position( (int)((angle / 360.0) * (float)max_pos) );
okano 0:4beb37ae37ce 122 }
okano 0:4beb37ae37ce 123
okano 0:4beb37ae37ce 124 int StepperMotor::move_steps( int s ) {
okano 0:4beb37ae37ce 125 set_target_pos( current_pos + s );
okano 0:4beb37ae37ce 126 return ( current_pos );
okano 0:4beb37ae37ce 127 }
okano 0:4beb37ae37ce 128
okano 0:4beb37ae37ce 129 void StepperMotor::set_rot_mode( RotMode m ) {
okano 0:4beb37ae37ce 130 rot_mode = m;
okano 0:4beb37ae37ce 131 }
okano 0:4beb37ae37ce 132
okano 0:4beb37ae37ce 133 void StepperMotor::set_sync_mode( SyncMode m ) {
okano 0:4beb37ae37ce 134 sync_mode = m;
okano 0:4beb37ae37ce 135 }
okano 0:4beb37ae37ce 136
okano 0:4beb37ae37ce 137 int StepperMotor::distance( void ) {
okano 0:4beb37ae37ce 138 return( target_pos - current_pos );
okano 0:4beb37ae37ce 139 }
okano 0:4beb37ae37ce 140
okano 0:4beb37ae37ce 141 void StepperMotor::set_pause( int sw ) {
okano 0:4beb37ae37ce 142 pause = sw;
okano 0:4beb37ae37ce 143 }
okano 0:4beb37ae37ce 144
okano 0:4beb37ae37ce 145 void StepperMotor::set_target_pos( int p ) {
okano 1:dc6cf8f8bcb7 146 p = p % max_pos;
okano 1:dc6cf8f8bcb7 147 target_pos = (p < 0) ? (p + max_pos) : p;
okano 1:dc6cf8f8bcb7 148
okano 0:4beb37ae37ce 149 if (sync_mode == SYNCHRONOUS)
okano 0:4beb37ae37ce 150 while ( distance() )
okano 0:4beb37ae37ce 151 wait( 0 );
okano 0:4beb37ae37ce 152 }
okano 0:4beb37ae37ce 153
okano 0:4beb37ae37ce 154 void StepperMotor::set_power_ctrl( int sw ) {
okano 0:4beb37ae37ce 155 power_ctrl = sw;
okano 0:4beb37ae37ce 156 }
okano 0:4beb37ae37ce 157
okano 0:4beb37ae37ce 158 void StepperMotor::set_steps_per_rotate( int steps ) {
okano 0:4beb37ae37ce 159 max_pos = steps;
okano 0:4beb37ae37ce 160 }
okano 0:4beb37ae37ce 161
okano 0:4beb37ae37ce 162 void StepperMotor::motor_maintain( void ) {
okano 0:4beb37ae37ce 163
okano 0:4beb37ae37ce 164 int diff;
okano 0:4beb37ae37ce 165 int direction;
okano 0:4beb37ae37ce 166
okano 0:4beb37ae37ce 167 if ( pause )
okano 0:4beb37ae37ce 168 return;
okano 0:4beb37ae37ce 169
okano 0:4beb37ae37ce 170 diff = target_pos - current_pos;
okano 0:4beb37ae37ce 171
okano 0:4beb37ae37ce 172 if ( !diff ) {
okano 0:4beb37ae37ce 173 pwr_out = power_ctrl ? 0 : 1;
okano 0:4beb37ae37ce 174 return;
okano 0:4beb37ae37ce 175 }
okano 0:4beb37ae37ce 176
okano 0:4beb37ae37ce 177 pwr_out = 1;
okano 0:4beb37ae37ce 178
okano 0:4beb37ae37ce 179 diff = (diff + max_pos) % max_pos;
okano 0:4beb37ae37ce 180
okano 0:4beb37ae37ce 181 if ( diff > (max_pos >> 1) )
okano 0:4beb37ae37ce 182 diff -= max_pos;
okano 0:4beb37ae37ce 183
okano 0:4beb37ae37ce 184 switch ( rot_mode ) {
okano 0:4beb37ae37ce 185 case NO_WRAPAROUND :
okano 0:4beb37ae37ce 186 direction = ( (target_pos - current_pos) < 0 ) ? -1 : 1;
okano 0:4beb37ae37ce 187 break;
okano 0:4beb37ae37ce 188 case CLOCKWISE_ONLY :
okano 0:4beb37ae37ce 189 direction = 1;
okano 0:4beb37ae37ce 190 break;
okano 0:4beb37ae37ce 191 case COUNTER_CLOCKWISE_ONLY :
okano 0:4beb37ae37ce 192 direction = -1;
okano 0:4beb37ae37ce 193 break;
okano 0:4beb37ae37ce 194 default : // SHORTEST :
okano 0:4beb37ae37ce 195 direction = ( diff < 0 ) ? -1 : 1;
okano 0:4beb37ae37ce 196 break;
okano 0:4beb37ae37ce 197 }
okano 0:4beb37ae37ce 198
okano 0:4beb37ae37ce 199
okano 0:4beb37ae37ce 200 current_pos = ((current_pos + max_pos) + direction) % max_pos;
okano 0:4beb37ae37ce 201
okano 0:4beb37ae37ce 202 // printf( "pos=%d ofst=%d puls=%d\r\n", current_pos, pos_offset, (current_pos + pos_offset) );
okano 0:4beb37ae37ce 203 motor_out = pat[ (current_pos + pos_offset) & 0x3 ];
okano 0:4beb37ae37ce 204 };
okano 0:4beb37ae37ce 205
okano 0:4beb37ae37ce 206 const unsigned char StepperMotor::pat[ 4 ] = { 0, 1, 3, 2 };