StepperMotor - Stepper motor control class library

Users » okano » Code » StepperMotor

Tedd OKANO / StepperMotor

Stepper motor control class library

Dependents: StepperMotor_HelloWorld

Components pages

Components pages are available for bipolar and unipolar motor libraries

Components / Stepper motor (bipolar)

A bipolar stepper motor driving pulse generator

Components / Stepper motor (unipolar)

A unipolar stepper motor driving pulse generator

StepperMotor.cpp@0:4beb37ae37ce, 2010-11-25 (annotated)

Committer:: okano
Date:: Thu Nov 25 11:11:51 2010 +0000
Revision:: 0:4beb37ae37ce
Child:: 1:dc6cf8f8bcb7

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	25 Nov 2010
Imports:	994
Forks:	0
Commits:	3
Dependents:	1
Dependencies:	0
Followers:	29
Issues:	0

Components

Stepper motor (bipolar)