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