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@2:f14b5e7276b0, 2017-09-13 (annotated)

Committer:
okano
Date:
Wed Sep 13 04:30:35 2017 +0000
Revision:
2:f14b5e7276b0
Parent:
1:dc6cf8f8bcb7 
version 0.6: fixed to keep last position while power-control disabled

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