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 MM |
18 | // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed |
19 | // TODO : Abstract this into Actuator | |
93694d6b | 20 | |
728477c4 JM |
21 | StepperMotor::StepperMotor() |
22 | { | |
23 | init(); | |
24 | } | |
df6a30f2 | 25 | |
728477c4 JM |
26 | StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en) |
27 | { | |
28 | init(); | |
29 | enable(false); | |
30 | set_high_on_debug(en.port_number, en.pin); | |
670fa10b L |
31 | } |
32 | ||
3494f3d0 JM |
33 | StepperMotor::~StepperMotor() |
34 | { | |
35 | delete step_signal_hook; | |
36 | } | |
37 | ||
728477c4 JM |
38 | void StepperMotor::init() |
39 | { | |
feb204be AW |
40 | this->moving = false; |
41 | this->paused = false; | |
42 | this->fx_counter = 0; | |
4464301d | 43 | this->stepped = 0; |
feb204be AW |
44 | this->fx_ticks_per_step = 0; |
45 | this->steps_to_move = 0; | |
bd0f7508 | 46 | this->is_move_finished = false; |
2f7d3dba | 47 | this->signal_step = false; |
7b49793d | 48 | this->step_signal_hook = new Hook(); |
8f91e4e6 | 49 | |
df6a30f2 MM |
50 | steps_per_mm = 1.0F; |
51 | max_rate = 50.0F; | |
9502f9d5 | 52 | minimum_step_rate = default_minimum_actuator_rate; |
df6a30f2 | 53 | |
78d0e16a MM |
54 | last_milestone_steps = 0; |
55 | last_milestone_mm = 0.0F; | |
3494f3d0 | 56 | current_position_steps= 0; |
feb204be AW |
57 | } |
58 | ||
728477c4 | 59 | |
93694d6b AW |
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 |
61 | // we also here check if the move is finished etc ... | |
728477c4 JM |
62 | void StepperMotor::step() |
63 | { | |
4a839bea JM |
64 | if(this->is_move_finished) return; // we can't do anything until the next move has been processed, but we will be able to offset the time by shortening the next step |
65 | ||
8aea2a35 | 66 | // output to pins 37t |
728477c4 | 67 | this->step_pin.set( 1 ); |
c9cc5e06 | 68 | THEKERNEL->step_ticker->reset_step_pins = true; |
7b49793d | 69 | |
8aea2a35 | 70 | // move counter back 11t |
6f6677fc JM |
71 | if(this->fx_counter > this->fx_ticks_per_step) { |
72 | this->fx_counter -= this->fx_ticks_per_step; | |
73 | }else{ | |
66045a90 | 74 | this->fx_counter= 0; // can't make it less than 0 as it is a uint, unless we get interrupted I don't think this case is possible |
6f6677fc | 75 | } |
813727fb | 76 | |
8aea2a35 AW |
77 | // we have moved a step 9t |
78 | this->stepped++; | |
feb204be | 79 | |
8aea2a35 | 80 | // Do we need to signal this step |
728477c4 | 81 | if( this->stepped == this->signal_step_number && this->signal_step ) { |
8aea2a35 AW |
82 | this->step_signal_hook->call(); |
83 | } | |
4464301d | 84 | |
58c32991 JM |
85 | // keep track of actuators actual position in steps |
86 | this->current_position_steps += (this->direction ? -1 : 1); | |
87 | ||
93694d6b | 88 | // Is this move finished ? |
728477c4 | 89 | if( this->stepped == this->steps_to_move ) { |
61134a65 | 90 | // Mark it as finished, then StepTicker will call signal_mode_finished() |
93694d6b | 91 | // This is so we don't call that before all the steps have been generated for this tick() |
8aea2a35 | 92 | this->is_move_finished = true; |
c9cc5e06 | 93 | THEKERNEL->step_ticker->a_move_finished = true; |
4a839bea | 94 | this->fx_counter = 0; // set this to zero here so we don't miss any for next move |
bd0f7508 | 95 | } |
bd0f7508 | 96 | } |
feb204be | 97 | |
8aea2a35 | 98 | |
6b080aff | 99 | // If the move is finished, the StepTicker will call this ( because we asked it to in tick() ) |
728477c4 JM |
100 | void StepperMotor::signal_move_finished() |
101 | { | |
728477c4 JM |
102 | // work is done ! 8t |
103 | this->moving = false; | |
104 | this->steps_to_move = 0; | |
9502f9d5 | 105 | this->minimum_step_rate = default_minimum_actuator_rate; |
7b49793d | 106 | |
728477c4 JM |
107 | // signal it to whatever cares 41t 411t |
108 | this->end_hook->call(); | |
feb204be | 109 | |
728477c4 JM |
110 | // We only need to do this if we were not instructed to move |
111 | if( this->moving == false ) { | |
112 | this->update_exit_tick(); | |
113 | } | |
4464301d | 114 | |
728477c4 | 115 | this->is_move_finished = false; |
feb204be AW |
116 | } |
117 | ||
672298b2 | 118 | // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick() |
728477c4 JM |
119 | inline void StepperMotor::update_exit_tick() |
120 | { | |
121 | if( !this->moving || this->paused || this->steps_to_move == 0 ) { | |
7b49793d | 122 | // We must exit tick() after setting the pins, no bresenham is done |
c9cc5e06 | 123 | THEKERNEL->step_ticker->remove_motor_from_active_list(this); |
728477c4 | 124 | } else { |
796c9f32 | 125 | // We must do the bresenham in tick() |
bd0f7508 | 126 | // We have to do this or there could be a bug where the removal still happens when it doesn't need to |
c9cc5e06 | 127 | THEKERNEL->step_ticker->add_motor_to_active_list(this); |
672298b2 AW |
128 | } |
129 | } | |
130 | ||
feb204be | 131 | // Instruct the StepperMotor to move a certain number of steps |
6f6677fc | 132 | void StepperMotor::move( bool direction, unsigned int steps, float initial_speed) |
728477c4 | 133 | { |
9c5fa39a MM |
134 | this->dir_pin.set(direction); |
135 | this->direction = direction; | |
feb204be AW |
136 | |
137 | // How many steps we have to move until the move is done | |
138 | this->steps_to_move = steps; | |
139 | ||
140 | // Zero our tool counters | |
feb204be AW |
141 | this->stepped = 0; |
142 | ||
2f7d3dba AW |
143 | // Do not signal steps until we get instructed to |
144 | this->signal_step = false; | |
145 | ||
feb204be | 146 | // Starting now we are moving |
728477c4 | 147 | if( steps > 0 ) { |
f3b48426 | 148 | if(initial_speed >= 0.0F) set_speed(initial_speed); |
7b49793d | 149 | this->moving = true; |
728477c4 | 150 | } else { |
7b49793d | 151 | this->moving = false; |
83ecfc46 | 152 | } |
7b49793d | 153 | this->update_exit_tick(); |
feb204be AW |
154 | } |
155 | ||
3494f3d0 JM |
156 | // this is called to set the step rate based on this blocks rate, we use this instead of set_speed for coordinated moves |
157 | // so that we can floor to a minimum speed which is proportional for all axis. the minimum step rate is set at the start | |
158 | // of each block based on the slowest axis of all coordinated axis. | |
159 | // the rate passed in is the requested rate, it is scaled for this motor based on steps_to_move and block_steps_event_count | |
160 | void StepperMotor::set_step_rate(float requested_rate, uint32_t block_steps_event_count) | |
728477c4 | 161 | { |
3494f3d0 | 162 | float rate= requested_rate * ((float)steps_to_move / (float)block_steps_event_count); |
9e089978 JM |
163 | if(rate < minimum_step_rate) { |
164 | rate= minimum_step_rate; | |
165 | } | |
3494f3d0 JM |
166 | set_speed(rate); |
167 | } | |
6f6677fc | 168 | |
3494f3d0 JM |
169 | // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate() |
170 | // we need to make sure that we have a minimum speed here and that it fits the 64bit fixed point fx counters | |
171 | // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors | |
172 | // fx_ticks_per_step is what actually sets the step rate, it is fixed point 32.32 | |
173 | void StepperMotor::set_speed( float speed ) | |
174 | { | |
175 | if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on | |
176 | this->fx_ticks_per_step= 0xFFFFF00000000000ULL; // that is 4,294,963,200 10us ticks which is ~11.9 hours for 1 step | |
177 | this->steps_per_second = THEKERNEL->step_ticker->frequency / (this->fx_ticks_per_step>>fx_shift); | |
178 | return; | |
6f6677fc JM |
179 | } |
180 | ||
feb204be AW |
181 | // How many steps we must output per second |
182 | this->steps_per_second = speed; | |
183 | ||
3eadcfee | 184 | // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64 |
3494f3d0 JM |
185 | // we need to use double here to match the 64bit resolution of the ticker |
186 | double ticks_per_step = (double)THEKERNEL->step_ticker->frequency / speed; | |
187 | if(ticks_per_step > 0xFFFFF000UL) { // maximum we can really do and allow a few overflow steps | |
188 | ticks_per_step= 0xFFFFF000UL; | |
189 | this->steps_per_second = THEKERNEL->step_ticker->frequency / ticks_per_step; | |
190 | } | |
191 | double double_fx_ticks_per_step = fx_increment * ticks_per_step; | |
755beef1 | 192 | |
6f6677fc | 193 | // set the new speed |
3eadcfee | 194 | this->fx_ticks_per_step = floor(double_fx_ticks_per_step); |
feb204be AW |
195 | } |
196 | ||
93694d6b | 197 | // Pause this stepper motor |
728477c4 JM |
198 | void StepperMotor::pause() |
199 | { | |
83ecfc46 AW |
200 | this->paused = true; |
201 | this->update_exit_tick(); | |
202 | } | |
feb204be | 203 | |
93694d6b | 204 | // Unpause this stepper motor |
728477c4 JM |
205 | void StepperMotor::unpause() |
206 | { | |
83ecfc46 AW |
207 | this->paused = false; |
208 | this->update_exit_tick(); | |
209 | } | |
feb204be AW |
210 | |
211 | ||
78d0e16a MM |
212 | void StepperMotor::change_steps_per_mm(float new_steps) |
213 | { | |
214 | steps_per_mm = new_steps; | |
215 | last_milestone_steps = lround(last_milestone_mm * steps_per_mm); | |
58c32991 | 216 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
217 | } |
218 | ||
219 | void StepperMotor::change_last_milestone(float new_milestone) | |
220 | { | |
221 | last_milestone_mm = new_milestone; | |
222 | last_milestone_steps = lround(last_milestone_mm * steps_per_mm); | |
58c32991 | 223 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
224 | } |
225 | ||
226 | int StepperMotor::steps_to_target(float target) | |
227 | { | |
338beb48 | 228 | int target_steps = lround(target * steps_per_mm); |
78d0e16a MM |
229 | return target_steps - last_milestone_steps; |
230 | } |