StepperMotor - 倒立振子ロボットで使用してるユニポーラのステッピングモータの駆動テストプログラムです。

Users » bant62 » Code » StepperMotor

Hiroshi M / Mbed 2 deprecated StepperMotor

倒立振子ロボットで使用してるユニポーラのステッピングモータの駆動テストプログラムです。

StepperMotor.cpp@0:5253455d51ec, 2012-07-07 (annotated)

Committer:: bant62
Date:: Sat Jul 07 09:12:06 2012 +0000
Revision:: 0:5253455d51ec

Who changed what in which revision?

User	Revision	Line number	New contents of line
bant62	0:5253455d51ec	1	#include "StepperMotor.h"
bant62	0:5253455d51ec	2	#include "mbed.h"
bant62	0:5253455d51ec	3
bant62	0:5253455d51ec	4	//Table for motor steps
bant62	0:5253455d51ec	5	static uint8_t ptn_ClockWise_1x1[] = {0x01, 0x02, 0x04, 0x08};
bant62	0:5253455d51ec	6	static uint8_t ptn_AntiClockWise_1x1[] = {0x08, 0x04, 0x02, 0x01};
bant62	0:5253455d51ec	7
bant62	0:5253455d51ec	8	static uint8_t ptn_ClockWise_2x2[] = {0x03, 0x06, 0x0C, 0x09};
bant62	0:5253455d51ec	9	static uint8_t ptn_AntiClockWise_2x2[] = {0x09, 0x0C, 0x06, 0x03};
bant62	0:5253455d51ec	10
bant62	0:5253455d51ec	11	static uint8_t ptn_ClockWise_1x2[] = {0x03, 0x02, 0x06, 0x04, 0x0C, 0x08, 0x09, 0x01};
bant62	0:5253455d51ec	12	static uint8_t ptn_AntiClockWise_1x2[] = {0x01, 0x09, 0x08, 0x0C, 0x04, 0x06, 0x02, 0x03};
bant62	0:5253455d51ec	13
bant62	0:5253455d51ec	14	static speed_data_t speed_data [] =
bant62	0:5253455d51ec	15	{
bant62	0:5253455d51ec	16	{ 1,0,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 0
bant62	0:5253455d51ec	17	{1000*2,5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 1
bant62	0:5253455d51ec	18	{500*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 2
bant62	0:5253455d51ec	19	{333*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 3
bant62	0:5253455d51ec	20	{250*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 4
bant62	0:5253455d51ec	21	{200, 7,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 5
bant62	0:5253455d51ec	22	{167, 8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 6
bant62	0:5253455d51ec	23	{143, 8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 7
bant62	0:5253455d51ec	24	{125, 9,ptn_ClockWise_1x1,ptn_AntiClockWise_2x2,4}, // 8
bant62	0:5253455d51ec	25	{111, 10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4}, // 9
bant62	0:5253455d51ec	26	{100, 10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4} // 10
bant62	0:5253455d51ec	27	};
bant62	0:5253455d51ec	28
bant62	0:5253455d51ec	29
bant62	0:5253455d51ec	30
bant62	0:5253455d51ec	31	// Constractor
bant62	0:5253455d51ec	32	StepperMotor::StepperMotor(PinName x_pin_no, PinName y_pin_no, PinName nx_pin_no, PinName ny_pin_no)
bant62	0:5253455d51ec	33	: _x(x_pin_no), _y(y_pin_no), _nx(nx_pin_no), _ny(ny_pin_no) {
bant62	0:5253455d51ec	34	}
bant62	0:5253455d51ec	35
bant62	0:5253455d51ec	36	// Destrutctor
bant62	0:5253455d51ec	37	StepperMotor::~StepperMotor() {
bant62	0:5253455d51ec	38	}
bant62	0:5253455d51ec	39
bant62	0:5253455d51ec	40	//
bant62	0:5253455d51ec	41	void StepperMotor::PulseEnable(void) {
bant62	0:5253455d51ec	42	Pulse.attach_us(this,&StepperMotor::SetPulse14us,PULSE_INTERVAL);
bant62	0:5253455d51ec	43	}
bant62	0:5253455d51ec	44
bant62	0:5253455d51ec	45	//
bant62	0:5253455d51ec	46	void StepperMotor::PulseDisable(void) {
bant62	0:5253455d51ec	47	Pulse.detach();
bant62	0:5253455d51ec	48	}
bant62	0:5253455d51ec	49
bant62	0:5253455d51ec	50	void StepperMotor::SetSpeed(int speed, motor_dir direction) {
bant62	0:5253455d51ec	51
bant62	0:5253455d51ec	52	if (speed > 10) speed = 10;
bant62	0:5253455d51ec	53	if (speed < 0) speed =0;
bant62	0:5253455d51ec	54
bant62	0:5253455d51ec	55
bant62	0:5253455d51ec	56	pwm_ratio = speed_data[speed].pwm_ratio;
bant62	0:5253455d51ec	57	max_pulse_count = speed_data[speed].max_pulse_count;
bant62	0:5253455d51ec	58
bant62	0:5253455d51ec	59	if (direction == CLOCK_WISE) {
bant62	0:5253455d51ec	60	ptn = speed_data[speed].clockwise_ptn;
bant62	0:5253455d51ec	61	}
bant62	0:5253455d51ec	62	else {
bant62	0:5253455d51ec	63	ptn = speed_data[speed].anticlockwise_ptn;
bant62	0:5253455d51ec	64	}
bant62	0:5253455d51ec	65	ptn_count = speed_data[speed].ptn_count;
bant62	0:5253455d51ec	66
bant62	0:5253455d51ec	67	pulse_count = 0;
bant62	0:5253455d51ec	68	ptn_index = 0;
bant62	0:5253455d51ec	69
bant62	0:5253455d51ec	70	pwm_on_count = pwm_ratio;
bant62	0:5253455d51ec	71	pwm_off_count = 10 - pwm_ratio;
bant62	0:5253455d51ec	72
bant62	0:5253455d51ec	73	state = PWM_ON;
bant62	0:5253455d51ec	74	}
bant62	0:5253455d51ec	75
bant62	0:5253455d51ec	76
bant62	0:5253455d51ec	77	/* private functions */
bant62	0:5253455d51ec	78
bant62	0:5253455d51ec	79	void StepperMotor::SetPulse14us(void) {
bant62	0:5253455d51ec	80
bant62	0:5253455d51ec	81	if (++pulse_count == max_pulse_count) {
bant62	0:5253455d51ec	82	pulse_count = 0;
bant62	0:5253455d51ec	83
bant62	0:5253455d51ec	84	if (++ptn_index == ptn_count) {
bant62	0:5253455d51ec	85	ptn_index = 0;
bant62	0:5253455d51ec	86	}
bant62	0:5253455d51ec	87	ptn_data = ptn[ptn_index];
bant62	0:5253455d51ec	88	}
bant62	0:5253455d51ec	89
bant62	0:5253455d51ec	90	if (pwm_ratio == 10) {
bant62	0:5253455d51ec	91	PulseOut();
bant62	0:5253455d51ec	92	} else if (pwm_ratio == 0) {
bant62	0:5253455d51ec	93	PulseStop();
bant62	0:5253455d51ec	94	} else {
bant62	0:5253455d51ec	95	switch (state) {
bant62	0:5253455d51ec	96	case PWM_ON:
bant62	0:5253455d51ec	97	if (--pwm_on_count !=0) {
bant62	0:5253455d51ec	98	PulseOut();
bant62	0:5253455d51ec	99	} else {
bant62	0:5253455d51ec	100	pwm_on_count = pwm_ratio;
bant62	0:5253455d51ec	101	state = PWM_OFF;
bant62	0:5253455d51ec	102	}
bant62	0:5253455d51ec	103	break;
bant62	0:5253455d51ec	104	case PWM_OFF:
bant62	0:5253455d51ec	105	if (--pwm_off_count != 0) {
bant62	0:5253455d51ec	106	PulseStop();
bant62	0:5253455d51ec	107	} else {
bant62	0:5253455d51ec	108	pwm_off_count = 10 - pwm_ratio;
bant62	0:5253455d51ec	109	state = PWM_ON;
bant62	0:5253455d51ec	110	}
bant62	0:5253455d51ec	111	break;
bant62	0:5253455d51ec	112	}
bant62	0:5253455d51ec	113	}
bant62	0:5253455d51ec	114	}
bant62	0:5253455d51ec	115
bant62	0:5253455d51ec	116	void StepperMotor::PulseOut(void) {
bant62	0:5253455d51ec	117	//X
bant62	0:5253455d51ec	118	if ((ptn_data & 0x01) == 0x01) {
bant62	0:5253455d51ec	119	_x = 1;
bant62	0:5253455d51ec	120	} else {
bant62	0:5253455d51ec	121	_x = 0;
bant62	0:5253455d51ec	122	}
bant62	0:5253455d51ec	123	//Y
bant62	0:5253455d51ec	124	if ((ptn_data & 0x02) == 0x02) {
bant62	0:5253455d51ec	125	_y = 1;
bant62	0:5253455d51ec	126	} else {
bant62	0:5253455d51ec	127	_y = 0;
bant62	0:5253455d51ec	128	}
bant62	0:5253455d51ec	129	//Negative X
bant62	0:5253455d51ec	130	if ((ptn_data & 0x04) == 0x04) {
bant62	0:5253455d51ec	131	_nx = 1;
bant62	0:5253455d51ec	132	} else {
bant62	0:5253455d51ec	133	_nx = 0;
bant62	0:5253455d51ec	134	}
bant62	0:5253455d51ec	135	//Negative Y
bant62	0:5253455d51ec	136	if ((ptn_data & 0x08) == 0x08) {
bant62	0:5253455d51ec	137	_ny = 1;
bant62	0:5253455d51ec	138	} else {
bant62	0:5253455d51ec	139	_ny = 0;
bant62	0:5253455d51ec	140	}
bant62	0:5253455d51ec	141	}
bant62	0:5253455d51ec	142
bant62	0:5253455d51ec	143	void StepperMotor::PulseStop(void) {
bant62	0:5253455d51ec	144	_x = 0;
bant62	0:5253455d51ec	145	_y = 0;
bant62	0:5253455d51ec	146	_nx = 0;
bant62	0:5253455d51ec	147	_ny = 0;
bant62	0:5253455d51ec	148	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	07 Jul 2012
Imports:	22
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	2

StepperMotor.cpp@0:5253455d51ec, 2012-07-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning