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/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
15 #include "StepperMotor.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"
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 #include "EndstopsPublicAccess.h"
38 #include "mbed.h" // for us_ticker_read()
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")
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")
73 // new-style actuator stuff
74 #define actuator_checksum CHEKCSUM("actuator")
76 #define step_pin_checksum CHECKSUM("step_pin")
77 #define dir_pin_checksum CHEKCSUM("dir_pin")
78 #define en_pin_checksum CHECKSUM("en_pin")
80 #define max_speed_checksum CHECKSUM("max_speed")
81 #define acceleration_checksum CHECKSUM("acceleration")
82 #define z_acceleration_checksum CHECKSUM("z_acceleration")
84 #define alpha_checksum CHECKSUM("alpha")
85 #define beta_checksum CHECKSUM("beta")
86 #define gamma_checksum CHECKSUM("gamma")
88 #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
90 #define enable_checksum CHECKSUM("enable")
91 #define halt_checksum CHECKSUM("halt")
92 #define soft_endstop_checksum CHECKSUM("soft_endstop")
93 #define xmin_checksum CHECKSUM("x_min")
94 #define ymin_checksum CHECKSUM("y_min")
95 #define zmin_checksum CHECKSUM("z_min")
96 #define xmax_checksum CHECKSUM("x_max")
97 #define ymax_checksum CHECKSUM("y_max")
98 #define zmax_checksum CHECKSUM("z_max")
100 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
102 // 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
103 // It takes care of cutting arcs into segments, same thing for line that are too long
107 this->inch_mode
= false;
108 this->absolute_mode
= true;
109 this->e_absolute_mode
= true;
110 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
111 memset(this->machine_position
, 0, sizeof machine_position
);
112 memset(this->compensated_machine_position
, 0, sizeof compensated_machine_position
);
113 this->arm_solution
= NULL
;
114 seconds_per_minute
= 60.0F
;
115 this->clearToolOffset();
116 this->compensationTransform
= nullptr;
117 this->get_e_scale_fnc
= nullptr;
118 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
119 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
120 this->next_command_is_MCS
= false;
121 this->disable_segmentation
= false;
122 this->disable_arm_solution
= false;
126 //Called when the module has just been loaded
127 void Robot::on_module_loaded()
129 this->register_for_event(ON_GCODE_RECEIVED
);
135 #define ACTUATOR_CHECKSUMS(X) { \
136 CHECKSUM(X "_step_pin"), \
137 CHECKSUM(X "_dir_pin"), \
138 CHECKSUM(X "_en_pin"), \
139 CHECKSUM(X "_steps_per_mm"), \
140 CHECKSUM(X "_max_rate"), \
141 CHECKSUM(X "_acceleration") \
144 void Robot::load_config()
146 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
147 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
148 // To make adding those solution easier, they have their own, separate object.
149 // Here we read the config to find out which arm solution to use
150 if (this->arm_solution
) delete this->arm_solution
;
151 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
152 // Note checksums are not const expressions when in debug mode, so don't use switch
153 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
154 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== corexz_checksum
) {
157 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
160 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
163 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== rotary_delta_checksum
) {
166 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== morgan_checksum
) {
169 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
171 } else if(solution_checksum
== cartesian_checksum
) {
172 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
175 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
178 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
179 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
180 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
181 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
182 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
183 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
184 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
186 // in mm/sec but specified in config as mm/min
187 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
188 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
189 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
190 this->max_speed
= THEKERNEL
->config
->value(max_speed_checksum
)->by_default( -60.0F
)->as_number() / 60.0F
;
192 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
193 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
194 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
196 // optional setting for a fixed G92 offset
197 std::vector
<float> t
= parse_number_list(g92
.c_str());
199 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
203 // default s value for laser
204 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
206 // Make our Primary XYZ StepperMotors, and potentially A B C
207 uint16_t const motor_checksums
[][6] = {
208 ACTUATOR_CHECKSUMS("alpha"), // X
209 ACTUATOR_CHECKSUMS("beta"), // Y
210 ACTUATOR_CHECKSUMS("gamma"), // Z
211 #if MAX_ROBOT_ACTUATORS > 3
212 ACTUATOR_CHECKSUMS("delta"), // A
213 #if MAX_ROBOT_ACTUATORS > 4
214 ACTUATOR_CHECKSUMS("epsilon"), // B
215 #if MAX_ROBOT_ACTUATORS > 5
216 ACTUATOR_CHECKSUMS("zeta") // C
222 // default acceleration setting, can be overriden with newer per axis settings
223 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
226 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
227 Pin pins
[3]; //step, dir, enable
228 for (size_t i
= 0; i
< 3; i
++) {
229 pins
[i
].from_string(THEKERNEL
->config
->value(motor_checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
232 if(!pins
[0].connected() || !pins
[1].connected()) { // step and dir must be defined, but enable is optional
234 THEKERNEL
->streams
->printf("FATAL: motor %c is not defined in config\n", 'X'+a
);
235 n_motors
= a
; // we only have this number of motors
238 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
241 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
242 // register this motor (NB This must be 0,1,2) of the actuators array
243 uint8_t n
= register_motor(sm
);
245 // this is a fatal error
246 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
250 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(motor_checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
251 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(motor_checksums
[a
][4])->by_default(30000.0F
)->as_number()/60.0F
); // it is in mm/min and converted to mm/sec
252 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(motor_checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
255 check_max_actuator_speeds(); // check the configs are sane
257 // if we have not specified a z acceleration see if the legacy config was set
258 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
259 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
261 actuators
[Z_AXIS
]->set_acceleration(acc
);
265 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
266 // so the first move can be correct if homing is not performed
267 ActuatorCoordinates actuator_pos
;
268 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
269 for (size_t i
= 0; i
< n_motors
; i
++)
270 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
272 //this->clearToolOffset();
274 soft_endstop_enabled
= THEKERNEL
->config
->value(soft_endstop_checksum
, enable_checksum
)->by_default(false)->as_bool();
275 soft_endstop_halt
= THEKERNEL
->config
->value(soft_endstop_checksum
, halt_checksum
)->by_default(true)->as_bool();
277 soft_endstop_min
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmin_checksum
)->by_default(NAN
)->as_number();
278 soft_endstop_min
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymin_checksum
)->by_default(NAN
)->as_number();
279 soft_endstop_min
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmin_checksum
)->by_default(NAN
)->as_number();
280 soft_endstop_max
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmax_checksum
)->by_default(NAN
)->as_number();
281 soft_endstop_max
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymax_checksum
)->by_default(NAN
)->as_number();
282 soft_endstop_max
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmax_checksum
)->by_default(NAN
)->as_number();
285 uint8_t Robot::register_motor(StepperMotor
*motor
)
287 // register this motor with the step ticker
288 THEKERNEL
->step_ticker
->register_motor(motor
);
289 if(n_motors
>= k_max_actuators
) {
290 // this is a fatal error
291 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
294 actuators
.push_back(motor
);
295 motor
->set_motor_id(n_motors
);
299 void Robot::push_state()
301 bool am
= this->absolute_mode
;
302 bool em
= this->e_absolute_mode
;
303 bool im
= this->inch_mode
;
304 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
308 void Robot::pop_state()
310 if(!state_stack
.empty()) {
311 auto s
= state_stack
.top();
313 this->feed_rate
= std::get
<0>(s
);
314 this->seek_rate
= std::get
<1>(s
);
315 this->absolute_mode
= std::get
<2>(s
);
316 this->e_absolute_mode
= std::get
<3>(s
);
317 this->inch_mode
= std::get
<4>(s
);
318 this->current_wcs
= std::get
<5>(s
);
322 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
324 std::vector
<wcs_t
> v
;
325 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
326 for(auto& i
: wcs_offsets
) {
329 v
.push_back(g92_offset
);
330 v
.push_back(tool_offset
);
334 void Robot::get_current_machine_position(float *pos
) const
336 // get real time current actuator position in mm
337 ActuatorCoordinates current_position
{
338 actuators
[X_AXIS
]->get_current_position(),
339 actuators
[Y_AXIS
]->get_current_position(),
340 actuators
[Z_AXIS
]->get_current_position()
343 // get machine position from the actuator position using FK
344 arm_solution
->actuator_to_cartesian(current_position
, pos
);
347 void Robot::print_position(uint8_t subcode
, std::string
& res
, bool ignore_extruders
) const
349 // M114.1 is a new way to do this (similar to how GRBL does it).
350 // it returns the realtime position based on the current step position of the actuators.
351 // this does require a FK to get a machine position from the actuator position
352 // and then invert all the transforms to get a workspace position from machine position
353 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
356 if(subcode
== 0) { // M114 print WCS
357 wcs_t pos
= mcs2wcs(machine_position
);
358 n
= snprintf(buf
, sizeof(buf
), "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
)));
360 } else if(subcode
== 4) {
361 // M114.4 print last milestone
362 n
= snprintf(buf
, sizeof(buf
), "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position
[X_AXIS
], machine_position
[Y_AXIS
], machine_position
[Z_AXIS
]);
364 } else if(subcode
== 5) {
365 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
366 // will differ from LMS by the compensation at the current position otherwise
367 n
= snprintf(buf
, sizeof(buf
), "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
]);
370 // get real time positions
372 get_current_machine_position(mpos
);
374 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
375 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
377 if(subcode
== 1) { // M114.1 print realtime WCS
378 wcs_t pos
= mcs2wcs(mpos
);
379 n
= snprintf(buf
, sizeof(buf
), "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
)));
381 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
382 n
= snprintf(buf
, sizeof(buf
), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
384 } else if(subcode
== 3) { // M114.3 print realtime actuator position
385 // get real time current actuator position in mm
386 ActuatorCoordinates current_position
{
387 actuators
[X_AXIS
]->get_current_position(),
388 actuators
[Y_AXIS
]->get_current_position(),
389 actuators
[Z_AXIS
]->get_current_position()
391 n
= snprintf(buf
, sizeof(buf
), "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position
[X_AXIS
], current_position
[Y_AXIS
], current_position
[Z_AXIS
]);
395 if(n
> sizeof(buf
)) n
= sizeof(buf
);
398 #if MAX_ROBOT_ACTUATORS > 3
399 // deal with the ABC axis
400 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
402 if(ignore_extruders
&& actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
403 if(subcode
== 4) { // M114.4 print last milestone
404 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
406 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
407 // current actuator position
408 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
410 if(n
> sizeof(buf
)) n
= sizeof(buf
);
411 if(n
> 0) res
.append(buf
, n
);
416 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
417 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
419 return std::make_tuple(
420 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
),
421 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
),
422 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
)
426 // this does a sanity check that actuator speeds do not exceed steps rate capability
427 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
428 void Robot::check_max_actuator_speeds()
430 for (size_t i
= 0; i
< n_motors
; i
++) {
431 if(actuators
[i
]->is_extruder()) continue; //extruders are not included in this check
433 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
434 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
435 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
436 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());
441 //A GCode has been received
442 //See if the current Gcode line has some orders for us
443 void Robot::on_gcode_received(void *argument
)
445 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
447 enum MOTION_MODE_T motion_mode
= NONE
;
451 case 0: motion_mode
= SEEK
; break;
452 case 1: motion_mode
= LINEAR
; break;
453 case 2: motion_mode
= CW_ARC
; break;
454 case 3: motion_mode
= CCW_ARC
; break;
455 case 4: { // G4 Dwell
456 uint32_t delay_ms
= 0;
457 if (gcode
->has_letter('P')) {
458 if(THEKERNEL
->is_grbl_mode()) {
459 // in grbl mode (and linuxcnc) P is decimal seconds
460 float f
= gcode
->get_value('P');
461 delay_ms
= f
* 1000.0F
;
464 // in reprap P is milliseconds, they always have to be different!
465 delay_ms
= gcode
->get_int('P');
468 if (gcode
->has_letter('S')) {
469 delay_ms
+= gcode
->get_int('S') * 1000;
473 THEKERNEL
->conveyor
->wait_for_idle();
474 // wait for specified time
475 uint32_t start
= us_ticker_read(); // mbed call
476 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
477 THEKERNEL
->call_event(ON_IDLE
, this);
478 if(THEKERNEL
->is_halted()) return;
484 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
485 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
486 size_t n
= gcode
->get_uint('P');
487 if(n
== 0) n
= current_wcs
; // set current coordinate system
491 std::tie(x
, y
, z
) = wcs_offsets
[n
];
492 if(gcode
->get_int('L') == 20) {
493 // this makes the current machine position (less compensation transform) the offset
494 // get current position in WCS
495 wcs_t pos
= mcs2wcs(machine_position
);
497 if(gcode
->has_letter('X')){
498 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
501 if(gcode
->has_letter('Y')){
502 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
504 if(gcode
->has_letter('Z')) {
505 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
510 // the value is the offset from machine zero
511 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
512 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
513 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
515 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
516 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
517 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
520 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
525 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
526 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
527 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
528 case 20: this->inch_mode
= true; break;
529 case 21: this->inch_mode
= false; break;
531 case 54: case 55: case 56: case 57: case 58: case 59:
532 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
533 current_wcs
= gcode
->g
- 54;
534 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
535 current_wcs
+= gcode
->subcode
;
536 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
540 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
541 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
544 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
545 // reset G92 offsets to 0
546 g92_offset
= wcs_t(0, 0, 0);
548 } else if(gcode
->subcode
== 3) {
549 // initialize G92 to the specified values, only used for saving it with M500
550 float x
= 0, y
= 0, z
= 0;
551 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
552 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
553 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
554 g92_offset
= wcs_t(x
, y
, z
);
557 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
559 std::tie(x
, y
, z
) = g92_offset
;
560 // get current position in WCS
561 wcs_t pos
= mcs2wcs(machine_position
);
563 // adjust g92 offset to make the current wpos == the value requested
564 if(gcode
->has_letter('X')){
565 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
567 if(gcode
->has_letter('Y')){
568 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
570 if(gcode
->has_letter('Z')) {
571 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
573 g92_offset
= wcs_t(x
, y
, z
);
576 #if MAX_ROBOT_ACTUATORS > 3
577 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
578 // reset the E position, legacy for 3d Printers to be reprap compatible
579 // find the selected extruder
580 int selected_extruder
= get_active_extruder();
581 if(selected_extruder
> 0) {
582 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
583 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
584 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
593 } else if( gcode
->has_m
) {
595 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
596 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
599 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
600 if(!THEKERNEL
->is_grbl_mode()) break;
601 // fall through to M2
602 case 2: // M2 end of program
604 absolute_mode
= true;
607 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
610 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
611 if(gcode
->get_num_args() > 0) {
612 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
614 for (int i
= 0; i
< n_motors
; ++i
) {
615 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
616 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
618 // handle E parameter as currently selected extruder ABC
619 if(gcode
->has_letter('E')) {
620 // find first selected extruder
621 int i
= get_active_extruder();
623 bm
|= (0x02<<i
); // set appropriate bit
627 THEKERNEL
->conveyor
->wait_for_idle();
628 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
633 THEKERNEL
->conveyor
->wait_for_idle();
634 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
637 case 82: e_absolute_mode
= true; break;
638 case 83: e_absolute_mode
= false; break;
640 case 92: // M92 - set steps per mm
641 for (int i
= 0; i
< n_motors
; ++i
) {
642 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
643 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
644 if(gcode
->has_letter(axis
)) {
645 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
647 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
649 gcode
->add_nl
= true;
650 check_max_actuator_speeds();
655 print_position(gcode
->subcode
, buf
, true); // ignore extruders as they will print E themselves
656 gcode
->txt_after_ok
.append(buf
);
660 case 120: // push state
664 case 121: // pop state
668 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
669 if(gcode
->get_num_args() == 0) {
670 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
671 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
673 if(gcode
->subcode
== 1) {
674 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
675 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
676 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
679 gcode
->stream
->printf(" S: %g ", this->max_speed
);
682 gcode
->add_nl
= true;
685 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
686 if (gcode
->has_letter('X' + i
)) {
687 float v
= gcode
->get_value('X'+i
);
688 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
689 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
693 if(gcode
->subcode
== 1) {
694 // ABC axis only handle actuator max speeds
695 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
696 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
697 int c
= 'A' + i
- A_AXIS
;
698 if(gcode
->has_letter(c
)) {
699 float v
= gcode
->get_value(c
);
700 actuators
[i
]->set_max_rate(v
);
705 if(gcode
->has_letter('S')) max_speed
= gcode
->get_value('S');
709 // this format is deprecated
710 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
711 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
712 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
713 if (gcode
->has_letter('A' + i
)) {
714 float v
= gcode
->get_value('A'+i
);
715 actuators
[i
]->set_max_rate(v
);
720 if(gcode
->subcode
== 1) check_max_actuator_speeds();
724 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
725 if (gcode
->has_letter('S')) {
726 float acc
= gcode
->get_value('S'); // mm/s^2
728 if (acc
< 1.0F
) acc
= 1.0F
;
729 this->default_acceleration
= acc
;
731 for (int i
= 0; i
< n_motors
; ++i
) {
732 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
733 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
734 if(gcode
->has_letter(axis
)) {
735 float acc
= gcode
->get_value(axis
); // mm/s^2
737 if (acc
<= 0.0F
) acc
= NAN
;
738 actuators
[i
]->set_acceleration(acc
);
743 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
744 if (gcode
->has_letter('X')) {
745 float jd
= gcode
->get_value('X');
749 THEKERNEL
->planner
->junction_deviation
= jd
;
751 if (gcode
->has_letter('Z')) {
752 float jd
= gcode
->get_value('Z');
753 // enforce minimum, -1 disables it and uses regular junction deviation
756 THEKERNEL
->planner
->z_junction_deviation
= jd
;
758 if (gcode
->has_letter('S')) {
759 float mps
= gcode
->get_value('S');
763 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
767 case 211: // M211 Sn turns soft endstops on/off
768 if(gcode
->has_letter('S')) {
769 soft_endstop_enabled
= gcode
->get_uint('S') == 1;
771 gcode
->stream
->printf("Soft endstops are %s", soft_endstop_enabled
? "Enabled" : "Disabled");
772 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
773 if(isnan(soft_endstop_min
[i
])) {
774 gcode
->stream
->printf(",%c min is disabled", 'X'+i
);
776 if(isnan(soft_endstop_max
[i
])) {
777 gcode
->stream
->printf(",%c max is disabled", 'X'+i
);
780 gcode
->stream
->printf(",%c axis is not homed", 'X'+i
);
783 gcode
->stream
->printf("\n");
787 case 220: // M220 - speed override percentage
788 if (gcode
->has_letter('S')) {
789 float factor
= gcode
->get_value('S');
790 // enforce minimum 10% speed
793 // enforce maximum 10x speed
794 if (factor
> 1000.0F
)
797 seconds_per_minute
= 6000.0F
/ factor
;
799 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
803 case 400: // wait until all moves are done up to this point
804 THEKERNEL
->conveyor
->wait_for_idle();
807 case 500: // M500 saves some volatile settings to config override file
808 case 503: { // M503 just prints the settings
809 gcode
->stream
->printf(";Steps per unit:\nM92 ");
810 for (int i
= 0; i
< n_motors
; ++i
) {
811 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
812 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
813 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
815 gcode
->stream
->printf("\n");
817 // only print if not NAN
818 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
819 for (int i
= 0; i
< n_motors
; ++i
) {
820 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
821 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
822 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
824 gcode
->stream
->printf("\n");
826 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
);
828 gcode
->stream
->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f S%1.5f\n", this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
], this->max_speed
);
830 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
831 for (int i
= 0; i
< n_motors
; ++i
) {
832 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
833 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
834 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
836 gcode
->stream
->printf("\n");
838 // get or save any arm solution specific optional values
839 BaseSolution::arm_options_t options
;
840 if(arm_solution
->get_optional(options
) && !options
.empty()) {
841 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
842 for(auto &i
: options
) {
843 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
845 gcode
->stream
->printf("\n");
848 // save wcs_offsets and current_wcs
849 // TODO this may need to be done whenever they change to be compliant
850 gcode
->stream
->printf(";WCS settings\n");
851 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
853 for(auto &i
: wcs_offsets
) {
854 if(i
!= wcs_t(0, 0, 0)) {
856 std::tie(x
, y
, z
) = i
;
857 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
862 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
863 // 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
864 if(g92_offset
!= wcs_t(0, 0, 0)) {
866 std::tie(x
, y
, z
) = g92_offset
;
867 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
873 case 665: { // M665 set optional arm solution variables based on arm solution.
874 // the parameter args could be any letter each arm solution only accepts certain ones
875 BaseSolution::arm_options_t options
= gcode
->get_args();
876 options
.erase('S'); // don't include the S
877 options
.erase('U'); // don't include the U
878 if(options
.size() > 0) {
879 // set the specified options
880 arm_solution
->set_optional(options
);
883 if(arm_solution
->get_optional(options
)) {
884 // foreach optional value
885 for(auto &i
: options
) {
886 // print all current values of supported options
887 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
888 gcode
->add_nl
= true;
892 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
893 this->delta_segments_per_second
= gcode
->get_value('S');
894 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
896 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
897 this->mm_per_line_segment
= gcode
->get_value('U');
898 this->delta_segments_per_second
= 0;
899 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
907 if( motion_mode
!= NONE
) {
908 is_g123
= motion_mode
!= SEEK
;
909 process_move(gcode
, motion_mode
);
915 next_command_is_MCS
= false; // must be on same line as G0 or G1
918 int Robot::get_active_extruder() const
920 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
921 // find first selected extruder
922 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
927 // process a G0/G1/G2/G3
928 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
930 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
931 // get XYZ and one E (which goes to the selected extruder)
932 float param
[4]{NAN
, NAN
, NAN
, NAN
};
934 // process primary axis
935 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
937 if( gcode
->has_letter(letter
) ) {
938 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
942 float offset
[3]{0,0,0};
943 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
944 if( gcode
->has_letter(letter
) ) {
945 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
949 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
950 float target
[n_motors
];
951 memcpy(target
, machine_position
, n_motors
*sizeof(float));
953 if(!next_command_is_MCS
) {
954 if(this->absolute_mode
) {
955 // apply wcs offsets and g92 offset and tool offset
956 if(!isnan(param
[X_AXIS
])) {
957 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
);
960 if(!isnan(param
[Y_AXIS
])) {
961 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
);
964 if(!isnan(param
[Z_AXIS
])) {
965 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
);
969 // they are deltas from the machine_position if specified
970 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
971 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
976 // already in machine coordinates, we do not add wcs or tool offset for that
977 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
978 if(!isnan(param
[i
])) target
[i
] = param
[i
];
984 #if MAX_ROBOT_ACTUATORS > 3
985 // process extruder parameters, for active extruder only (only one active extruder at a time)
986 int selected_extruder
= 0;
987 if(gcode
->has_letter('E')) {
988 selected_extruder
= get_active_extruder();
989 param
[E_AXIS
]= gcode
->get_value('E');
992 // do E for the selected extruder
993 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
994 if(this->e_absolute_mode
) {
995 target
[selected_extruder
]= param
[E_AXIS
];
996 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
998 delta_e
= param
[E_AXIS
];
999 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
1003 // process ABC axis, this is mutually exclusive to using E for an extruder, so if E is used and A then the results are undefined
1004 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
1005 char letter
= 'A'+i
-A_AXIS
;
1006 if(gcode
->has_letter(letter
)) {
1007 float p
= gcode
->get_value(letter
);
1008 if(this->absolute_mode
) {
1011 target
[i
]= p
+ machine_position
[i
];
1017 if( gcode
->has_letter('F') ) {
1018 if( motion_mode
== SEEK
)
1019 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
1021 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
1024 // S is modal When specified on a G0/1/2/3 command
1025 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
1029 // Perform any physical actions
1030 switch(motion_mode
) {
1034 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
1038 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
1043 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
1044 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1048 // needed to act as start of next arc command
1049 memcpy(arc_milestone
, target
, sizeof(arc_milestone
));
1052 // set machine_position to the calculated target
1053 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1057 // reset the machine position for all axis. Used for homing.
1058 // after homing we supply the cartesian coordinates that the head is at when homed,
1059 // however for Z this is the compensated machine position (if enabled)
1060 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1061 // this will make the results from M114 and ? consistent after homing.
1062 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1063 void Robot::reset_axis_position(float x
, float y
, float z
)
1065 // set both the same initially
1066 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1067 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1068 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1070 if(compensationTransform
) {
1071 // apply inverse transform to get machine_position
1072 compensationTransform(machine_position
, true);
1075 // now set the actuator positions based on the supplied compensated position
1076 ActuatorCoordinates actuator_pos
;
1077 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1078 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1079 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1082 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1083 void Robot::reset_axis_position(float position
, int axis
)
1085 compensated_machine_position
[axis
] = position
;
1086 if(axis
<= Z_AXIS
) {
1087 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1089 #if MAX_ROBOT_ACTUATORS > 3
1090 }else if(axis
< n_motors
) {
1091 // ABC and/or extruders need to be set as there is no arm solution for them
1092 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1093 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1098 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1099 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1100 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1102 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1103 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1106 // now correct axis positions then recorrect actuator to account for rounding
1107 reset_position_from_current_actuator_position();
1110 // Use FK to find out where actuator is and reset to match
1111 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1112 void Robot::reset_position_from_current_actuator_position()
1114 ActuatorCoordinates actuator_pos
;
1115 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1116 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1117 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1120 // discover machine position from where actuators actually are
1121 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1122 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1124 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1125 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1127 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1128 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1129 // to get everything in perfect sync.
1130 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1131 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1132 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1135 // Handle extruders and/or ABC axis
1136 #if MAX_ROBOT_ACTUATORS > 3
1137 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1138 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1139 float ap
= actuator_pos
[i
];
1140 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
1141 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1142 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1147 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1148 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1149 // all transforms and is what we actually convert to actuator positions
1150 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1152 float deltas
[n_motors
];
1153 float transformed_target
[n_motors
]; // adjust target for bed compensation
1154 float unit_vec
[N_PRIMARY_AXIS
];
1156 // unity transform by default
1157 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1159 // check function pointer and call if set to transform the target to compensate for bed
1160 if(compensationTransform
) {
1161 // some compensation strategies can transform XYZ, some just change Z
1162 compensationTransform(transformed_target
, false);
1165 // check soft endstops only for homed axis that are enabled
1166 if(soft_endstop_enabled
) {
1167 for (int i
= 0; i
<= Z_AXIS
; ++i
) {
1168 if(!is_homed(i
)) continue;
1169 if( (!isnan(soft_endstop_min
[i
]) && transformed_target
[i
] < soft_endstop_min
[i
]) || (!isnan(soft_endstop_max
[i
]) && transformed_target
[i
] > soft_endstop_max
[i
]) ) {
1170 if(soft_endstop_halt
) {
1171 if(THEKERNEL
->is_grbl_mode()) {
1172 THEKERNEL
->streams
->printf("error:");
1174 THEKERNEL
->streams
->printf("Error: ");
1177 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i
+'X');
1178 THEKERNEL
->call_event(ON_HALT
, nullptr);
1181 //} else if(soft_endstop_truncate) {
1182 // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
1183 // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
1187 if(THEKERNEL
->is_grbl_mode()) {
1188 THEKERNEL
->streams
->printf("error:");
1190 THEKERNEL
->streams
->printf("Error: ");
1192 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - entire move ignored\n", i
+'X');
1201 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1203 // find distance moved by each axis, use transformed target from the current compensated machine position
1204 for (size_t i
= 0; i
< n_motors
; i
++) {
1205 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1206 if(deltas
[i
] == 0) continue;
1207 // at least one non zero delta
1209 if(i
< N_PRIMARY_AXIS
) {
1210 sos
+= powf(deltas
[i
], 2);
1215 if(!move
) return false;
1217 // see if this is a primary axis move or not
1218 bool auxilliary_move
= true;
1219 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1220 if(deltas
[i
] != 0) {
1221 auxilliary_move
= false;
1226 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1227 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1229 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1230 // 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
1231 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1233 if(!auxilliary_move
) {
1234 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1235 // find distance unit vector for primary axis only
1236 unit_vec
[i
] = deltas
[i
] / distance
;
1238 // Do not move faster than the configured cartesian limits for XYZ
1239 if ( i
<= Z_AXIS
&& max_speeds
[i
] > 0 ) {
1240 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1242 if (axis_speed
> max_speeds
[i
])
1243 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1247 if(this->max_speed
> 0 && rate_mm_s
> this->max_speed
) {
1248 rate_mm_s
= this->max_speed
;
1252 // find actuator position given the machine position, use actual adjusted target
1253 ActuatorCoordinates actuator_pos
;
1254 if(!disable_arm_solution
) {
1255 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1258 // basically the same as cartesian, would be used for special homing situations like for scara
1259 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1260 actuator_pos
[i
] = transformed_target
[i
];
1264 #if MAX_ROBOT_ACTUATORS > 3
1266 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1267 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1268 actuator_pos
[i
]= transformed_target
[i
];
1269 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1270 // NOTE this relies on the fact only one extruder is active at a time
1271 // scale for volumetric or flow rate
1272 // TODO is this correct? scaling the absolute target? what if the scale changes?
1273 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1274 actuator_pos
[i
] *= get_e_scale_fnc();
1276 if(auxilliary_move
) {
1277 // for E only moves we need to use the scaled E to calculate the distance
1278 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1281 if(auxilliary_move
) {
1282 distance
= sqrtf(sos
); // distance in mm of the e move
1283 if(distance
< 0.00001F
) return false;
1287 // use default acceleration to start with
1288 float acceleration
= default_acceleration
;
1290 float isecs
= rate_mm_s
/ distance
;
1292 // check per-actuator speed limits
1293 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1294 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1295 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1297 float actuator_rate
= d
* isecs
;
1298 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1299 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1300 isecs
= rate_mm_s
/ distance
;
1303 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1304 // 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.
1305 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1306 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1307 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1308 float ca
= fabsf((d
/distance
) * acceleration
);
1310 acceleration
*= ( ma
/ ca
);
1316 // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
1317 while(THEKERNEL
->get_feed_hold()) {
1318 THEKERNEL
->call_event(ON_IDLE
, this);
1319 // if we also got a HALT then break out of this
1320 if(THEKERNEL
->is_halted()) return false;
1323 // Append the block to the planner
1324 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1325 // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
1326 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1327 // this is the new compensated machine position
1328 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1336 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1337 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1339 if(THEKERNEL
->is_halted()) return false;
1341 // catch negative or zero feed rates
1342 if(rate_mm_s
<= 0.0F
) {
1346 // get the absolute target position, default is current machine_position
1347 float target
[n_motors
];
1348 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1350 // add in the deltas to get new target
1351 for (int i
= 0; i
< naxis
; i
++) {
1352 target
[i
] += delta
[i
];
1355 // submit for planning and if moved update machine_position
1356 if(append_milestone(target
, rate_mm_s
)) {
1357 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1364 // Append a move to the queue ( cutting it into segments if needed )
1365 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1367 // catch negative or zero feed rates and return the same error as GRBL does
1368 if(rate_mm_s
<= 0.0F
) {
1369 gcode
->is_error
= true;
1370 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1374 // Find out the distance for this move in XYZ in MCS
1375 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 ));
1377 if(millimeters_of_travel
< 0.00001F
) {
1378 // we have no movement in XYZ, probably E only extrude or retract
1379 return this->append_milestone(target
, rate_mm_s
);
1383 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1384 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1385 We ask Extruder to do all the work but we need to pass in the relevant data.
1386 NOTE we need to do this before we segment the line (for deltas)
1388 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1389 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1390 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1391 rate_mm_s
*= data
[1]; // adjust the feedrate
1395 // 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.
1396 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1397 // 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
1400 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1403 } else if(this->delta_segments_per_second
> 1.0F
) {
1404 // enabled if set to something > 1, it is set to 0.0 by default
1405 // segment based on current speed and requested segments per second
1406 // the faster the travel speed the fewer segments needed
1407 // NOTE rate is mm/sec and we take into account any speed override
1408 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1409 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1410 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1413 if(this->mm_per_line_segment
== 0.0F
) {
1414 segments
= 1; // don't split it up
1416 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1422 // A vector to keep track of the endpoint of each segment
1423 float segment_delta
[n_motors
];
1424 float segment_end
[n_motors
];
1425 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1427 // How far do we move each segment?
1428 for (int i
= 0; i
< n_motors
; i
++)
1429 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1431 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1432 // We always add another point after this loop so we stop at segments-1, ie i < segments
1433 for (int i
= 1; i
< segments
; i
++) {
1434 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1435 for (int j
= 0; j
< n_motors
; j
++)
1436 segment_end
[j
] += segment_delta
[j
];
1438 // Append the end of this segment to the queue
1439 // this can block waiting for free block queue or if in feed hold
1440 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1445 // Append the end of this full move to the queue
1446 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1448 this->next_command_is_MCS
= false; // always reset this
1454 // Append an arc to the queue ( cutting it into segments as needed )
1455 // TODO does not support any E parameters so cannot be used for 3D printing.
1456 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1458 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1459 // catch negative or zero feed rates and return the same error as GRBL does
1460 if(rate_mm_s
<= 0.0F
) {
1461 gcode
->is_error
= true;
1462 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1467 // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
1468 float center_axis0
= this->arc_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1469 float center_axis1
= this->arc_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1470 float linear_travel
= target
[this->plane_axis_2
] - this->arc_milestone
[this->plane_axis_2
];
1471 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to start position
1472 float r_axis1
= -offset
[this->plane_axis_1
];
1473 float rt_axis0
= target
[this->plane_axis_0
] - this->arc_milestone
[this->plane_axis_0
] - offset
[this->plane_axis_0
]; // Radius vector from center to target position
1474 float rt_axis1
= target
[this->plane_axis_1
] - this->arc_milestone
[this->plane_axis_1
] - offset
[this->plane_axis_1
];
1475 float angular_travel
= 0;
1476 //check for condition where atan2 formula will fail due to everything canceling out exactly
1477 if((this->arc_milestone
[this->plane_axis_0
]==target
[this->plane_axis_0
]) && (this->arc_milestone
[this->plane_axis_1
]==target
[this->plane_axis_1
])) {
1478 if (is_clockwise
) { // set angular_travel to -2pi for a clockwise full circle
1479 angular_travel
= (-2 * PI
);
1480 } else { // set angular_travel to 2pi for a counterclockwise full circle
1481 angular_travel
= (2 * PI
);
1484 // Patch from GRBL Firmware - Christoph Baumann 04072015
1485 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1486 // Only run if not a full circle or angular travel will incorrectly result in 0.0f
1487 angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1488 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is reverse of other 2 planes
1489 if (is_clockwise
) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
1490 if (angular_travel
> 0) { angular_travel
-= (2 * PI
); }
1491 } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
1492 if (angular_travel
< 0) { angular_travel
+= (2 * PI
); }
1496 // Find the distance for this gcode
1497 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1499 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1500 if( millimeters_of_travel
< 0.000001F
) {
1504 // limit segments by maximum arc error
1505 float arc_segment
= this->mm_per_arc_segment
;
1506 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1507 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1508 if (this->mm_per_arc_segment
< min_err_segment
) {
1509 arc_segment
= min_err_segment
;
1513 // catch fall through on above
1514 if(arc_segment
< 0.0001F
) {
1515 arc_segment
= 0.5F
; /// the old default, so we avoid the divide by zero
1518 // Figure out how many segments for this gcode
1519 // 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
1520 uint16_t segments
= floorf(millimeters_of_travel
/ arc_segment
);
1524 float theta_per_segment
= angular_travel
/ segments
;
1525 float linear_per_segment
= linear_travel
/ segments
;
1527 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1528 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1529 r_T = [cos(phi) -sin(phi);
1530 sin(phi) cos(phi] * r ;
1531 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1532 defined from the circle center to the initial position. Each line segment is formed by successive
1533 vector rotations. This requires only two cos() and sin() computations to form the rotation
1534 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1535 all float numbers are single precision on the Arduino. (True float precision will not have
1536 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1537 tool precision in some cases. Therefore, arc path correction is implemented.
1539 Small angle approximation may be used to reduce computation overhead further. This approximation
1540 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1541 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1542 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1543 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1544 issue for CNC machines with the single precision Arduino calculations.
1545 This approximation also allows mc_arc to immediately insert a line segment into the planner
1546 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1547 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1548 This is important when there are successive arc motions.
1550 // Vector rotation matrix values
1551 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1552 float sin_T
= theta_per_segment
;
1554 // TODO we need to handle the ABC axis here by segmenting them
1555 float arc_target
[n_motors
];
1562 // init array for all axis
1563 memcpy(arc_target
, machine_position
, n_motors
*sizeof(float));
1565 // Initialize the linear axis
1566 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1568 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1569 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1571 if (count
< this->arc_correction
) {
1572 // Apply vector rotation matrix
1573 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1574 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1578 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1579 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1580 cos_Ti
= cosf(i
* theta_per_segment
);
1581 sin_Ti
= sinf(i
* theta_per_segment
);
1582 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1583 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1587 // Update arc_target location
1588 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1589 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1590 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1592 // Append this segment to the queue
1593 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1598 // Ensure last segment arrives at target location.
1599 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1604 // Do the math for an arc and add it to the queue
1605 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1609 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1611 // Set clockwise/counter-clockwise sign for mc_arc computations
1612 bool is_clockwise
= false;
1613 if( motion_mode
== CW_ARC
) {
1614 is_clockwise
= true;
1618 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1622 float Robot::theta(float x
, float y
)
1624 float t
= atanf(x
/ fabs(y
));
1636 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1638 this->plane_axis_0
= axis_0
;
1639 this->plane_axis_1
= axis_1
;
1640 this->plane_axis_2
= axis_2
;
1643 void Robot::clearToolOffset()
1645 this->tool_offset
= wcs_t(0,0,0);
1648 void Robot::setToolOffset(const float offset
[3])
1650 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1653 float Robot::get_feed_rate() const
1655 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;
1658 bool Robot::is_homed(uint8_t i
) const
1660 if(i
>= 3) return false; // safety
1662 // if we are homing we ignore soft endstops so return false
1664 bool ok
= PublicData::get_value(endstops_checksum
, get_homing_status_checksum
, 0, &homing
);
1665 if(!ok
|| homing
) return false;
1667 // check individual axis homing status
1669 ok
= PublicData::get_value(endstops_checksum
, get_homed_status_checksum
, 0, homed
);
1670 if(!ok
) return false;