Adds pointer acceleration to combat missed steps and losing track of position
Fork of Stepper_Motor_X27168 by
Stepper_Motor_X27168.cpp@1:e7c0920184e4, 2016-05-02 (annotated)
- Committer:
- clively6
- Date:
- Mon May 02 15:03:35 2016 +0000
- Revision:
- 1:e7c0920184e4
- Parent:
- 0:c346170974bc
Added accelleration to combat missed steps;
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; |
clively6 | 1:e7c0920184e4 | 9 | max_speed = 1000; |
agarg45 | 0:c346170974bc | 10 | cur_state = STOP_MOTOR; |
agarg45 | 0:c346170974bc | 11 | cur_position = 0; |
agarg45 | 0:c346170974bc | 12 | } |
agarg45 | 0:c346170974bc | 13 | |
agarg45 | 0:c346170974bc | 14 | StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int init_step_position) |
agarg45 | 0:c346170974bc | 15 | :motor_control(A_f, A_r, B_f, B_r) |
agarg45 | 0:c346170974bc | 16 | { |
agarg45 | 0:c346170974bc | 17 | StepperMotor_X27168(A_f, A_r, B_f, B_r); |
agarg45 | 0:c346170974bc | 18 | step_position(init_step_position); |
agarg45 | 0:c346170974bc | 19 | } |
agarg45 | 0:c346170974bc | 20 | |
agarg45 | 0:c346170974bc | 21 | void StepperMotor_X27168::set_speed(float s) { |
agarg45 | 0:c346170974bc | 22 | speed = s; |
agarg45 | 0:c346170974bc | 23 | } |
agarg45 | 0:c346170974bc | 24 | |
clively6 | 1:e7c0920184e4 | 25 | void StepperMotor_X27168::set_max_speed(float s) { |
clively6 | 1:e7c0920184e4 | 26 | max_speed = s; |
clively6 | 1:e7c0920184e4 | 27 | } |
clively6 | 1:e7c0920184e4 | 28 | |
agarg45 | 0:c346170974bc | 29 | int StepperMotor_X27168::get_speed() { |
agarg45 | 0:c346170974bc | 30 | return speed; |
agarg45 | 0:c346170974bc | 31 | } |
agarg45 | 0:c346170974bc | 32 | |
clively6 | 1:e7c0920184e4 | 33 | int StepperMotor_X27168::get_position() { |
clively6 | 1:e7c0920184e4 | 34 | return cur_position; |
clively6 | 1:e7c0920184e4 | 35 | } |
clively6 | 1:e7c0920184e4 | 36 | |
agarg45 | 0:c346170974bc | 37 | void StepperMotor_X27168::set_max_position(int p) { |
agarg45 | 0:c346170974bc | 38 | if(p<MAX_POS) { |
agarg45 | 0:c346170974bc | 39 | max_position = p; |
agarg45 | 0:c346170974bc | 40 | } |
agarg45 | 0:c346170974bc | 41 | } |
agarg45 | 0:c346170974bc | 42 | |
agarg45 | 0:c346170974bc | 43 | int StepperMotor_X27168::get_max_position() { |
agarg45 | 0:c346170974bc | 44 | return max_position; |
agarg45 | 0:c346170974bc | 45 | } |
agarg45 | 0:c346170974bc | 46 | |
agarg45 | 0:c346170974bc | 47 | int StepperMotor_X27168::step(int dir) { |
agarg45 | 0:c346170974bc | 48 | if(dir==2) |
agarg45 | 0:c346170974bc | 49 | cur_state = STOP_MOTOR; |
agarg45 | 0:c346170974bc | 50 | else if(dir == 0) { |
agarg45 | 0:c346170974bc | 51 | cur_state = (Polarity)((cur_state+1)%4); |
agarg45 | 0:c346170974bc | 52 | |
agarg45 | 0:c346170974bc | 53 | if(cur_position <= MAX_POS) { |
agarg45 | 0:c346170974bc | 54 | cur_position++; |
agarg45 | 0:c346170974bc | 55 | } |
agarg45 | 0:c346170974bc | 56 | } |
agarg45 | 0:c346170974bc | 57 | else if (dir == 1) { |
agarg45 | 0:c346170974bc | 58 | cur_state = (Polarity)((cur_state-1)%4); |
agarg45 | 0:c346170974bc | 59 | cur_state = (Polarity)(cur_state == 255 ? cur_state + 4 : cur_state); |
agarg45 | 0:c346170974bc | 60 | |
agarg45 | 0:c346170974bc | 61 | if(cur_position>= 0) { |
agarg45 | 0:c346170974bc | 62 | cur_position--; |
agarg45 | 0:c346170974bc | 63 | } |
agarg45 | 0:c346170974bc | 64 | } |
agarg45 | 0:c346170974bc | 65 | else |
agarg45 | 0:c346170974bc | 66 | return -1; |
agarg45 | 0:c346170974bc | 67 | |
agarg45 | 0:c346170974bc | 68 | switch (cur_state) { |
agarg45 | 0:c346170974bc | 69 | case 0: |
agarg45 | 0:c346170974bc | 70 | motor_control = 0x1; |
agarg45 | 0:c346170974bc | 71 | break; |
agarg45 | 0:c346170974bc | 72 | case 1: |
agarg45 | 0:c346170974bc | 73 | motor_control = 0x4; |
agarg45 | 0:c346170974bc | 74 | break; |
agarg45 | 0:c346170974bc | 75 | case 2: |
agarg45 | 0:c346170974bc | 76 | motor_control = 0x2; |
agarg45 | 0:c346170974bc | 77 | break; |
agarg45 | 0:c346170974bc | 78 | case 3: |
agarg45 | 0:c346170974bc | 79 | motor_control = 0x8; |
agarg45 | 0:c346170974bc | 80 | break; |
agarg45 | 0:c346170974bc | 81 | case 4: |
agarg45 | 0:c346170974bc | 82 | motor_control = 0x0; |
agarg45 | 0:c346170974bc | 83 | break; |
agarg45 | 0:c346170974bc | 84 | } |
agarg45 | 0:c346170974bc | 85 | wait(1.0/speed); |
agarg45 | 0:c346170974bc | 86 | return cur_position; |
agarg45 | 0:c346170974bc | 87 | } |
agarg45 | 0:c346170974bc | 88 | |
agarg45 | 0:c346170974bc | 89 | void StepperMotor_X27168::step_position(int pos) { |
agarg45 | 0:c346170974bc | 90 | if(pos > max_position) |
agarg45 | 0:c346170974bc | 91 | pos = max_position; |
agarg45 | 0:c346170974bc | 92 | else if(pos < 0) |
agarg45 | 0:c346170974bc | 93 | pos = 0; |
agarg45 | 0:c346170974bc | 94 | |
agarg45 | 0:c346170974bc | 95 | while(cur_position < pos) { |
agarg45 | 0:c346170974bc | 96 | step(0); |
agarg45 | 0:c346170974bc | 97 | } |
agarg45 | 0:c346170974bc | 98 | while(cur_position > pos) { |
agarg45 | 0:c346170974bc | 99 | step(1); |
agarg45 | 0:c346170974bc | 100 | } |
agarg45 | 0:c346170974bc | 101 | |
agarg45 | 0:c346170974bc | 102 | step(2); |
agarg45 | 0:c346170974bc | 103 | } |
agarg45 | 0:c346170974bc | 104 | |
clively6 | 1:e7c0920184e4 | 105 | void StepperMotor_X27168::set_position_dynamic(float pos){ |
clively6 | 1:e7c0920184e4 | 106 | if(pos > max_position) |
clively6 | 1:e7c0920184e4 | 107 | pos = max_position; |
clively6 | 1:e7c0920184e4 | 108 | else if(pos < 0) |
clively6 | 1:e7c0920184e4 | 109 | pos = 0; |
clively6 | 1:e7c0920184e4 | 110 | |
clively6 | 1:e7c0920184e4 | 111 | |
clively6 | 1:e7c0920184e4 | 112 | int delta = abs(pos - cur_position); |
clively6 | 1:e7c0920184e4 | 113 | speed = 100; |
clively6 | 1:e7c0920184e4 | 114 | |
clively6 | 1:e7c0920184e4 | 115 | set_speed(speed); |
clively6 | 1:e7c0920184e4 | 116 | |
clively6 | 1:e7c0920184e4 | 117 | |
clively6 | 1:e7c0920184e4 | 118 | while(cur_position < pos) { |
clively6 | 1:e7c0920184e4 | 119 | if(abs(pos - cur_position) > delta/2 && speed < max_speed) |
clively6 | 1:e7c0920184e4 | 120 | speed +=50; |
clively6 | 1:e7c0920184e4 | 121 | if(abs(pos - cur_position) <= delta/2&& abs(pos - cur_position) <= 25 && speed >100) |
clively6 | 1:e7c0920184e4 | 122 | speed -= 50; |
clively6 | 1:e7c0920184e4 | 123 | |
clively6 | 1:e7c0920184e4 | 124 | step(0); |
clively6 | 1:e7c0920184e4 | 125 | } |
clively6 | 1:e7c0920184e4 | 126 | while(cur_position > pos) { |
clively6 | 1:e7c0920184e4 | 127 | if(abs(pos - cur_position) > delta/2 && speed < max_speed) |
clively6 | 1:e7c0920184e4 | 128 | speed +=50; |
clively6 | 1:e7c0920184e4 | 129 | if(abs(pos - cur_position) <= delta/2 && abs(pos - cur_position) <= 25 && speed >100) |
clively6 | 1:e7c0920184e4 | 130 | speed -= 50; |
clively6 | 1:e7c0920184e4 | 131 | |
clively6 | 1:e7c0920184e4 | 132 | step(1); |
clively6 | 1:e7c0920184e4 | 133 | } |
clively6 | 1:e7c0920184e4 | 134 | |
clively6 | 1:e7c0920184e4 | 135 | step(2); |
clively6 | 1:e7c0920184e4 | 136 | |
clively6 | 1:e7c0920184e4 | 137 | |
clively6 | 1:e7c0920184e4 | 138 | |
clively6 | 1:e7c0920184e4 | 139 | } |
clively6 | 1:e7c0920184e4 | 140 | |
agarg45 | 0:c346170974bc | 141 | void StepperMotor_X27168::angle_position(float angle) { |
clively6 | 1:e7c0920184e4 | 142 | set_position_dynamic(int(angle*2)); |
agarg45 | 0:c346170974bc | 143 | } |