Commit | Line | Data |
---|---|---|
7b49793d | 1 | /* |
5886a464 | 2 | This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl) |
4cff3ded AW |
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/>. |
4cff3ded AW |
6 | */ |
7 | ||
5673fe39 MM |
8 | #include "Stepper.h" |
9 | ||
4cff3ded AW |
10 | #include "libs/Module.h" |
11 | #include "libs/Kernel.h" | |
4cff3ded | 12 | #include "Planner.h" |
3fceb8eb | 13 | #include "Conveyor.h" |
5673fe39 | 14 | #include "StepperMotor.h" |
61134a65 JM |
15 | #include "Robot.h" |
16 | #include "checksumm.h" | |
17 | #include "SlowTicker.h" | |
18 | #include "Config.h" | |
8d54c34c | 19 | #include "ConfigValue.h" |
66383b80 | 20 | #include "Gcode.h" |
558e170c | 21 | #include "Block.h" |
a157d099 | 22 | #include "StepTicker.h" |
5673fe39 | 23 | |
4cff3ded AW |
24 | #include <vector> |
25 | using namespace std; | |
5673fe39 | 26 | |
4cff3ded | 27 | #include "libs/nuts_bolts.h" |
2f7d3dba | 28 | #include "libs/Hook.h" |
5673fe39 | 29 | |
db453125 AW |
30 | #include <mri.h> |
31 | ||
edac9072 | 32 | // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves |
d337942a | 33 | // TODO: This does accel, accel should be in StepperMotor |
edac9072 | 34 | |
b375ba1d JM |
35 | Stepper::Stepper() |
36 | { | |
4cff3ded | 37 | this->current_block = NULL; |
1013f6a3 | 38 | this->force_speed_update = false; |
728477c4 | 39 | this->halted= false; |
4cff3ded AW |
40 | } |
41 | ||
42 | //Called when the module has just been loaded | |
b375ba1d JM |
43 | void Stepper::on_module_loaded() |
44 | { | |
3a4fa0c1 AW |
45 | this->register_for_event(ON_BLOCK_BEGIN); |
46 | this->register_for_event(ON_BLOCK_END); | |
6b833f7d | 47 | this->register_for_event(ON_GCODE_EXECUTE); |
31e600f6 | 48 | this->register_for_event(ON_GCODE_RECEIVED); |
3d1a4519 | 49 | this->register_for_event(ON_HALT); |
7b49793d | 50 | |
4cff3ded | 51 | // Get onfiguration |
7b49793d | 52 | this->on_config_reload(this); |
4cff3ded | 53 | |
ded56b35 | 54 | // Acceleration ticker |
a157d099 | 55 | THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); }); |
4cff3ded | 56 | |
feb204be | 57 | // Attach to the end_of_move stepper event |
314ab8f7 MM |
58 | THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move ); |
59 | THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move ); | |
60 | THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move ); | |
4cff3ded AW |
61 | } |
62 | ||
2bb8b390 | 63 | // Get configuration from the config file |
b375ba1d JM |
64 | void Stepper::on_config_reload(void *argument) |
65 | { | |
5a884140 MM |
66 | // Steppers start off by default |
67 | this->turn_enable_pins_off(); | |
da24d6ae AW |
68 | } |
69 | ||
b375ba1d | 70 | void Stepper::on_halt(void *argument) |
3d1a4519 | 71 | { |
728477c4 JM |
72 | if(argument == nullptr) { |
73 | this->turn_enable_pins_off(); | |
74 | this->halted= true; | |
75 | }else{ | |
76 | this->halted= false; | |
77 | } | |
3d1a4519 JM |
78 | } |
79 | ||
b375ba1d JM |
80 | void Stepper::on_gcode_received(void *argument) |
81 | { | |
82 | Gcode *gcode = static_cast<Gcode *>(argument); | |
31e600f6 JM |
83 | // Attach gcodes to the last block for on_gcode_execute |
84 | if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) { | |
e0ee24ed | 85 | THEKERNEL->conveyor->append_gcode(gcode); |
1cf31736 | 86 | } |
31e600f6 JM |
87 | } |
88 | ||
edac9072 | 89 | // React to enable/disable gcodes |
b375ba1d JM |
90 | void Stepper::on_gcode_execute(void *argument) |
91 | { | |
92 | Gcode *gcode = static_cast<Gcode *>(argument); | |
6b833f7d | 93 | |
b375ba1d JM |
94 | if( gcode->has_m) { |
95 | if( gcode->m == 17 ) { | |
7b49793d | 96 | this->turn_enable_pins_on(); |
831bade1 | 97 | } |
b375ba1d | 98 | if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) { |
7b49793d | 99 | this->turn_enable_pins_off(); |
6b833f7d MM |
100 | } |
101 | } | |
102 | } | |
103 | ||
edac9072 | 104 | // Enable steppers |
b375ba1d JM |
105 | void Stepper::turn_enable_pins_on() |
106 | { | |
107 | for (StepperMotor *m : THEKERNEL->robot->actuators) | |
9c5fa39a | 108 | m->enable(true); |
831bade1 | 109 | this->enable_pins_status = true; |
0bfaf040 | 110 | THEKERNEL->call_event(ON_ENABLE, (void*)1); |
831bade1 AW |
111 | } |
112 | ||
edac9072 | 113 | // Disable steppers |
b375ba1d JM |
114 | void Stepper::turn_enable_pins_off() |
115 | { | |
116 | for (StepperMotor *m : THEKERNEL->robot->actuators) | |
9c5fa39a | 117 | m->enable(false); |
831bade1 | 118 | this->enable_pins_status = false; |
0bfaf040 | 119 | THEKERNEL->call_event(ON_ENABLE, nullptr); |
831bade1 AW |
120 | } |
121 | ||
7b49793d | 122 | // A new block is popped from the queue |
b375ba1d JM |
123 | void Stepper::on_block_begin(void *argument) |
124 | { | |
125 | Block *block = static_cast<Block *>(argument); | |
3a4fa0c1 | 126 | |
5090327a JM |
127 | // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc) |
128 | if(block->millimeters > 0.0F && (block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0) ) { | |
4464301d | 129 | block->take(); |
5090327a | 130 | |
b375ba1d | 131 | } else { |
5090327a JM |
132 | // none of the steppers move this block so make sure they know that |
133 | for(auto a : THEKERNEL->robot->actuators) { | |
134 | a->set_moved_last_block(false); | |
135 | } | |
4464301d AW |
136 | return; |
137 | } | |
813727fb | 138 | |
7b49793d | 139 | // We can't move with the enable pins off |
b375ba1d | 140 | if( this->enable_pins_status == false ) { |
831bade1 AW |
141 | this->turn_enable_pins_on(); |
142 | } | |
143 | ||
7b49793d | 144 | // Setup : instruct stepper motors to move |
c6796a29 JM |
145 | // Find the stepper with the more steps, it's the one the speed calculations will want to follow |
146 | this->main_stepper= nullptr; | |
b375ba1d | 147 | if( block->steps[ALPHA_STEPPER] > 0 ) { |
c6796a29 JM |
148 | THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER])->set_moved_last_block(true); |
149 | this->main_stepper = THEKERNEL->robot->alpha_stepper_motor; | |
150 | }else{ | |
151 | THEKERNEL->robot->alpha_stepper_motor->set_moved_last_block(false); | |
b375ba1d | 152 | } |
c6796a29 | 153 | |
b375ba1d | 154 | if( block->steps[BETA_STEPPER ] > 0 ) { |
c6796a29 JM |
155 | THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ])->set_moved_last_block(true); |
156 | if(this->main_stepper == nullptr || THEKERNEL->robot->beta_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move()) | |
157 | this->main_stepper = THEKERNEL->robot->beta_stepper_motor; | |
158 | }else{ | |
159 | THEKERNEL->robot->beta_stepper_motor->set_moved_last_block(false); | |
b375ba1d | 160 | } |
d6023ce6 | 161 | |
b375ba1d | 162 | if( block->steps[GAMMA_STEPPER] > 0 ) { |
c6796a29 JM |
163 | THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER])->set_moved_last_block(true); |
164 | if(this->main_stepper == nullptr || THEKERNEL->robot->gamma_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move()) | |
165 | this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; | |
166 | }else{ | |
167 | THEKERNEL->robot->gamma_stepper_motor->set_moved_last_block(false); | |
b375ba1d | 168 | } |
3a4fa0c1 | 169 | |
1a2d88eb | 170 | this->current_block = block; |
feb204be | 171 | |
7b49793d | 172 | // Setup acceleration for this block |
1a2d88eb | 173 | this->trapezoid_generator_reset(); |
ded56b35 | 174 | |
edac9072 | 175 | // Set the initial speed for this move |
a157d099 | 176 | this->trapezoid_generator_tick(); |
13256955 JM |
177 | |
178 | // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block | |
398e9edc JM |
179 | THEKERNEL->step_ticker->synchronize_acceleration(false); |
180 | ||
181 | // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step | |
182 | if( block->decelerate_after > 0 && block->decelerate_after+1 < this->main_stepper->steps_to_move ) { | |
183 | this->main_stepper->signal_step= block->decelerate_after+1; // we make it +1 as deceleration does not start until steps > decelerate_after | |
184 | } | |
3a4fa0c1 AW |
185 | } |
186 | ||
187 | // Current block is discarded | |
b375ba1d JM |
188 | void Stepper::on_block_end(void *argument) |
189 | { | |
813727fb | 190 | this->current_block = NULL; //stfu ! |
3a4fa0c1 | 191 | } |
da24d6ae | 192 | |
feb204be | 193 | // When a stepper motor has finished it's assigned movement |
b375ba1d JM |
194 | uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy) |
195 | { | |
feb204be | 196 | // We care only if none is still moving |
b375ba1d JM |
197 | if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ) { |
198 | return 0; | |
199 | } | |
7b49793d | 200 | |
feb204be | 201 | // This block is finished, release it |
b375ba1d | 202 | if( this->current_block != NULL ) { |
7b49793d | 203 | this->current_block->release(); |
4cff3ded | 204 | } |
7b49793d | 205 | |
9e672116 | 206 | return 0; |
4cff3ded AW |
207 | } |
208 | ||
83ecfc46 | 209 | |
4cff3ded AW |
210 | // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event |
211 | // interrupt. It can be assumed that the trapezoid-generator-parameters and the | |
212 | // current_block stays untouched by outside handlers for the duration of this function call. | |
4dfd2dce | 213 | // NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv |
a157d099 | 214 | void Stepper::trapezoid_generator_tick(void) |
b375ba1d | 215 | { |
edac9072 | 216 | // Do not do the accel math for nothing |
35e7b158 | 217 | if(this->current_block && this->main_stepper->moving ) { |
1cf31736 JM |
218 | |
219 | // Store this here because we use it a lot down there | |
813727fb | 220 | uint32_t current_steps_completed = this->main_stepper->stepped; |
8febdc71 | 221 | float last_rate= trapezoid_adjusted_rate; |
7b49793d | 222 | |
b375ba1d JM |
223 | if( this->force_speed_update ) { |
224 | // Do not accel, just set the value | |
225 | this->force_speed_update = false; | |
8febdc71 | 226 | last_rate= -1; |
b375ba1d JM |
227 | |
228 | } else if(THEKERNEL->conveyor->is_flushing()) { | |
229 | // if we are flushing the queue, decelerate to 0 then finish this block | |
230 | if (trapezoid_adjusted_rate > current_block->rate_delta * 1.5F) { | |
231 | trapezoid_adjusted_rate -= current_block->rate_delta; | |
232 | ||
233 | } else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) { | |
c1c802b1 JM |
234 | for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors |
235 | if (current_block) current_block->release(); | |
236 | THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped | |
a157d099 | 237 | return; |
c1c802b1 | 238 | |
b375ba1d JM |
239 | } else { |
240 | trapezoid_adjusted_rate = current_block->rate_delta * 0.5F; | |
241 | } | |
242 | ||
d6ba35b8 | 243 | } else if(current_steps_completed <= this->current_block->accelerate_until) { |
b375ba1d | 244 | // If we are accelerating |
1cf31736 | 245 | // Increase speed |
edac9072 | 246 | this->trapezoid_adjusted_rate += this->current_block->rate_delta; |
b375ba1d JM |
247 | if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) { |
248 | this->trapezoid_adjusted_rate = this->current_block->nominal_rate; | |
249 | } | |
250 | ||
f3b48426 | 251 | } else if (current_steps_completed > this->current_block->decelerate_after) { |
b375ba1d JM |
252 | // If we are decelerating |
253 | // Reduce speed | |
254 | // NOTE: We will only reduce speed if the result will be > 0. This catches small | |
255 | // rounding errors that might leave steps hanging after the last trapezoid tick. | |
256 | if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) { | |
257 | this->trapezoid_adjusted_rate -= this->current_block->rate_delta; | |
258 | } else { | |
259 | this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F; | |
260 | } | |
261 | if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) { | |
262 | this->trapezoid_adjusted_rate = this->current_block->final_rate; | |
263 | } | |
264 | ||
265 | } else if (trapezoid_adjusted_rate != current_block->nominal_rate) { | |
266 | // If we are cruising | |
267 | // Make sure we cruise at exactly nominal rate | |
268 | this->trapezoid_adjusted_rate = this->current_block->nominal_rate; | |
269 | } | |
270 | ||
8febdc71 JM |
271 | if(last_rate != trapezoid_adjusted_rate) { |
272 | // don't call this if speed did not change | |
273 | this->set_step_events_per_second(this->trapezoid_adjusted_rate); | |
274 | } | |
ded56b35 | 275 | } |
4cff3ded AW |
276 | } |
277 | ||
4cff3ded AW |
278 | // Initializes the trapezoid generator from the current block. Called whenever a new |
279 | // block begins. | |
b375ba1d JM |
280 | inline void Stepper::trapezoid_generator_reset() |
281 | { | |
4cff3ded | 282 | this->trapezoid_adjusted_rate = this->current_block->initial_rate; |
1013f6a3 | 283 | this->force_speed_update = true; |
4cff3ded AW |
284 | } |
285 | ||
feb204be | 286 | // Update the speed for all steppers |
da947c62 MM |
287 | void Stepper::set_step_events_per_second( float steps_per_second ) |
288 | { | |
cb2e6bc6 JM |
289 | float isps= steps_per_second / this->current_block->steps_event_count; |
290 | ||
feb204be | 291 | // Instruct the stepper motors |
b375ba1d | 292 | if( THEKERNEL->robot->alpha_stepper_motor->moving ) { |
cb2e6bc6 | 293 | THEKERNEL->robot->alpha_stepper_motor->set_speed(isps * this->current_block->steps[ALPHA_STEPPER]); |
b375ba1d JM |
294 | } |
295 | if( THEKERNEL->robot->beta_stepper_motor->moving ) { | |
cb2e6bc6 | 296 | THEKERNEL->robot->beta_stepper_motor->set_speed(isps * this->current_block->steps[BETA_STEPPER]); |
b375ba1d JM |
297 | } |
298 | if( THEKERNEL->robot->gamma_stepper_motor->moving ) { | |
cb2e6bc6 | 299 | THEKERNEL->robot->gamma_stepper_motor->set_speed(isps * this->current_block->steps[GAMMA_STEPPER]); |
b375ba1d | 300 | } |
4cff3ded | 301 | |
edac9072 | 302 | // Other modules might want to know the speed changed |
314ab8f7 | 303 | THEKERNEL->call_event(ON_SPEED_CHANGE, this); |
4cff3ded AW |
304 | } |
305 | ||
2f7d3dba | 306 |