Hiroshi M
/
StepperMotor
倒立振子ロボットで使用してるユニポーラのステッピングモータの駆動テストプログラムです。
Revision 0:5253455d51ec, committed 2012-07-07
- Comitter:
- bant62
- Date:
- Sat Jul 07 09:12:06 2012 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r 5253455d51ec StepperMotor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StepperMotor.cpp Sat Jul 07 09:12:06 2012 +0000 @@ -0,0 +1,148 @@ +#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; +}
diff -r 000000000000 -r 5253455d51ec StepperMotor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StepperMotor.h Sat Jul 07 09:12:06 2012 +0000 @@ -0,0 +1,73 @@ +/* Copyright (C) 2012 mbed.org, MIT License + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without restriction, + * including without limitation the rights to use, copy, modify, merge, publish, distribute, + * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + + +#ifndef MBED_STEPPER_MOTOR_H +#define MBED_STEPPER_MOTOR_H + +#include "mbed.h" + +typedef enum {PWM_ON, PWM_OFF} pwm_state; +typedef enum {CLOCK_WISE, ANTI_CLOCK_WISE} motor_dir; + +struct speed_data_t { + int max_pulse_count; + int pwm_ratio; + uint8_t *clockwise_ptn; + uint8_t *anticlockwise_ptn; + int ptn_count; +}; + +#define PULSE_INTERVAL 14 // 13us + +class StepperMotor +{ +public: + StepperMotor(PinName x_pin_no, PinName y_pin_no, PinName nx_pin_no, PinName ny_pin_no); + ~StepperMotor(); + + void SetSpeed(int speed, motor_dir direction); + void PulseEnable(void); + void PulseDisable(void); + +private: + pwm_state state; + motor_dir direction; + int ptn_index; + int ptn_count; + int pwm_ratio; + int pwm_on_count; + int pwm_off_count; + int max_pulse_count; + int pulse_count; + uint8_t *ptn; + uint8_t ptn_data; + + DigitalOut _x; + DigitalOut _y; + DigitalOut _nx; + DigitalOut _ny; + + Ticker Pulse; + + void SetPulse14us(void); + void PulseOut(void); + void PulseStop(void); +}; + +#endif
diff -r 000000000000 -r 5253455d51ec main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Jul 07 09:12:06 2012 +0000 @@ -0,0 +1,67 @@ +#include "mbed.h" +#include "StepperMotor.h" + +BusOut myleds(LED1, LED2, LED3, LED4); + +Serial pc(USBTX, USBRX); + +DigitalOut ae_fxma108(p29); + +DigitalIn pb(p19); +DigitalIn pw(p20); + +const char Welcome_Message[] = + "\r\nHello mbed World!\r\n" + "Expand your creativity and enjoy making.\r\n\r\n" + "Stepper Motor Test Mode.\r\n\r\n"; + + +StepperMotor r_motor(p28,p27,p26,p25); +StepperMotor l_motor(p24,p23,p22,p21); + +int main() { + pc.printf(Welcome_Message); + + int old_pb=0; + int new_pb; + int old_pw=0; + int new_pw; + static int speed = 0; + + pb.mode(PullUp); + pw.mode(PullUp); + + ae_fxma108 = 0; + + r_motor.SetSpeed(speed,CLOCK_WISE); + l_motor.SetSpeed(speed,ANTI_CLOCK_WISE); + + r_motor.PulseEnable(); + l_motor.PulseEnable(); + + while(1) { + new_pb = pb; + if ((new_pb==0) && (old_pb==1)) { + if (speed == 0) continue; + speed--; + pc.printf("speed: %d\r\n",speed); + r_motor.SetSpeed(speed,CLOCK_WISE); + l_motor.SetSpeed(speed,ANTI_CLOCK_WISE); + } + old_pb = new_pb; + + + new_pw = pw; + if ((new_pw==0) && (old_pw==1)) { + if (speed == 10) continue; + speed++; + + pc.printf("speed: %d\r\n",speed); + r_motor.SetSpeed(speed,CLOCK_WISE); + l_motor.SetSpeed(speed,ANTI_CLOCK_WISE); + } + old_pw = new_pw; + + myleds = speed; + } +}
diff -r 000000000000 -r 5253455d51ec mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Jul 07 09:12:06 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479