HCoop / clinton / Smoothieware.git / blame_incremental
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (non-incremental) | history | HEAD
remove debug of block
[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp
... / ...
CommitLineData
1/*
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)
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.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
6*/
7
8#include "libs/Module.h"
9#include "libs/Kernel.h"
10
11#include "Robot.h"
12#include "Planner.h"
13#include "Conveyor.h"
14#include "Pin.h"
15#include "StepperMotor.h"
16#include "Gcode.h"
17#include "PublicDataRequest.h"
18#include "PublicData.h"
19#include "arm_solutions/BaseSolution.h"
20#include "arm_solutions/CartesianSolution.h"
21#include "arm_solutions/RotatableCartesianSolution.h"
22#include "arm_solutions/LinearDeltaSolution.h"
23#include "arm_solutions/RotaryDeltaSolution.h"
24#include "arm_solutions/HBotSolution.h"
25#include "arm_solutions/CoreXZSolution.h"
26#include "arm_solutions/MorganSCARASolution.h"
27#include "StepTicker.h"
28#include "checksumm.h"
29#include "utils.h"
30#include "ConfigValue.h"
31#include "libs/StreamOutput.h"
32#include "StreamOutputPool.h"
33#include "ExtruderPublicAccess.h"
34#include "GcodeDispatch.h"
35#include "ActuatorCoordinates.h"
36
37#include "mbed.h" // for us_ticker_read()
38#include "mri.h"
39
40#include <fastmath.h>
41#include <string>
42#include <algorithm>
43using std::string;
44
45#define default_seek_rate_checksum CHECKSUM("default_seek_rate")
46#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
47#define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
48#define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
49#define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
50#define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
51#define arc_correction_checksum CHECKSUM("arc_correction")
52#define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
53#define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
54#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
55#define segment_z_moves_checksum CHECKSUM("segment_z_moves")
56#define save_g92_checksum CHECKSUM("save_g92")
57
58// arm solutions
59#define arm_solution_checksum CHECKSUM("arm_solution")
60#define cartesian_checksum CHECKSUM("cartesian")
61#define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
62#define rostock_checksum CHECKSUM("rostock")
63#define linear_delta_checksum CHECKSUM("linear_delta")
64#define rotary_delta_checksum CHECKSUM("rotary_delta")
65#define delta_checksum CHECKSUM("delta")
66#define hbot_checksum CHECKSUM("hbot")
67#define corexy_checksum CHECKSUM("corexy")
68#define corexz_checksum CHECKSUM("corexz")
69#define kossel_checksum CHECKSUM("kossel")
70#define morgan_checksum CHECKSUM("morgan")
71
72// new-style actuator stuff
73#define actuator_checksum CHEKCSUM("actuator")
74
75#define step_pin_checksum CHECKSUM("step_pin")
76#define dir_pin_checksum CHEKCSUM("dir_pin")
77#define en_pin_checksum CHECKSUM("en_pin")
78
79#define steps_per_mm_checksum CHECKSUM("steps_per_mm")
80#define max_rate_checksum CHECKSUM("max_rate")
81#define acceleration_checksum CHECKSUM("acceleration")
82#define z_acceleration_checksum CHECKSUM("z_acceleration")
83
84#define alpha_checksum CHECKSUM("alpha")
85#define beta_checksum CHECKSUM("beta")
86#define gamma_checksum CHECKSUM("gamma")
87
88#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
89#define PI 3.14159265358979323846F // force to be float, do not use M_PI
90
91// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
92// It takes care of cutting arcs into segments, same thing for line that are too long
93
94Robot::Robot()
95{
96 this->inch_mode = false;
97 this->absolute_mode = true;
98 this->e_absolute_mode = true;
99 this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
100 memset(this->last_milestone, 0, sizeof last_milestone);
101 memset(this->last_machine_position, 0, sizeof last_machine_position);
102 this->arm_solution = NULL;
103 seconds_per_minute = 60.0F;
104 this->clearToolOffset();
105 this->compensationTransform = nullptr;
106 this->wcs_offsets.fill(wcs_t(0.0F, 0.0F, 0.0F));
107 this->g92_offset = wcs_t(0.0F, 0.0F, 0.0F);
108 this->next_command_is_MCS = false;
109 this->disable_segmentation= false;
110 this->n_motors= 0;
111 this->actuators.fill(nullptr);
112}
113
114//Called when the module has just been loaded
115void Robot::on_module_loaded()
116{
117 this->register_for_event(ON_GCODE_RECEIVED);
118
119 // Configuration
120 this->load_config();
121}
122
123#define ACTUATOR_CHECKSUMS(X) { \
124 CHECKSUM(X "_step_pin"), \
125 CHECKSUM(X "_dir_pin"), \
126 CHECKSUM(X "_en_pin"), \
127 CHECKSUM(X "_steps_per_mm"), \
128 CHECKSUM(X "_max_rate"), \
129 CHECKSUM(X "_acceleration") \
130}
131
132void Robot::load_config()
133{
134 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
135 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
136 // To make adding those solution easier, they have their own, separate object.
137 // Here we read the config to find out which arm solution to use
138 if (this->arm_solution) delete this->arm_solution;
139 int solution_checksum = get_checksum(THEKERNEL->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
140 // Note checksums are not const expressions when in debug mode, so don't use switch
141 if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
142 this->arm_solution = new HBotSolution(THEKERNEL->config);
143
144 } else if(solution_checksum == corexz_checksum) {
145 this->arm_solution = new CoreXZSolution(THEKERNEL->config);
146
147 } else if(solution_checksum == rostock_checksum || solution_checksum == kossel_checksum || solution_checksum == delta_checksum || solution_checksum == linear_delta_checksum) {
148 this->arm_solution = new LinearDeltaSolution(THEKERNEL->config);
149
150 } else if(solution_checksum == rotatable_cartesian_checksum) {
151 this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
152
153 } else if(solution_checksum == rotary_delta_checksum) {
154 this->arm_solution = new RotaryDeltaSolution(THEKERNEL->config);
155
156 } else if(solution_checksum == morgan_checksum) {
157 this->arm_solution = new MorganSCARASolution(THEKERNEL->config);
158
159 } else if(solution_checksum == cartesian_checksum) {
160 this->arm_solution = new CartesianSolution(THEKERNEL->config);
161
162 } else {
163 this->arm_solution = new CartesianSolution(THEKERNEL->config);
164 }
165
166 this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default( 100.0F)->as_number();
167 this->seek_rate = THEKERNEL->config->value(default_seek_rate_checksum )->by_default( 100.0F)->as_number();
168 this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default( 0.0F)->as_number();
169 this->delta_segments_per_second = THEKERNEL->config->value(delta_segments_per_second_checksum )->by_default(0.0f )->as_number();
170 this->mm_per_arc_segment = THEKERNEL->config->value(mm_per_arc_segment_checksum )->by_default( 0.0f)->as_number();
171 this->mm_max_arc_error = THEKERNEL->config->value(mm_max_arc_error_checksum )->by_default( 0.01f)->as_number();
172 this->arc_correction = THEKERNEL->config->value(arc_correction_checksum )->by_default( 5 )->as_number();
173
174 // in mm/sec but specified in config as mm/min
175 this->max_speeds[X_AXIS] = THEKERNEL->config->value(x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
176 this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
177 this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
178
179 this->segment_z_moves = THEKERNEL->config->value(segment_z_moves_checksum )->by_default(true)->as_bool();
180 this->save_g92 = THEKERNEL->config->value(save_g92_checksum )->by_default(false)->as_bool();
181
182 // Make our Primary XYZ StepperMotors
183 uint16_t const checksums[][6] = {
184 ACTUATOR_CHECKSUMS("alpha"), // X
185 ACTUATOR_CHECKSUMS("beta"), // Y
186 ACTUATOR_CHECKSUMS("gamma"), // Z
187 };
188
189 // default acceleration setting, can be overriden with newer per axis settings
190 this->default_acceleration= THEKERNEL->config->value(acceleration_checksum)->by_default(100.0F )->as_number(); // Acceleration is in mm/s^2
191
192 // make each motor
193 for (size_t a = X_AXIS; a <= Z_AXIS; a++) {
194 Pin pins[3]; //step, dir, enable
195 for (size_t i = 0; i < 3; i++) {
196 pins[i].from_string(THEKERNEL->config->value(checksums[a][i])->by_default("nc")->as_string())->as_output();
197 }
198 StepperMotor *sm = new StepperMotor(pins[0], pins[1], pins[2]);
199 // register this motor (NB This must be 0,1,2) of the actuators array
200 uint8_t n= register_motor(sm);
201 if(n != a) {
202 // this is a fatal error
203 THEKERNEL->streams->printf("FATAL: motor %d does not match index %d\n", n, a);
204 __debugbreak();
205 }
206
207 actuators[a]->change_steps_per_mm(THEKERNEL->config->value(checksums[a][3])->by_default(a == 2 ? 2560.0F : 80.0F)->as_number());
208 actuators[a]->set_max_rate(THEKERNEL->config->value(checksums[a][4])->by_default(30000.0F)->as_number()/60.0F); // it is in mm/min and converted to mm/sec
209 actuators[a]->set_acceleration(THEKERNEL->config->value(checksums[a][5])->by_default(NAN)->as_number()); // mm/secs²
210 }
211
212 check_max_actuator_speeds(); // check the configs are sane
213
214 // if we have not specified a z acceleration see if the legacy config was set
215 if(isnan(actuators[Z_AXIS]->get_acceleration())) {
216 float acc= THEKERNEL->config->value(z_acceleration_checksum)->by_default(NAN)->as_number(); // disabled by default
217 if(!isnan(acc)) {
218 actuators[Z_AXIS]->set_acceleration(acc);
219 }
220 }
221
222 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
223 // so the first move can be correct if homing is not performed
224 ActuatorCoordinates actuator_pos;
225 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
226 for (size_t i = 0; i < n_motors; i++)
227 actuators[i]->change_last_milestone(actuator_pos[i]);
228
229 //this->clearToolOffset();
230}
231
232uint8_t Robot::register_motor(StepperMotor *motor)
233{
234 // register this motor with the step ticker
235 THEKERNEL->step_ticker->register_motor(motor);
236 if(n_motors >= k_max_actuators) {
237 // this is a fatal error
238 THEKERNEL->streams->printf("FATAL: too many motors, increase k_max_actuators\n");
239 __debugbreak();
240 }
241 actuators[n_motors++]= motor;
242 return n_motors-1;
243}
244
245void Robot::push_state()
246{
247 bool am = this->absolute_mode;
248 bool em = this->e_absolute_mode;
249 bool im = this->inch_mode;
250 saved_state_t s(this->feed_rate, this->seek_rate, am, em, im, current_wcs);
251 state_stack.push(s);
252}
253
254void Robot::pop_state()
255{
256 if(!state_stack.empty()) {
257 auto s = state_stack.top();
258 state_stack.pop();
259 this->feed_rate = std::get<0>(s);
260 this->seek_rate = std::get<1>(s);
261 this->absolute_mode = std::get<2>(s);
262 this->e_absolute_mode = std::get<3>(s);
263 this->inch_mode = std::get<4>(s);
264 this->current_wcs = std::get<5>(s);
265 }
266}
267
268std::vector<Robot::wcs_t> Robot::get_wcs_state() const
269{
270 std::vector<wcs_t> v;
271 v.push_back(wcs_t(current_wcs, MAX_WCS, 0));
272 for(auto& i : wcs_offsets) {
273 v.push_back(i);
274 }
275 v.push_back(g92_offset);
276 v.push_back(tool_offset);
277 return v;
278}
279
280int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
281{
282 // M114.1 is a new way to do this (similar to how GRBL does it).
283 // it returns the realtime position based on the current step position of the actuators.
284 // this does require a FK to get a machine position from the actuator position
285 // and then invert all the transforms to get a workspace position from machine position
286 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
287 int n = 0;
288 if(subcode == 0) { // M114 print WCS
289 wcs_t pos= mcs2wcs(last_milestone);
290 n = snprintf(buf, bufsize, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
291
292 } else if(subcode == 4) { // M114.4 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
293 n = snprintf(buf, bufsize, "LMS: X:%1.4f Y:%1.4f Z:%1.4f", last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
294
295 } else if(subcode == 5) { // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
296 n = snprintf(buf, bufsize, "LMP: X:%1.4f Y:%1.4f Z:%1.4f", last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
297
298 } else {
299 // get real time positions
300 // current actuator position in mm
301 ActuatorCoordinates current_position{
302 actuators[X_AXIS]->get_current_position(),
303 actuators[Y_AXIS]->get_current_position(),
304 actuators[Z_AXIS]->get_current_position()
305 };
306
307 // get machine position from the actuator position using FK
308 float mpos[3];
309 arm_solution->actuator_to_cartesian(current_position, mpos);
310
311 if(subcode == 1) { // M114.1 print realtime WCS
312 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
313 wcs_t pos= mcs2wcs(mpos);
314 n = snprintf(buf, bufsize, "WPOS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
315
316 } else if(subcode == 2) { // M114.2 print realtime Machine coordinate system
317 n = snprintf(buf, bufsize, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
318
319 } else if(subcode == 3) { // M114.3 print realtime actuator position
320 n = snprintf(buf, bufsize, "APOS: A:%1.4f B:%1.4f C:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
321 }
322 }
323 return n;
324}
325
326// converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
327Robot::wcs_t Robot::mcs2wcs(const Robot::wcs_t& pos) const
328{
329 return std::make_tuple(
330 std::get<X_AXIS>(pos) - std::get<X_AXIS>(wcs_offsets[current_wcs]) + std::get<X_AXIS>(g92_offset) - std::get<X_AXIS>(tool_offset),
331 std::get<Y_AXIS>(pos) - std::get<Y_AXIS>(wcs_offsets[current_wcs]) + std::get<Y_AXIS>(g92_offset) - std::get<Y_AXIS>(tool_offset),
332 std::get<Z_AXIS>(pos) - std::get<Z_AXIS>(wcs_offsets[current_wcs]) + std::get<Z_AXIS>(g92_offset) - std::get<Z_AXIS>(tool_offset)
333 );
334}
335
336// this does a sanity check that actuator speeds do not exceed steps rate capability
337// we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
338void Robot::check_max_actuator_speeds()
339{
340 for (size_t i = 0; i < n_motors; i++) {
341 float step_freq = actuators[i]->get_max_rate() * actuators[i]->get_steps_per_mm();
342 if (step_freq > THEKERNEL->base_stepping_frequency) {
343 actuators[i]->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / actuators[i]->get_steps_per_mm()));
344 THEKERNEL->streams->printf("WARNING: actuator %d rate exceeds base_stepping_frequency * ..._steps_per_mm: %f, setting to %f\n", i, step_freq, actuators[i]->max_rate);
345 }
346 }
347}
348
349//A GCode has been received
350//See if the current Gcode line has some orders for us
351void Robot::on_gcode_received(void *argument)
352{
353 Gcode *gcode = static_cast<Gcode *>(argument);
354
355 enum MOTION_MODE_T motion_mode= NONE;
356
357 if( gcode->has_g) {
358 switch( gcode->g ) {
359 case 0: motion_mode = SEEK; break;
360 case 1: motion_mode = LINEAR; break;
361 case 2: motion_mode = CW_ARC; break;
362 case 3: motion_mode = CCW_ARC; break;
363 case 4: { // G4 pause
364 uint32_t delay_ms = 0;
365 if (gcode->has_letter('P')) {
366 delay_ms = gcode->get_int('P');
367 }
368 if (gcode->has_letter('S')) {
369 delay_ms += gcode->get_int('S') * 1000;
370 }
371 if (delay_ms > 0) {
372 // drain queue
373 THEKERNEL->conveyor->wait_for_empty_queue();
374 // wait for specified time
375 uint32_t start = us_ticker_read(); // mbed call
376 while ((us_ticker_read() - start) < delay_ms * 1000) {
377 THEKERNEL->call_event(ON_IDLE, this);
378 if(THEKERNEL->is_halted()) return;
379 }
380 }
381 }
382 break;
383
384 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
385 if(gcode->has_letter('L') && (gcode->get_int('L') == 2 || gcode->get_int('L') == 20) && gcode->has_letter('P')) {
386 size_t n = gcode->get_uint('P');
387 if(n == 0) n = current_wcs; // set current coordinate system
388 else --n;
389 if(n < MAX_WCS) {
390 float x, y, z;
391 std::tie(x, y, z) = wcs_offsets[n];
392 if(gcode->get_int('L') == 20) {
393 // this makes the current machine position (less compensation transform) the offset
394 // get current position in WCS
395 wcs_t pos= mcs2wcs(last_milestone);
396
397 if(gcode->has_letter('X')){
398 x -= to_millimeters(gcode->get_value('X')) - std::get<X_AXIS>(pos);
399 }
400
401 if(gcode->has_letter('Y')){
402 y -= to_millimeters(gcode->get_value('Y')) - std::get<Y_AXIS>(pos);
403 }
404 if(gcode->has_letter('Z')) {
405 z -= to_millimeters(gcode->get_value('Z')) - std::get<Z_AXIS>(pos);
406 }
407
408 } else {
409 // the value is the offset from machine zero
410 if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X'));
411 if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y'));
412 if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z'));
413 }
414 wcs_offsets[n] = wcs_t(x, y, z);
415 }
416 }
417 break;
418
419 case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
420 case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
421 case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
422 case 20: this->inch_mode = true; break;
423 case 21: this->inch_mode = false; break;
424
425 case 54: case 55: case 56: case 57: case 58: case 59:
426 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
427 current_wcs = gcode->g - 54;
428 if(gcode->g == 59 && gcode->subcode > 0) {
429 current_wcs += gcode->subcode;
430 if(current_wcs >= MAX_WCS) current_wcs = MAX_WCS - 1;
431 }
432 break;
433
434 case 90: this->absolute_mode = true; this->e_absolute_mode = true; break;
435 case 91: this->absolute_mode = false; this->e_absolute_mode = false; break;
436
437 case 92: {
438 if(gcode->subcode == 1 || gcode->subcode == 2 || gcode->get_num_args() == 0) {
439 // reset G92 offsets to 0
440 g92_offset = wcs_t(0, 0, 0);
441
442 } else if(gcode->subcode == 3) {
443 // initialize G92 to the specified values, only used for saving it with M500
444 float x= 0, y= 0, z= 0;
445 if(gcode->has_letter('X')) x= gcode->get_value('X');
446 if(gcode->has_letter('Y')) y= gcode->get_value('Y');
447 if(gcode->has_letter('Z')) z= gcode->get_value('Z');
448 g92_offset = wcs_t(x, y, z);
449
450 } else {
451 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
452 float x, y, z;
453 std::tie(x, y, z) = g92_offset;
454 // get current position in WCS
455 wcs_t pos= mcs2wcs(last_milestone);
456
457 // adjust g92 offset to make the current wpos == the value requested
458 if(gcode->has_letter('X')){
459 x += to_millimeters(gcode->get_value('X')) - std::get<X_AXIS>(pos);
460 }
461 if(gcode->has_letter('Y')){
462 y += to_millimeters(gcode->get_value('Y')) - std::get<Y_AXIS>(pos);
463 }
464 if(gcode->has_letter('Z')) {
465 z += to_millimeters(gcode->get_value('Z')) - std::get<Z_AXIS>(pos);
466 }
467 g92_offset = wcs_t(x, y, z);
468 }
469
470 #if MAX_ROBOT_ACTUATORS > 3
471 if(gcode->subcode == 0 && (gcode->has_letter('E') || gcode->get_num_args() == 0)){
472 // reset the E position, legacy for 3d Printers to be reprap compatible
473 // find the selected extruder
474 // NOTE this will only work when E is 0 if volumetric and/or scaling is used as the actuator last milestone will be different if it was scaled
475 for (int i = E_AXIS; i < n_motors; ++i) {
476 if(actuators[i]->is_selected()) {
477 float e= gcode->has_letter('E') ? gcode->get_value('E') : 0;
478 last_milestone[i]= last_machine_position[i]= e;
479 actuators[i]->change_last_milestone(e);
480 break;
481 }
482 }
483 }
484 #endif
485
486 return;
487 }
488 }
489
490 } else if( gcode->has_m) {
491 switch( gcode->m ) {
492 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
493 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
494 // break;
495
496 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
497 if(!THEKERNEL->is_grbl_mode()) break;
498 // fall through to M2
499 case 2: // M2 end of program
500 current_wcs = 0;
501 absolute_mode = true;
502 break;
503 case 17:
504 THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on
505 break;
506
507 case 18: // this used to support parameters, now it ignores them
508 case 84:
509 THEKERNEL->conveyor->wait_for_empty_queue();
510 THEKERNEL->call_event(ON_ENABLE, nullptr); // turn all enable pins off
511 break;
512
513 case 82: e_absolute_mode= true; break;
514 case 83: e_absolute_mode= false; break;
515
516 case 92: // M92 - set steps per mm
517 if (gcode->has_letter('X'))
518 actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X')));
519 if (gcode->has_letter('Y'))
520 actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y')));
521 if (gcode->has_letter('Z'))
522 actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z')));
523
524 gcode->stream->printf("X:%f Y:%f Z:%f ", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
525 gcode->add_nl = true;
526 check_max_actuator_speeds();
527 return;
528
529 case 114:{
530 char buf[64];
531 int n= print_position(gcode->subcode, buf, sizeof buf);
532 if(n > 0) gcode->txt_after_ok.append(buf, n);
533 return;
534 }
535
536 case 120: // push state
537 push_state();
538 break;
539
540 case 121: // pop state
541 pop_state();
542 break;
543
544 case 203: // M203 Set maximum feedrates in mm/sec
545 if (gcode->has_letter('X'))
546 this->max_speeds[X_AXIS] = gcode->get_value('X');
547 if (gcode->has_letter('Y'))
548 this->max_speeds[Y_AXIS] = gcode->get_value('Y');
549 if (gcode->has_letter('Z'))
550 this->max_speeds[Z_AXIS] = gcode->get_value('Z');
551 for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
552 if (gcode->has_letter('A' + i))
553 actuators[i]->set_max_rate(gcode->get_value('A' + i));
554 }
555 check_max_actuator_speeds();
556
557 if(gcode->get_num_args() == 0) {
558 gcode->stream->printf("X:%g Y:%g Z:%g",
559 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]);
560 for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
561 gcode->stream->printf(" %c : %g", 'A' + i, actuators[i]->get_max_rate()); //xxx
562 }
563 gcode->add_nl = true;
564 }
565 break;
566
567 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
568 if (gcode->has_letter('S')) {
569 float acc = gcode->get_value('S'); // mm/s^2
570 // enforce minimum
571 if (acc < 1.0F) acc = 1.0F;
572 this->default_acceleration = acc;
573 }
574 for (int i = X_AXIS; i <= Z_AXIS; ++i) {
575 if (gcode->has_letter(i+'X')) {
576 float acc = gcode->get_value(i+'X'); // mm/s^2
577 // enforce positive
578 if (acc <= 0.0F) acc = NAN;
579 actuators[i]->set_acceleration(acc);
580 }
581 }
582 break;
583
584 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
585 if (gcode->has_letter('X')) {
586 float jd = gcode->get_value('X');
587 // enforce minimum
588 if (jd < 0.0F)
589 jd = 0.0F;
590 THEKERNEL->planner->junction_deviation = jd;
591 }
592 if (gcode->has_letter('Z')) {
593 float jd = gcode->get_value('Z');
594 // enforce minimum, -1 disables it and uses regular junction deviation
595 if (jd < -1.0F)
596 jd = -1.0F;
597 THEKERNEL->planner->z_junction_deviation = jd;
598 }
599 if (gcode->has_letter('S')) {
600 float mps = gcode->get_value('S');
601 // enforce minimum
602 if (mps < 0.0F)
603 mps = 0.0F;
604 THEKERNEL->planner->minimum_planner_speed = mps;
605 }
606 break;
607
608 case 220: // M220 - speed override percentage
609 if (gcode->has_letter('S')) {
610 float factor = gcode->get_value('S');
611 // enforce minimum 10% speed
612 if (factor < 10.0F)
613 factor = 10.0F;
614 // enforce maximum 10x speed
615 if (factor > 1000.0F)
616 factor = 1000.0F;
617
618 seconds_per_minute = 6000.0F / factor;
619 } else {
620 gcode->stream->printf("Speed factor at %6.2f %%\n", 6000.0F / seconds_per_minute);
621 }
622 break;
623
624 case 400: // wait until all moves are done up to this point
625 THEKERNEL->conveyor->wait_for_empty_queue();
626 break;
627
628 case 500: // M500 saves some volatile settings to config override file
629 case 503: { // M503 just prints the settings
630 gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
631
632 // only print XYZ if not NAN
633 gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration);
634 for (int i = X_AXIS; i <= Z_AXIS; ++i) {
635 if(!isnan(actuators[i]->get_acceleration())) gcode->stream->printf("%c%1.5f ", 'X'+i, actuators[i]->get_acceleration());
636 }
637 gcode->stream->printf("\n");
638
639 gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
640 gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f",
641 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
642 actuators[X_AXIS]->get_max_rate(), actuators[Y_AXIS]->get_max_rate(), actuators[Z_AXIS]->get_max_rate());
643 gcode->stream->printf("\n");
644
645 // get or save any arm solution specific optional values
646 BaseSolution::arm_options_t options;
647 if(arm_solution->get_optional(options) && !options.empty()) {
648 gcode->stream->printf(";Optional arm solution specific settings:\nM665");
649 for(auto &i : options) {
650 gcode->stream->printf(" %c%1.4f", i.first, i.second);
651 }
652 gcode->stream->printf("\n");
653 }
654
655 // save wcs_offsets and current_wcs
656 // TODO this may need to be done whenever they change to be compliant
657 gcode->stream->printf(";WCS settings\n");
658 gcode->stream->printf("%s\n", wcs2gcode(current_wcs).c_str());
659 int n = 1;
660 for(auto &i : wcs_offsets) {
661 if(i != wcs_t(0, 0, 0)) {
662 float x, y, z;
663 std::tie(x, y, z) = i;
664 gcode->stream->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n, x, y, z, wcs2gcode(n-1).c_str());
665 }
666 ++n;
667 }
668 if(save_g92) {
669 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
670 // also it needs to be used to set Z0 on rotary deltas as M206/306 can't be used, so saving it is necessary in that case
671 if(g92_offset != wcs_t(0, 0, 0)) {
672 float x, y, z;
673 std::tie(x, y, z) = g92_offset;
674 gcode->stream->printf("G92.3 X%f Y%f Z%f\n", x, y, z); // sets G92 to the specified values
675 }
676 }
677 }
678 break;
679
680 case 665: { // M665 set optional arm solution variables based on arm solution.
681 // the parameter args could be any letter each arm solution only accepts certain ones
682 BaseSolution::arm_options_t options = gcode->get_args();
683 options.erase('S'); // don't include the S
684 options.erase('U'); // don't include the U
685 if(options.size() > 0) {
686 // set the specified options
687 arm_solution->set_optional(options);
688 }
689 options.clear();
690 if(arm_solution->get_optional(options)) {
691 // foreach optional value
692 for(auto &i : options) {
693 // print all current values of supported options
694 gcode->stream->printf("%c: %8.4f ", i.first, i.second);
695 gcode->add_nl = true;
696 }
697 }
698
699 if(gcode->has_letter('S')) { // set delta segments per second, not saved by M500
700 this->delta_segments_per_second = gcode->get_value('S');
701 gcode->stream->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second);
702
703 } else if(gcode->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
704 this->mm_per_line_segment = gcode->get_value('U');
705 this->delta_segments_per_second = 0;
706 gcode->stream->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment);
707 }
708
709 break;
710 }
711 }
712 }
713
714 if( motion_mode != NONE) {
715 process_move(gcode, motion_mode);
716 }
717
718 next_command_is_MCS = false; // must be on same line as G0 or G1
719}
720
721// process a G0/G1/G2/G3
722void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode)
723{
724 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
725 float param[4]{NAN, NAN, NAN, NAN};
726
727 // process primary axis
728 for(int i= X_AXIS; i <= Z_AXIS; ++i) {
729 char letter= 'X'+i;
730 if( gcode->has_letter(letter) ) {
731 param[i] = this->to_millimeters(gcode->get_value(letter));
732 }
733 }
734
735 float offset[3]{0,0,0};
736 for(char letter = 'I'; letter <= 'K'; letter++) {
737 if( gcode->has_letter(letter) ) {
738 offset[letter - 'I'] = this->to_millimeters(gcode->get_value(letter));
739 }
740 }
741
742 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
743 float target[n_motors];
744 memcpy(target, last_milestone, n_motors*sizeof(float));
745
746 if(!next_command_is_MCS) {
747 if(this->absolute_mode) {
748 // apply wcs offsets and g92 offset and tool offset
749 if(!isnan(param[X_AXIS])) {
750 target[X_AXIS]= param[X_AXIS] + std::get<X_AXIS>(wcs_offsets[current_wcs]) - std::get<X_AXIS>(g92_offset) + std::get<X_AXIS>(tool_offset);
751 }
752
753 if(!isnan(param[Y_AXIS])) {
754 target[Y_AXIS]= param[Y_AXIS] + std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(g92_offset) + std::get<Y_AXIS>(tool_offset);
755 }
756
757 if(!isnan(param[Z_AXIS])) {
758 target[Z_AXIS]= param[Z_AXIS] + std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(g92_offset) + std::get<Z_AXIS>(tool_offset);
759 }
760
761 }else{
762 // they are deltas from the last_milestone if specified
763 for(int i= X_AXIS; i <= Z_AXIS; ++i) {
764 if(!isnan(param[i])) target[i] = param[i] + last_milestone[i];
765 }
766 }
767
768 }else{
769 // already in machine coordinates, we do not add tool offset for that
770 for(int i= X_AXIS; i <= Z_AXIS; ++i) {
771 if(!isnan(param[i])) target[i] = param[i];
772 }
773 }
774
775 // process extruder parameters, for active extruder only (only one active extruder at a time)
776 selected_extruder= 0;
777 if(gcode->has_letter('E')) {
778 for (int i = E_AXIS; i < n_motors; ++i) {
779 // find first selected extruder
780 if(actuators[i]->is_selected()) {
781 param[E_AXIS]= gcode->get_value('E');
782 selected_extruder= i;
783 break;
784 }
785 }
786 }
787
788 // do E for the selected extruder
789 float delta_e= NAN;
790 if(selected_extruder > 0 && !isnan(param[E_AXIS])) {
791 if(this->e_absolute_mode) {
792 target[selected_extruder]= param[E_AXIS];
793 delta_e= target[selected_extruder] - last_milestone[selected_extruder];
794 }else{
795 delta_e= param[E_AXIS];
796 target[selected_extruder] = delta_e + last_milestone[selected_extruder];
797 }
798 }
799
800 if( gcode->has_letter('F') ) {
801 if( motion_mode == SEEK )
802 this->seek_rate = this->to_millimeters( gcode->get_value('F') );
803 else
804 this->feed_rate = this->to_millimeters( gcode->get_value('F') );
805 }
806
807 bool moved= false;
808
809 // Perform any physical actions
810 switch(motion_mode) {
811 case NONE: break;
812
813 case SEEK:
814 moved= this->append_line(gcode, target, this->seek_rate / seconds_per_minute, delta_e );
815 break;
816
817 case LINEAR:
818 moved= this->append_line(gcode, target, this->feed_rate / seconds_per_minute, delta_e );
819 break;
820
821 case CW_ARC:
822 case CCW_ARC:
823 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
824 moved= this->compute_arc(gcode, offset, target, motion_mode);
825 break;
826 }
827
828 if(moved) {
829 // set last_milestone to the calculated target
830 memcpy(last_milestone, target, n_motors*sizeof(float));
831 }
832}
833
834// reset the machine position for all axis. Used for homing.
835// During homing compensation is turned off (actually not used as it drives steppers directly)
836// once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
837// so in those cases the final position is compensated.
838void Robot::reset_axis_position(float x, float y, float z)
839{
840 // these are set to the same as compensation was not used to get to the current position
841 last_machine_position[X_AXIS]= last_milestone[X_AXIS] = x;
842 last_machine_position[Y_AXIS]= last_milestone[Y_AXIS] = y;
843 last_machine_position[Z_AXIS]= last_milestone[Z_AXIS] = z;
844
845 // now set the actuator positions to match
846 ActuatorCoordinates actuator_pos;
847 arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos);
848 for (size_t i = X_AXIS; i <= Z_AXIS; i++)
849 actuators[i]->change_last_milestone(actuator_pos[i]);
850}
851
852// Reset the position for an axis (used in homing, and to reset extruder after suspend)
853void Robot::reset_axis_position(float position, int axis)
854{
855 last_milestone[axis] = position;
856 if(axis <= Z_AXIS) {
857 reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
858 }else{
859 // extruders need to be set not calculated
860 last_machine_position[axis]= position;
861 }
862}
863
864// similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
865// then sets the axis positions to match. currently only called from Endstops.cpp
866void Robot::reset_actuator_position(const ActuatorCoordinates &ac)
867{
868 for (size_t i = X_AXIS; i <= Z_AXIS; i++)
869 actuators[i]->change_last_milestone(ac[i]);
870
871 // now correct axis positions then recorrect actuator to account for rounding
872 reset_position_from_current_actuator_position();
873}
874
875// Use FK to find out where actuator is and reset to match
876void Robot::reset_position_from_current_actuator_position()
877{
878 ActuatorCoordinates actuator_pos;
879 for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
880 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
881 actuator_pos[i] = actuators[i]->get_current_position();
882 }
883
884 // discover machine position from where actuators actually are
885 arm_solution->actuator_to_cartesian(actuator_pos, last_machine_position);
886 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
887 memcpy(last_milestone, last_machine_position, sizeof last_milestone);
888
889 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
890 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
891 // to get everything in perfect sync.
892 arm_solution->cartesian_to_actuator(last_machine_position, actuator_pos);
893 for (size_t i = X_AXIS; i <= Z_AXIS; i++)
894 actuators[i]->change_last_milestone(actuator_pos[i]);
895}
896
897// Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
898// target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
899// all transforms and is what we actually convert to actuator positions
900bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s)
901{
902 float deltas[n_motors];
903 float transformed_target[n_motors]; // adjust target for bed compensation and WCS offsets
904 float unit_vec[N_PRIMARY_AXIS];
905 float millimeters_of_travel= 0;
906
907 // catch negative or zero feed rates and return the same error as GRBL does
908 if(rate_mm_s <= 0.0F) {
909 gcode->is_error= true;
910 gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0");
911 return false;
912 }
913
914 // unity transform by default
915 memcpy(transformed_target, target, n_motors*sizeof(float));
916
917 // check function pointer and call if set to transform the target to compensate for bed
918 if(compensationTransform) {
919 // some compensation strategies can transform XYZ, some just change Z
920 compensationTransform(transformed_target);
921 }
922
923 bool move= false;
924 float sos= 0;
925
926 // find distance moved by each axis, use transformed target from the current machine position
927 for (size_t i = 0; i < n_motors; i++) {
928 deltas[i] = transformed_target[i] - last_machine_position[i];
929 if(deltas[i] == 0) continue;
930 // at least one non zero delta
931 move = true;
932 if(i <= Z_AXIS) {
933 sos += powf(deltas[i], 2);
934 }
935 }
936
937 // nothing moved
938 if(!move) return false;
939
940 // set if none of the primary axis is moving
941 bool auxilliary_move= false;
942 if(sos > 0.0F){
943 millimeters_of_travel= sqrtf(sos);
944
945 } else if(n_motors >= E_AXIS) { // if we have more than 3 axis/actuators (XYZE)
946 // non primary axis move (like extrude)
947 // select the biggest one, will be the only active E
948 auto mi= std::max_element(&deltas[E_AXIS], &deltas[n_motors], [](float a, float b){ return std::abs(a) < std::abs(b); } );
949 millimeters_of_travel= std::abs(*mi);
950 auxilliary_move= true;
951
952 }else{
953 // shouldn't happen but just in case
954 return false;
955 }
956
957 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
958 // as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
959 if(millimeters_of_travel < 0.00001F) return false;
960
961 // this is the machine position
962 memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float));
963
964 if(!auxilliary_move) {
965 // find distance unit vector for primary axis only
966 for (size_t i = X_AXIS; i <= Z_AXIS; i++)
967 unit_vec[i] = deltas[i] / millimeters_of_travel;
968 }
969
970 // Do not move faster than the configured cartesian limits for XYZ
971 for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
972 if ( max_speeds[axis] > 0 ) {
973 float axis_speed = fabsf(unit_vec[axis] * rate_mm_s);
974
975 if (axis_speed > max_speeds[axis])
976 rate_mm_s *= ( max_speeds[axis] / axis_speed );
977 }
978 }
979
980 // find actuator position given the machine position, use actual adjusted target
981 ActuatorCoordinates actuator_pos;
982 arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos );
983
984#if MAX_ROBOT_ACTUATORS > 3
985 // for the extruders just copy the position
986 for (size_t i = E_AXIS; i < n_motors; i++) {
987 actuator_pos[i]= last_machine_position[i];
988 if(!isnan(this->e_scale)) {
989 // NOTE this relies on the fact only one extruder is active at a time
990 // scale for volumetric or flow rate
991 // TODO is this correct? scaling the absolute target? what if the scale changes?
992 // for volumetric it basically converts mm³ to mm, but what about flow rate?
993 actuator_pos[i] *= this->e_scale;
994 }
995 }
996#endif
997
998 // use default acceleration to start with
999 float acceleration = default_acceleration;
1000
1001 float isecs = rate_mm_s / millimeters_of_travel;
1002
1003 // check per-actuator speed limits
1004 for (size_t actuator = 0; actuator < n_motors; actuator++) {
1005 float d = fabsf(actuator_pos[actuator] - actuators[actuator]->get_last_milestone());
1006 if(d == 0 || !actuators[actuator]->is_selected()) continue; // no movement for this actuator
1007
1008 float actuator_rate= d * isecs;
1009 if (actuator_rate > actuators[actuator]->get_max_rate()) {
1010 rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
1011 isecs = rate_mm_s / millimeters_of_travel;
1012 }
1013
1014 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1015 // TODO we may need to do all of them, check E won't limit XYZ
1016 // if(auxilliary_move || actuator <= Z_AXIS) {
1017 float ma = actuators[actuator]->get_acceleration(); // in mm/sec²
1018 if(!isnan(ma)) { // if axis does not have acceleration set then it uses the default_acceleration
1019 float ca = fabsf((deltas[actuator]/millimeters_of_travel) * acceleration);
1020 if (ca > ma) {
1021 acceleration *= ( ma / ca );
1022 }
1023 }
1024 // }
1025 }
1026
1027 // Append the block to the planner
1028 THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, millimeters_of_travel, auxilliary_move? nullptr : unit_vec, acceleration );
1029
1030 return true;
1031}
1032
1033// Used to plan a single move used by things like endstops when homing, zprobe, extruder retracts etc.
1034// TODO this pretty much duplicates append_milestone, so try to refactor it away.
1035bool Robot::solo_move(const float *delta, float rate_mm_s, uint8_t naxis)
1036{
1037 if(THEKERNEL->is_halted()) return false;
1038
1039 // catch negative or zero feed rates and return the same error as GRBL does
1040 if(rate_mm_s <= 0.0F) {
1041 return false;
1042 }
1043
1044 bool move= false;
1045 float sos= 0;
1046
1047 // find distance moved by each axis
1048 for (size_t i = 0; i < naxis; i++) {
1049 if(delta[i] == 0) continue;
1050 // at least one non zero delta
1051 move = true;
1052 sos += powf(delta[i], 2);
1053 }
1054
1055 // nothing moved
1056 if(!move) return false;
1057
1058 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1059 // as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
1060 if(sos < 0.00001F) return false;
1061
1062 float millimeters_of_travel= sqrtf(sos);
1063
1064 // this is the new machine position
1065 for (int axis = 0; axis < naxis; axis++) {
1066 this->last_machine_position[axis] += delta[axis];
1067 }
1068 // we also need to update last_milestone here which is the same as last_machine_position as there was no compensation
1069 memcpy(this->last_milestone, this->last_machine_position, naxis*sizeof(float));
1070
1071
1072 // Do not move faster than the configured cartesian limits for XYZ
1073 for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
1074 if ( max_speeds[axis] > 0 ) {
1075 float axis_speed = fabsf(delta[axis] / millimeters_of_travel * rate_mm_s);
1076
1077 if (axis_speed > max_speeds[axis])
1078 rate_mm_s *= ( max_speeds[axis] / axis_speed );
1079 }
1080 }
1081
1082 // find actuator position given the machine position
1083 ActuatorCoordinates actuator_pos;
1084 arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos );
1085
1086 // for the extruders just copy the position, need to copy all actuators
1087 for (size_t i = N_PRIMARY_AXIS; i < n_motors; i++) {
1088 actuator_pos[i]= last_machine_position[i];
1089 }
1090
1091 // use default acceleration to start with
1092 float acceleration = default_acceleration;
1093 float isecs = rate_mm_s / millimeters_of_travel;
1094
1095 // check per-actuator speed limits
1096 for (size_t actuator = 0; actuator < naxis; actuator++) {
1097 float d = fabsf(actuator_pos[actuator] - actuators[actuator]->get_last_milestone());
1098 if(d == 0) continue; // no movement for this actuator
1099
1100 float actuator_rate= d * isecs;
1101 if (actuator_rate > actuators[actuator]->get_max_rate()) {
1102 rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
1103 isecs = rate_mm_s / millimeters_of_travel;
1104 }
1105
1106 // adjust acceleration to lowest found in an active axis
1107 float ma = actuators[actuator]->get_acceleration(); // in mm/sec²
1108 if(!isnan(ma)) { // if axis does not have acceleration set then it uses the default_acceleration
1109 float ca = fabsf((d/millimeters_of_travel) * acceleration);
1110 if (ca > ma) {
1111 acceleration *= ( ma / ca );
1112 }
1113 }
1114 }
1115 // Append the block to the planner
1116 THEKERNEL->planner->append_block(actuator_pos, n_motors, rate_mm_s, millimeters_of_travel, nullptr, acceleration);
1117
1118 return true;
1119}
1120
1121// Append a move to the queue ( cutting it into segments if needed )
1122bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, float delta_e)
1123{
1124 // by default there is no e scaling required, but if volumetric extrusion is enabled this will be set to scale the parameter
1125 this->e_scale= NAN;
1126
1127 // Find out the distance for this move in XYZ in MCS
1128 float millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_milestone[Z_AXIS], 2 ));
1129
1130 if(millimeters_of_travel < 0.00001F) {
1131 // we have no movement in XYZ, probably E only extrude or retract which is always in mm, so no E scaling required
1132 return this->append_milestone(gcode, target, rate_mm_s);
1133 }
1134
1135 /*
1136 For extruders, we need to do some extra work...
1137 if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit.
1138 Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
1139 We ask Extruder to do all the work but we need to pass in the relevant data.
1140 NOTE we need to do this before we segment the line (for deltas)
1141 This also sets any scaling due to flow rate and volumetric if a G1
1142 */
1143 if(!isnan(delta_e) && gcode->has_g && gcode->g == 1) {
1144 float data[2]= {delta_e, rate_mm_s / millimeters_of_travel};
1145 if(PublicData::set_value(extruder_checksum, target_checksum, data)) {
1146 rate_mm_s *= data[1]; // adjust the feedrate
1147 // we may need to scale the amount moved too
1148 this->e_scale= data[0];
1149 }
1150 }
1151
1152 // We cut the line into smaller segments. This is only needed on a cartesian robot for zgrid, but always necessary for robots with rotational axes like Deltas.
1153 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1154 // The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
1155 uint16_t segments;
1156
1157 if(this->disable_segmentation || (!segment_z_moves && !gcode->has_letter('X') && !gcode->has_letter('Y'))) {
1158 segments= 1;
1159
1160 } else if(this->delta_segments_per_second > 1.0F) {
1161 // enabled if set to something > 1, it is set to 0.0 by default
1162 // segment based on current speed and requested segments per second
1163 // the faster the travel speed the fewer segments needed
1164 // NOTE rate is mm/sec and we take into account any speed override
1165 float seconds = millimeters_of_travel / rate_mm_s;
1166 segments = max(1.0F, ceilf(this->delta_segments_per_second * seconds));
1167 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1168
1169 } else {
1170 if(this->mm_per_line_segment == 0.0F) {
1171 segments = 1; // don't split it up
1172 } else {
1173 segments = ceilf( millimeters_of_travel / this->mm_per_line_segment);
1174 }
1175 }
1176
1177 bool moved= false;
1178 if (segments > 1) {
1179 // A vector to keep track of the endpoint of each segment
1180 float segment_delta[n_motors];
1181 float segment_end[n_motors];
1182 memcpy(segment_end, last_milestone, n_motors*sizeof(float));
1183
1184 // How far do we move each segment?
1185 for (int i = 0; i < n_motors; i++)
1186 segment_delta[i] = (target[i] - last_milestone[i]) / segments;
1187
1188 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1189 // We always add another point after this loop so we stop at segments-1, ie i < segments
1190 for (int i = 1; i < segments; i++) {
1191 if(THEKERNEL->is_halted()) return false; // don't queue any more segments
1192 for (int i = 0; i < n_motors; i++)
1193 segment_end[i] += segment_delta[i];
1194
1195 // Append the end of this segment to the queue
1196 bool b= this->append_milestone(gcode, segment_end, rate_mm_s);
1197 moved= moved || b;
1198 }
1199 }
1200
1201 // Append the end of this full move to the queue
1202 if(this->append_milestone(gcode, target, rate_mm_s)) moved= true;
1203
1204 this->next_command_is_MCS = false; // always reset this
1205
1206 return moved;
1207}
1208
1209
1210// Append an arc to the queue ( cutting it into segments as needed )
1211bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[], float radius, bool is_clockwise )
1212{
1213
1214 // Scary math
1215 float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
1216 float center_axis1 = this->last_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
1217 float linear_travel = target[this->plane_axis_2] - this->last_milestone[this->plane_axis_2];
1218 float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
1219 float r_axis1 = -offset[this->plane_axis_1];
1220 float rt_axis0 = target[this->plane_axis_0] - center_axis0;
1221 float rt_axis1 = target[this->plane_axis_1] - center_axis1;
1222
1223 // Patch from GRBL Firmware - Christoph Baumann 04072015
1224 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1225 float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
1226 if (is_clockwise) { // Correct atan2 output per direction
1227 if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
1228 } else {
1229 if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); }
1230 }
1231
1232 // Find the distance for this gcode
1233 float millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel));
1234
1235 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1236 if( millimeters_of_travel < 0.00001F ) {
1237 return false;
1238 }
1239
1240 // limit segments by maximum arc error
1241 float arc_segment = this->mm_per_arc_segment;
1242 if ((this->mm_max_arc_error > 0) && (2 * radius > this->mm_max_arc_error)) {
1243 float min_err_segment = 2 * sqrtf((this->mm_max_arc_error * (2 * radius - this->mm_max_arc_error)));
1244 if (this->mm_per_arc_segment < min_err_segment) {
1245 arc_segment = min_err_segment;
1246 }
1247 }
1248 // Figure out how many segments for this gcode
1249 uint16_t segments = ceilf(millimeters_of_travel / arc_segment);
1250
1251 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1252 float theta_per_segment = angular_travel / segments;
1253 float linear_per_segment = linear_travel / segments;
1254
1255 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1256 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1257 r_T = [cos(phi) -sin(phi);
1258 sin(phi) cos(phi] * r ;
1259 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1260 defined from the circle center to the initial position. Each line segment is formed by successive
1261 vector rotations. This requires only two cos() and sin() computations to form the rotation
1262 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1263 all float numbers are single precision on the Arduino. (True float precision will not have
1264 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1265 tool precision in some cases. Therefore, arc path correction is implemented.
1266
1267 Small angle approximation may be used to reduce computation overhead further. This approximation
1268 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1269 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1270 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1271 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1272 issue for CNC machines with the single precision Arduino calculations.
1273 This approximation also allows mc_arc to immediately insert a line segment into the planner
1274 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1275 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1276 This is important when there are successive arc motions.
1277 */
1278 // Vector rotation matrix values
1279 float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation
1280 float sin_T = theta_per_segment;
1281
1282 float arc_target[3];
1283 float sin_Ti;
1284 float cos_Ti;
1285 float r_axisi;
1286 uint16_t i;
1287 int8_t count = 0;
1288
1289 // Initialize the linear axis
1290 arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
1291
1292 bool moved= false;
1293 for (i = 1; i < segments; i++) { // Increment (segments-1)
1294 if(THEKERNEL->is_halted()) return false; // don't queue any more segments
1295
1296 if (count < this->arc_correction ) {
1297 // Apply vector rotation matrix
1298 r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
1299 r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
1300 r_axis1 = r_axisi;
1301 count++;
1302 } else {
1303 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1304 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1305 cos_Ti = cosf(i * theta_per_segment);
1306 sin_Ti = sinf(i * theta_per_segment);
1307 r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti;
1308 r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti;
1309 count = 0;
1310 }
1311
1312 // Update arc_target location
1313 arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
1314 arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
1315 arc_target[this->plane_axis_2] += linear_per_segment;
1316
1317 // Append this segment to the queue
1318 bool b= this->append_milestone(gcode, arc_target, this->feed_rate / seconds_per_minute);
1319 moved= moved || b;
1320 }
1321
1322 // Ensure last segment arrives at target location.
1323 if(this->append_milestone(gcode, target, this->feed_rate / seconds_per_minute)) moved= true;
1324
1325 return moved;
1326}
1327
1328// Do the math for an arc and add it to the queue
1329bool Robot::compute_arc(Gcode * gcode, const float offset[], const float target[], enum MOTION_MODE_T motion_mode)
1330{
1331
1332 // Find the radius
1333 float radius = hypotf(offset[this->plane_axis_0], offset[this->plane_axis_1]);
1334
1335 // Set clockwise/counter-clockwise sign for mc_arc computations
1336 bool is_clockwise = false;
1337 if( motion_mode == CW_ARC ) {
1338 is_clockwise = true;
1339 }
1340
1341 // Append arc
1342 return this->append_arc(gcode, target, offset, radius, is_clockwise );
1343}
1344
1345
1346float Robot::theta(float x, float y)
1347{
1348 float t = atanf(x / fabs(y));
1349 if (y > 0) {
1350 return(t);
1351 } else {
1352 if (t > 0) {
1353 return(PI - t);
1354 } else {
1355 return(-PI - t);
1356 }
1357 }
1358}
1359
1360void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
1361{
1362 this->plane_axis_0 = axis_0;
1363 this->plane_axis_1 = axis_1;
1364 this->plane_axis_2 = axis_2;
1365}
1366
1367void Robot::clearToolOffset()
1368{
1369 this->tool_offset= wcs_t(0,0,0);
1370}
1371
1372void Robot::setToolOffset(const float offset[3])
1373{
1374 this->tool_offset= wcs_t(offset[0], offset[1], offset[2]);
1375}
1376
1377float Robot::get_feed_rate() const
1378{
1379 return THEKERNEL->gcode_dispatch->get_modal_command() == 0 ? seek_rate : feed_rate;
1380}
Unnamed repository; edit this file 'description' to name the repository.