Hiroshi M
/
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 | } |