stepper motor controller
Fork of Stepper_Motor_X27168 by
Stepper_Motor_X27168.cpp@0:c346170974bc, 2015-10-20 (annotated)
- Committer:
- agarg45
- Date:
- Tue Oct 20 00:36:06 2015 +0000
- Revision:
- 0:c346170974bc
- Child:
- 1:64889c1fd698
Stepper Motor X27168 ver 1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
agarg45 | 0:c346170974bc | 1 | #include "StepperMotor_X27168.h" |
agarg45 | 0:c346170974bc | 2 | |
agarg45 | 0:c346170974bc | 3 | StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r) |
agarg45 | 0:c346170974bc | 4 | :motor_control(A_f, A_r, B_f, B_r) |
agarg45 | 0:c346170974bc | 5 | { |
agarg45 | 0:c346170974bc | 6 | motor_control = 0x0; |
agarg45 | 0:c346170974bc | 7 | max_position = MAX_POS; |
agarg45 | 0:c346170974bc | 8 | speed = DEFAULT_SPEED; |
agarg45 | 0:c346170974bc | 9 | cur_state = STOP_MOTOR; |
agarg45 | 0:c346170974bc | 10 | cur_position = 0; |
agarg45 | 0:c346170974bc | 11 | } |
agarg45 | 0:c346170974bc | 12 | |
agarg45 | 0:c346170974bc | 13 | StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int init_step_position) |
agarg45 | 0:c346170974bc | 14 | :motor_control(A_f, A_r, B_f, B_r) |
agarg45 | 0:c346170974bc | 15 | { |
agarg45 | 0:c346170974bc | 16 | StepperMotor_X27168(A_f, A_r, B_f, B_r); |
agarg45 | 0:c346170974bc | 17 | step_position(init_step_position); |
agarg45 | 0:c346170974bc | 18 | } |
agarg45 | 0:c346170974bc | 19 | |
agarg45 | 0:c346170974bc | 20 | void StepperMotor_X27168::set_speed(float s) { |
agarg45 | 0:c346170974bc | 21 | speed = s; |
agarg45 | 0:c346170974bc | 22 | } |
agarg45 | 0:c346170974bc | 23 | |
agarg45 | 0:c346170974bc | 24 | int StepperMotor_X27168::get_speed() { |
agarg45 | 0:c346170974bc | 25 | return speed; |
agarg45 | 0:c346170974bc | 26 | } |
agarg45 | 0:c346170974bc | 27 | |
agarg45 | 0:c346170974bc | 28 | void StepperMotor_X27168::set_max_position(int p) { |
agarg45 | 0:c346170974bc | 29 | if(p<MAX_POS) { |
agarg45 | 0:c346170974bc | 30 | max_position = p; |
agarg45 | 0:c346170974bc | 31 | } |
agarg45 | 0:c346170974bc | 32 | } |
agarg45 | 0:c346170974bc | 33 | |
agarg45 | 0:c346170974bc | 34 | int StepperMotor_X27168::get_max_position() { |
agarg45 | 0:c346170974bc | 35 | return max_position; |
agarg45 | 0:c346170974bc | 36 | } |
agarg45 | 0:c346170974bc | 37 | |
agarg45 | 0:c346170974bc | 38 | int StepperMotor_X27168::step(int dir) { |
agarg45 | 0:c346170974bc | 39 | if(dir==2) |
agarg45 | 0:c346170974bc | 40 | cur_state = STOP_MOTOR; |
agarg45 | 0:c346170974bc | 41 | else if(dir == 0) { |
agarg45 | 0:c346170974bc | 42 | cur_state = (Polarity)((cur_state+1)%4); |
agarg45 | 0:c346170974bc | 43 | |
agarg45 | 0:c346170974bc | 44 | if(cur_position <= MAX_POS) { |
agarg45 | 0:c346170974bc | 45 | cur_position++; |
agarg45 | 0:c346170974bc | 46 | } |
agarg45 | 0:c346170974bc | 47 | } |
agarg45 | 0:c346170974bc | 48 | else if (dir == 1) { |
agarg45 | 0:c346170974bc | 49 | cur_state = (Polarity)((cur_state-1)%4); |
agarg45 | 0:c346170974bc | 50 | cur_state = (Polarity)(cur_state == 255 ? cur_state + 4 : cur_state); |
agarg45 | 0:c346170974bc | 51 | |
agarg45 | 0:c346170974bc | 52 | if(cur_position>= 0) { |
agarg45 | 0:c346170974bc | 53 | cur_position--; |
agarg45 | 0:c346170974bc | 54 | } |
agarg45 | 0:c346170974bc | 55 | } |
agarg45 | 0:c346170974bc | 56 | else |
agarg45 | 0:c346170974bc | 57 | return -1; |
agarg45 | 0:c346170974bc | 58 | |
agarg45 | 0:c346170974bc | 59 | switch (cur_state) { |
agarg45 | 0:c346170974bc | 60 | case 0: |
agarg45 | 0:c346170974bc | 61 | motor_control = 0x1; |
agarg45 | 0:c346170974bc | 62 | break; |
agarg45 | 0:c346170974bc | 63 | case 1: |
agarg45 | 0:c346170974bc | 64 | motor_control = 0x4; |
agarg45 | 0:c346170974bc | 65 | break; |
agarg45 | 0:c346170974bc | 66 | case 2: |
agarg45 | 0:c346170974bc | 67 | motor_control = 0x2; |
agarg45 | 0:c346170974bc | 68 | break; |
agarg45 | 0:c346170974bc | 69 | case 3: |
agarg45 | 0:c346170974bc | 70 | motor_control = 0x8; |
agarg45 | 0:c346170974bc | 71 | break; |
agarg45 | 0:c346170974bc | 72 | case 4: |
agarg45 | 0:c346170974bc | 73 | motor_control = 0x0; |
agarg45 | 0:c346170974bc | 74 | break; |
agarg45 | 0:c346170974bc | 75 | } |
agarg45 | 0:c346170974bc | 76 | wait(1.0/speed); |
agarg45 | 0:c346170974bc | 77 | return cur_position; |
agarg45 | 0:c346170974bc | 78 | } |
agarg45 | 0:c346170974bc | 79 | |
agarg45 | 0:c346170974bc | 80 | void StepperMotor_X27168::step_position(int pos) { |
agarg45 | 0:c346170974bc | 81 | if(pos > max_position) |
agarg45 | 0:c346170974bc | 82 | pos = max_position; |
agarg45 | 0:c346170974bc | 83 | else if(pos < 0) |
agarg45 | 0:c346170974bc | 84 | pos = 0; |
agarg45 | 0:c346170974bc | 85 | |
agarg45 | 0:c346170974bc | 86 | while(cur_position < pos) { |
agarg45 | 0:c346170974bc | 87 | step(0); |
agarg45 | 0:c346170974bc | 88 | } |
agarg45 | 0:c346170974bc | 89 | while(cur_position > pos) { |
agarg45 | 0:c346170974bc | 90 | step(1); |
agarg45 | 0:c346170974bc | 91 | } |
agarg45 | 0:c346170974bc | 92 | |
agarg45 | 0:c346170974bc | 93 | step(2); |
agarg45 | 0:c346170974bc | 94 | } |
agarg45 | 0:c346170974bc | 95 | |
agarg45 | 0:c346170974bc | 96 | void StepperMotor_X27168::angle_position(float angle) { |
agarg45 | 0:c346170974bc | 97 | step_position(int(angle*2)); |
agarg45 | 0:c346170974bc | 98 | } |