Commit | Line | Data |
---|---|---|
7b49793d | 1 | /* |
feb204be AW |
2 | This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). |
3 | Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. | |
4 | Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. | |
7b49793d | 5 | You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. |
feb204be | 6 | */ |
670fa10b | 7 | #include "StepperMotor.h" |
5673fe39 MM |
8 | |
9 | #include "Kernel.h" | |
8f91e4e6 | 10 | #include "MRI_Hooks.h" |
61134a65 JM |
11 | #include "StepTicker.h" |
12 | ||
13 | #include <math.h> | |
feb204be | 14 | |
3494f3d0 | 15 | // in steps/sec the default minimum speed (was 20steps/sec hardcoded) |
9502f9d5 | 16 | float StepperMotor::default_minimum_actuator_rate= 20.0F; |
3494f3d0 | 17 | |
d337942a | 18 | // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed |
93694d6b | 19 | |
728477c4 JM |
20 | StepperMotor::StepperMotor() |
21 | { | |
22 | init(); | |
23 | } | |
df6a30f2 | 24 | |
728477c4 JM |
25 | StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en) |
26 | { | |
27 | init(); | |
28 | enable(false); | |
29 | set_high_on_debug(en.port_number, en.pin); | |
670fa10b L |
30 | } |
31 | ||
3494f3d0 JM |
32 | StepperMotor::~StepperMotor() |
33 | { | |
3494f3d0 JM |
34 | } |
35 | ||
728477c4 JM |
36 | void StepperMotor::init() |
37 | { | |
96f12364 | 38 | // register this motor with the step ticker, and get its index in that array and bit position |
1fce036c | 39 | this->index= THEKERNEL->step_ticker->register_motor(this); |
feb204be | 40 | this->moving = false; |
feb204be | 41 | this->fx_counter = 0; |
cb2e6bc6 | 42 | this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set |
4464301d | 43 | this->stepped = 0; |
feb204be | 44 | this->steps_to_move = 0; |
bd0f7508 | 45 | this->is_move_finished = false; |
c6796a29 | 46 | this->last_step_tick_valid= false; |
d6023ce6 | 47 | this->last_step_tick= 0; |
8f91e4e6 | 48 | |
df6a30f2 MM |
49 | steps_per_mm = 1.0F; |
50 | max_rate = 50.0F; | |
9502f9d5 | 51 | minimum_step_rate = default_minimum_actuator_rate; |
df6a30f2 | 52 | |
78d0e16a MM |
53 | last_milestone_steps = 0; |
54 | last_milestone_mm = 0.0F; | |
3494f3d0 | 55 | current_position_steps= 0; |
398e9edc | 56 | signal_step= 0; |
feb204be AW |
57 | } |
58 | ||
728477c4 | 59 | |
93694d6b | 60 | // This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated |
cb2e6bc6 | 61 | // we also here check if the move is finished etc .. |
c6796a29 | 62 | // This is in highest priority interrupt so cannot be pre-empted |
728477c4 JM |
63 | void StepperMotor::step() |
64 | { | |
f7918926 JM |
65 | // ignore if we are still processing the end of a block |
66 | if(this->is_move_finished) return; | |
67 | ||
8aea2a35 | 68 | // output to pins 37t |
728477c4 | 69 | this->step_pin.set( 1 ); |
7b49793d | 70 | |
8aea2a35 | 71 | // move counter back 11t |
cb2e6bc6 | 72 | this->fx_counter -= this->fx_ticks_per_step; |
813727fb | 73 | |
8aea2a35 AW |
74 | // we have moved a step 9t |
75 | this->stepped++; | |
feb204be | 76 | |
58c32991 JM |
77 | // keep track of actuators actual position in steps |
78 | this->current_position_steps += (this->direction ? -1 : 1); | |
79 | ||
398e9edc JM |
80 | // we may need to callback on a specific step, usually used to synchronize deceleration timer |
81 | if(this->signal_step != 0 && this->stepped == this->signal_step) { | |
82 | THEKERNEL->step_ticker->synchronize_acceleration(true); | |
83 | this->signal_step= 0; | |
84 | } | |
85 | ||
93694d6b | 86 | // Is this move finished ? |
728477c4 | 87 | if( this->stepped == this->steps_to_move ) { |
61134a65 | 88 | // Mark it as finished, then StepTicker will call signal_mode_finished() |
93694d6b | 89 | // This is so we don't call that before all the steps have been generated for this tick() |
8aea2a35 | 90 | this->is_move_finished = true; |
cb2e6bc6 | 91 | THEKERNEL->step_ticker->a_move_finished= true; |
c6796a29 | 92 | this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was |
bd0f7508 | 93 | } |
bd0f7508 | 94 | } |
feb204be | 95 | |
37102904 JM |
96 | void StepperMotor::force_finish_move() |
97 | { | |
98 | this->is_move_finished = true; | |
99 | THEKERNEL->step_ticker->a_move_finished= true; | |
100 | this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was | |
101 | this->steps_to_move= this->stepped; | |
102 | } | |
8aea2a35 | 103 | |
6b080aff | 104 | // If the move is finished, the StepTicker will call this ( because we asked it to in tick() ) |
728477c4 JM |
105 | void StepperMotor::signal_move_finished() |
106 | { | |
728477c4 JM |
107 | // work is done ! 8t |
108 | this->moving = false; | |
109 | this->steps_to_move = 0; | |
9502f9d5 | 110 | this->minimum_step_rate = default_minimum_actuator_rate; |
7b49793d | 111 | |
c6796a29 JM |
112 | // signal it to whatever cares |
113 | // in this call a new block may start, new moves set and new speeds | |
728477c4 | 114 | this->end_hook->call(); |
feb204be | 115 | |
728477c4 | 116 | // We only need to do this if we were not instructed to move |
c6796a29 | 117 | if( !this->moving ) { |
728477c4 JM |
118 | this->update_exit_tick(); |
119 | } | |
4464301d | 120 | |
728477c4 | 121 | this->is_move_finished = false; |
feb204be AW |
122 | } |
123 | ||
35e7b158 | 124 | // This is just a way not to check for ( !this->moving || this->fx_ticks_per_step == 0 ) at every tick() |
1fce036c | 125 | void StepperMotor::update_exit_tick() |
728477c4 | 126 | { |
35e7b158 | 127 | if( !this->moving || this->steps_to_move == 0 ) { |
dc3542cf | 128 | // No more ticks will be recieved and no more events from StepTicker |
c9cc5e06 | 129 | THEKERNEL->step_ticker->remove_motor_from_active_list(this); |
728477c4 | 130 | } else { |
dc3542cf | 131 | // we will now get ticks and StepTIcker will send us events |
c9cc5e06 | 132 | THEKERNEL->step_ticker->add_motor_to_active_list(this); |
672298b2 AW |
133 | } |
134 | } | |
135 | ||
feb204be | 136 | // Instruct the StepperMotor to move a certain number of steps |
c6796a29 | 137 | StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed) |
728477c4 | 138 | { |
9c5fa39a MM |
139 | this->dir_pin.set(direction); |
140 | this->direction = direction; | |
feb204be AW |
141 | |
142 | // How many steps we have to move until the move is done | |
143 | this->steps_to_move = steps; | |
144 | ||
145 | // Zero our tool counters | |
feb204be | 146 | this->stepped = 0; |
c6796a29 JM |
147 | this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again |
148 | if(this->last_step_tick_valid) { | |
c6796a29 | 149 | // we set this based on when the last step was, thus compensating for missed ticks |
d6023ce6 JM |
150 | uint32_t ts= THEKERNEL->step_ticker->ticks_since(this->last_step_tick); |
151 | // if an axis stops too soon then we can get a huge number of ticks here which causes problems, so if the number of ticks is too great we ignore them | |
152 | // example of when this happens is when one axis is going very slow an the min 20steps/sec kicks in, the axis will reach its target much sooner leaving a long gap | |
153 | // until the end of the block. | |
488cf43c | 154 | // TODO we may need to set this based on the current step rate, trouble is we don't know what that is yet, we could use the last fx_ticks_per_step as a guide |
f7918926 JM |
155 | if(ts > 5) ts= 5; // limit to 50us catch up around 1-2 steps |
156 | else if(ts > 15) ts= 0; // no way to know what the delay was | |
d6023ce6 | 157 | this->fx_counter= ts*fx_increment; |
c6796a29 JM |
158 | }else{ |
159 | this->fx_counter = 0; // set to zero as there was no step last block | |
160 | } | |
feb204be AW |
161 | |
162 | // Starting now we are moving | |
728477c4 | 163 | if( steps > 0 ) { |
f3b48426 | 164 | if(initial_speed >= 0.0F) set_speed(initial_speed); |
7b49793d | 165 | this->moving = true; |
728477c4 | 166 | } else { |
7b49793d | 167 | this->moving = false; |
83ecfc46 | 168 | } |
7b49793d | 169 | this->update_exit_tick(); |
c6796a29 | 170 | return this; |
feb204be AW |
171 | } |
172 | ||
3494f3d0 | 173 | // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate() |
cb2e6bc6 | 174 | // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters |
3494f3d0 | 175 | // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors |
cb2e6bc6 | 176 | // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14 |
c6796a29 | 177 | StepperMotor* StepperMotor::set_speed( float speed ) |
3494f3d0 | 178 | { |
cb2e6bc6 JM |
179 | if(speed < minimum_step_rate) { |
180 | speed= minimum_step_rate; | |
6f6677fc JM |
181 | } |
182 | ||
cb2e6bc6 JM |
183 | // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on |
184 | // this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec | |
185 | // this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift); | |
186 | // return; | |
187 | // } | |
188 | ||
feb204be AW |
189 | // How many steps we must output per second |
190 | this->steps_per_second = speed; | |
191 | ||
cb2e6bc6 JM |
192 | // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic |
193 | this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed); | |
c6796a29 | 194 | return this; |
feb204be AW |
195 | } |
196 | ||
78d0e16a MM |
197 | void StepperMotor::change_steps_per_mm(float new_steps) |
198 | { | |
199 | steps_per_mm = new_steps; | |
586cc733 | 200 | last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm); |
58c32991 | 201 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
202 | } |
203 | ||
204 | void StepperMotor::change_last_milestone(float new_milestone) | |
205 | { | |
206 | last_milestone_mm = new_milestone; | |
586cc733 | 207 | last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm); |
58c32991 | 208 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
209 | } |
210 | ||
211 | int StepperMotor::steps_to_target(float target) | |
212 | { | |
586cc733 | 213 | int target_steps = lroundf(target * steps_per_mm); |
78d0e16a MM |
214 | return target_steps - last_milestone_steps; |
215 | } |