StepperMotor -

Users » farbodjam » Code » StepperMotor

StepperMotor.cpp@0:6a968d1c95c7, 2011-08-27 (annotated)

Committer:: farbodjam
Date:: Sat Aug 27 09:56:46 2011 +0000
Revision:: 0:6a968d1c95c7

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	27 Aug 2011
Imports:	42
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	4

StepperMotor.cpp@0:6a968d1c95c7, 2011-08-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning