Hiroshi M
/
StepperMotor
倒立振子ロボットで使用してるユニポーラのステッピングモータの駆動テストプログラムです。
StepperMotor.cpp
- Committer:
- bant62
- Date:
- 2012-07-07
- Revision:
- 0:5253455d51ec
File content as of revision 0:5253455d51ec:
#include "StepperMotor.h" #include "mbed.h" //Table for motor steps static uint8_t ptn_ClockWise_1x1[] = {0x01, 0x02, 0x04, 0x08}; static uint8_t ptn_AntiClockWise_1x1[] = {0x08, 0x04, 0x02, 0x01}; static uint8_t ptn_ClockWise_2x2[] = {0x03, 0x06, 0x0C, 0x09}; static uint8_t ptn_AntiClockWise_2x2[] = {0x09, 0x0C, 0x06, 0x03}; static uint8_t ptn_ClockWise_1x2[] = {0x03, 0x02, 0x06, 0x04, 0x0C, 0x08, 0x09, 0x01}; static uint8_t ptn_AntiClockWise_1x2[] = {0x01, 0x09, 0x08, 0x0C, 0x04, 0x06, 0x02, 0x03}; static speed_data_t speed_data [] = { { 1,0,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 0 {1000*2,5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 1 {500*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 2 {333*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 3 {250*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8}, // 4 {200, 7,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 5 {167, 8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 6 {143, 8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4}, // 7 {125, 9,ptn_ClockWise_1x1,ptn_AntiClockWise_2x2,4}, // 8 {111, 10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4}, // 9 {100, 10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4} // 10 }; // Constractor StepperMotor::StepperMotor(PinName x_pin_no, PinName y_pin_no, PinName nx_pin_no, PinName ny_pin_no) : _x(x_pin_no), _y(y_pin_no), _nx(nx_pin_no), _ny(ny_pin_no) { } // Destrutctor StepperMotor::~StepperMotor() { } // void StepperMotor::PulseEnable(void) { Pulse.attach_us(this,&StepperMotor::SetPulse14us,PULSE_INTERVAL); } // void StepperMotor::PulseDisable(void) { Pulse.detach(); } void StepperMotor::SetSpeed(int speed, motor_dir direction) { if (speed > 10) speed = 10; if (speed < 0) speed =0; pwm_ratio = speed_data[speed].pwm_ratio; max_pulse_count = speed_data[speed].max_pulse_count; if (direction == CLOCK_WISE) { ptn = speed_data[speed].clockwise_ptn; } else { ptn = speed_data[speed].anticlockwise_ptn; } ptn_count = speed_data[speed].ptn_count; pulse_count = 0; ptn_index = 0; pwm_on_count = pwm_ratio; pwm_off_count = 10 - pwm_ratio; state = PWM_ON; } /* private functions */ void StepperMotor::SetPulse14us(void) { if (++pulse_count == max_pulse_count) { pulse_count = 0; if (++ptn_index == ptn_count) { ptn_index = 0; } ptn_data = ptn[ptn_index]; } if (pwm_ratio == 10) { PulseOut(); } else if (pwm_ratio == 0) { PulseStop(); } else { switch (state) { case PWM_ON: if (--pwm_on_count !=0) { PulseOut(); } else { pwm_on_count = pwm_ratio; state = PWM_OFF; } break; case PWM_OFF: if (--pwm_off_count != 0) { PulseStop(); } else { pwm_off_count = 10 - pwm_ratio; state = PWM_ON; } break; } } } void StepperMotor::PulseOut(void) { //X if ((ptn_data & 0x01) == 0x01) { _x = 1; } else { _x = 0; } //Y if ((ptn_data & 0x02) == 0x02) { _y = 1; } else { _y = 0; } //Negative X if ((ptn_data & 0x04) == 0x04) { _nx = 1; } else { _nx = 0; } //Negative Y if ((ptn_data & 0x08) == 0x08) { _ny = 1; } else { _ny = 0; } } void StepperMotor::PulseStop(void) { _x = 0; _y = 0; _nx = 0; _ny = 0; }