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