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 save_g54_checksum CHECKSUM("save_g54")
58 #define set_g92_checksum CHECKSUM("set_g92")
61 #define arm_solution_checksum CHECKSUM("arm_solution")
62 #define cartesian_checksum CHECKSUM("cartesian")
63 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
64 #define rostock_checksum CHECKSUM("rostock")
65 #define linear_delta_checksum CHECKSUM("linear_delta")
66 #define rotary_delta_checksum CHECKSUM("rotary_delta")
67 #define delta_checksum CHECKSUM("delta")
68 #define hbot_checksum CHECKSUM("hbot")
69 #define corexy_checksum CHECKSUM("corexy")
70 #define corexz_checksum CHECKSUM("corexz")
71 #define kossel_checksum CHECKSUM("kossel")
72 #define morgan_checksum CHECKSUM("morgan")
74 // new-style actuator stuff
75 #define actuator_checksum CHEKCSUM("actuator")
77 #define step_pin_checksum CHECKSUM("step_pin")
78 #define dir_pin_checksum CHEKCSUM("dir_pin")
79 #define en_pin_checksum CHECKSUM("en_pin")
81 #define max_speed_checksum CHECKSUM("max_speed")
82 #define acceleration_checksum CHECKSUM("acceleration")
83 #define z_acceleration_checksum CHECKSUM("z_acceleration")
85 #define alpha_checksum CHECKSUM("alpha")
86 #define beta_checksum CHECKSUM("beta")
87 #define gamma_checksum CHECKSUM("gamma")
89 #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
91 #define enable_checksum CHECKSUM("enable")
92 #define halt_checksum CHECKSUM("halt")
93 #define soft_endstop_checksum CHECKSUM("soft_endstop")
94 #define xmin_checksum CHECKSUM("x_min")
95 #define ymin_checksum CHECKSUM("y_min")
96 #define zmin_checksum CHECKSUM("z_min")
97 #define xmax_checksum CHECKSUM("x_max")
98 #define ymax_checksum CHECKSUM("y_max")
99 #define zmax_checksum CHECKSUM("z_max")
101 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
103 // 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
104 // It takes care of cutting arcs into segments, same thing for line that are too long
108 this->inch_mode
= false;
109 this->absolute_mode
= true;
110 this->e_absolute_mode
= true;
111 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
112 memset(this->machine_position
, 0, sizeof machine_position
);
113 memset(this->compensated_machine_position
, 0, sizeof compensated_machine_position
);
114 this->arm_solution
= NULL
;
115 seconds_per_minute
= 60.0F
;
116 this->clearToolOffset();
117 this->compensationTransform
= nullptr;
118 this->get_e_scale_fnc
= nullptr;
119 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
120 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
121 this->next_command_is_MCS
= false;
122 this->disable_segmentation
= false;
123 this->disable_arm_solution
= false;
127 //Called when the module has just been loaded
128 void Robot::on_module_loaded()
130 this->register_for_event(ON_GCODE_RECEIVED
);
136 #define ACTUATOR_CHECKSUMS(X) { \
137 CHECKSUM(X "_step_pin"), \
138 CHECKSUM(X "_dir_pin"), \
139 CHECKSUM(X "_en_pin"), \
140 CHECKSUM(X "_steps_per_mm"), \
141 CHECKSUM(X "_max_rate"), \
142 CHECKSUM(X "_acceleration") \
145 void Robot::load_config()
147 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
148 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
149 // To make adding those solution easier, they have their own, separate object.
150 // Here we read the config to find out which arm solution to use
151 if (this->arm_solution
) delete this->arm_solution
;
152 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
153 // Note checksums are not const expressions when in debug mode, so don't use switch
154 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
155 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
157 } else if(solution_checksum
== corexz_checksum
) {
158 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
160 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
161 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
163 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
164 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
166 } else if(solution_checksum
== rotary_delta_checksum
) {
167 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
169 } else if(solution_checksum
== morgan_checksum
) {
170 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
172 } else if(solution_checksum
== cartesian_checksum
) {
173 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
176 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
179 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
180 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
181 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
182 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
183 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
184 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
185 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
187 // in mm/sec but specified in config as mm/min
188 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
189 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
190 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
191 this->max_speed
= THEKERNEL
->config
->value(max_speed_checksum
)->by_default( -60.0F
)->as_number() / 60.0F
;
193 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
194 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
195 this->save_g54
= THEKERNEL
->config
->value(save_g54_checksum
)->by_default(THEKERNEL
->is_grbl_mode())->as_bool();
196 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
198 // optional setting for a fixed G92 offset
199 std::vector
<float> t
= parse_number_list(g92
.c_str());
201 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
205 // default s value for laser
206 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
208 // Make our Primary XYZ StepperMotors, and potentially A B C
209 uint16_t const motor_checksums
[][6] = {
210 ACTUATOR_CHECKSUMS("alpha"), // X
211 ACTUATOR_CHECKSUMS("beta"), // Y
212 ACTUATOR_CHECKSUMS("gamma"), // Z
213 #if MAX_ROBOT_ACTUATORS > 3
214 ACTUATOR_CHECKSUMS("delta"), // A
215 #if MAX_ROBOT_ACTUATORS > 4
216 ACTUATOR_CHECKSUMS("epsilon"), // B
217 #if MAX_ROBOT_ACTUATORS > 5
218 ACTUATOR_CHECKSUMS("zeta") // C
224 // default acceleration setting, can be overriden with newer per axis settings
225 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
228 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
229 Pin pins
[3]; //step, dir, enable
230 for (size_t i
= 0; i
< 3; i
++) {
231 pins
[i
].from_string(THEKERNEL
->config
->value(motor_checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
234 if(!pins
[0].connected() || !pins
[1].connected()) { // step and dir must be defined, but enable is optional
236 THEKERNEL
->streams
->printf("FATAL: motor %c is not defined in config\n", 'X'+a
);
237 n_motors
= a
; // we only have this number of motors
240 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
243 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
244 // register this motor (NB This must be 0,1,2) of the actuators array
245 uint8_t n
= register_motor(sm
);
247 // this is a fatal error
248 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
252 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(motor_checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
253 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
254 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(motor_checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
257 check_max_actuator_speeds(); // check the configs are sane
259 // if we have not specified a z acceleration see if the legacy config was set
260 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
261 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
263 actuators
[Z_AXIS
]->set_acceleration(acc
);
267 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
268 // so the first move can be correct if homing is not performed
269 ActuatorCoordinates actuator_pos
;
270 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
271 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
272 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
275 // initialize any extra axis to machine position
276 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
277 actuators
[i
]->change_last_milestone(machine_position
[i
]);
280 //this->clearToolOffset();
282 soft_endstop_enabled
= THEKERNEL
->config
->value(soft_endstop_checksum
, enable_checksum
)->by_default(false)->as_bool();
283 soft_endstop_halt
= THEKERNEL
->config
->value(soft_endstop_checksum
, halt_checksum
)->by_default(true)->as_bool();
285 soft_endstop_min
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmin_checksum
)->by_default(NAN
)->as_number();
286 soft_endstop_min
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymin_checksum
)->by_default(NAN
)->as_number();
287 soft_endstop_min
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmin_checksum
)->by_default(NAN
)->as_number();
288 soft_endstop_max
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmax_checksum
)->by_default(NAN
)->as_number();
289 soft_endstop_max
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymax_checksum
)->by_default(NAN
)->as_number();
290 soft_endstop_max
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmax_checksum
)->by_default(NAN
)->as_number();
293 uint8_t Robot::register_motor(StepperMotor
*motor
)
295 // register this motor with the step ticker
296 THEKERNEL
->step_ticker
->register_motor(motor
);
297 if(n_motors
>= k_max_actuators
) {
298 // this is a fatal error
299 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
302 actuators
.push_back(motor
);
303 motor
->set_motor_id(n_motors
);
307 void Robot::push_state()
309 bool am
= this->absolute_mode
;
310 bool em
= this->e_absolute_mode
;
311 bool im
= this->inch_mode
;
312 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
316 void Robot::pop_state()
318 if(!state_stack
.empty()) {
319 auto s
= state_stack
.top();
321 this->feed_rate
= std::get
<0>(s
);
322 this->seek_rate
= std::get
<1>(s
);
323 this->absolute_mode
= std::get
<2>(s
);
324 this->e_absolute_mode
= std::get
<3>(s
);
325 this->inch_mode
= std::get
<4>(s
);
326 this->current_wcs
= std::get
<5>(s
);
330 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
332 std::vector
<wcs_t
> v
;
333 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
334 for(auto& i
: wcs_offsets
) {
337 v
.push_back(g92_offset
);
338 v
.push_back(tool_offset
);
342 void Robot::get_current_machine_position(float *pos
) const
344 // get real time current actuator position in mm
345 ActuatorCoordinates current_position
{
346 actuators
[X_AXIS
]->get_current_position(),
347 actuators
[Y_AXIS
]->get_current_position(),
348 actuators
[Z_AXIS
]->get_current_position()
351 // get machine position from the actuator position using FK
352 arm_solution
->actuator_to_cartesian(current_position
, pos
);
355 void Robot::print_position(uint8_t subcode
, std::string
& res
, bool ignore_extruders
) const
357 // M114.1 is a new way to do this (similar to how GRBL does it).
358 // it returns the realtime position based on the current step position of the actuators.
359 // this does require a FK to get a machine position from the actuator position
360 // and then invert all the transforms to get a workspace position from machine position
361 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
364 if(subcode
== 0) { // M114 print WCS
365 wcs_t pos
= mcs2wcs(machine_position
);
366 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
)));
368 } else if(subcode
== 4) {
369 // M114.4 print last milestone
370 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
]);
372 } else if(subcode
== 5) {
373 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
374 // will differ from LMS by the compensation at the current position otherwise
375 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
]);
378 // get real time positions
380 get_current_machine_position(mpos
);
382 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
383 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
385 if(subcode
== 1) { // M114.1 print realtime WCS
386 wcs_t pos
= mcs2wcs(mpos
);
387 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
)));
389 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
390 n
= snprintf(buf
, sizeof(buf
), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
392 } else if(subcode
== 3) { // M114.3 print realtime actuator position
393 // get real time current actuator position in mm
394 ActuatorCoordinates current_position
{
395 actuators
[X_AXIS
]->get_current_position(),
396 actuators
[Y_AXIS
]->get_current_position(),
397 actuators
[Z_AXIS
]->get_current_position()
399 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
]);
403 if(n
> sizeof(buf
)) n
= sizeof(buf
);
406 #if MAX_ROBOT_ACTUATORS > 3
407 // deal with the ABC axis
408 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
410 if(ignore_extruders
&& actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
411 if(subcode
== 4) { // M114.4 print last milestone
412 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
414 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
415 // current actuator position
416 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
418 if(n
> sizeof(buf
)) n
= sizeof(buf
);
419 if(n
> 0) res
.append(buf
, n
);
424 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
425 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
427 return std::make_tuple(
428 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
),
429 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
),
430 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
)
434 // this does a sanity check that actuator speeds do not exceed steps rate capability
435 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
436 void Robot::check_max_actuator_speeds()
438 for (size_t i
= 0; i
< n_motors
; i
++) {
439 if(actuators
[i
]->is_extruder()) continue; //extruders are not included in this check
441 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
442 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
443 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
444 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());
449 //A GCode has been received
450 //See if the current Gcode line has some orders for us
451 void Robot::on_gcode_received(void *argument
)
453 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
455 enum MOTION_MODE_T motion_mode
= NONE
;
459 case 0: motion_mode
= SEEK
; break;
460 case 1: motion_mode
= LINEAR
; break;
461 case 2: motion_mode
= CW_ARC
; break;
462 case 3: motion_mode
= CCW_ARC
; break;
463 case 4: { // G4 Dwell
464 uint32_t delay_ms
= 0;
465 if (gcode
->has_letter('P')) {
466 if(THEKERNEL
->is_grbl_mode()) {
467 // in grbl mode (and linuxcnc) P is decimal seconds
468 float f
= gcode
->get_value('P');
469 delay_ms
= f
* 1000.0F
;
472 // in reprap P is milliseconds, they always have to be different!
473 delay_ms
= gcode
->get_int('P');
476 if (gcode
->has_letter('S')) {
477 delay_ms
+= gcode
->get_int('S') * 1000;
481 THEKERNEL
->conveyor
->wait_for_idle();
482 // wait for specified time
483 uint32_t start
= us_ticker_read(); // mbed call
484 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
485 THEKERNEL
->call_event(ON_IDLE
, this);
486 if(THEKERNEL
->is_halted()) return;
492 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
493 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
494 size_t n
= gcode
->get_uint('P');
495 if(n
== 0) n
= current_wcs
; // set current coordinate system
499 std::tie(x
, y
, z
) = wcs_offsets
[n
];
500 if(gcode
->get_int('L') == 20) {
501 // this makes the current machine position (less compensation transform) the offset
502 // get current position in WCS
503 wcs_t pos
= mcs2wcs(machine_position
);
505 if(gcode
->has_letter('X')){
506 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
509 if(gcode
->has_letter('Y')){
510 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
512 if(gcode
->has_letter('Z')) {
513 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
518 // the value is the offset from machine zero
519 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
520 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
521 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
523 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
524 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
525 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
528 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
533 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
534 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
535 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
536 case 20: this->inch_mode
= true; break;
537 case 21: this->inch_mode
= false; break;
539 case 54: case 55: case 56: case 57: case 58: case 59:
540 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
541 current_wcs
= gcode
->g
- 54;
542 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
543 current_wcs
+= gcode
->subcode
;
544 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
548 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
549 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
552 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
553 // reset G92 offsets to 0
554 g92_offset
= wcs_t(0, 0, 0);
556 } else if(gcode
->subcode
== 3) {
557 // initialize G92 to the specified values, only used for saving it with M500
558 float x
= 0, y
= 0, z
= 0;
559 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
560 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
561 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
562 g92_offset
= wcs_t(x
, y
, z
);
565 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
567 std::tie(x
, y
, z
) = g92_offset
;
568 // get current position in WCS
569 wcs_t pos
= mcs2wcs(machine_position
);
571 // adjust g92 offset to make the current wpos == the value requested
572 if(gcode
->has_letter('X')){
573 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
575 if(gcode
->has_letter('Y')){
576 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
578 if(gcode
->has_letter('Z')) {
579 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
581 g92_offset
= wcs_t(x
, y
, z
);
584 #if MAX_ROBOT_ACTUATORS > 3
585 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
586 // reset the E position, legacy for 3d Printers to be reprap compatible
587 // find the selected extruder
588 int selected_extruder
= get_active_extruder();
589 if(selected_extruder
> 0) {
590 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
591 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
592 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
601 } else if( gcode
->has_m
) {
603 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
604 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
607 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
608 if(!THEKERNEL
->is_grbl_mode()) break;
609 // fall through to M2
610 case 2: // M2 end of program
612 absolute_mode
= true;
615 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
618 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
619 if(gcode
->get_num_args() > 0) {
620 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
622 for (int i
= 0; i
< n_motors
; ++i
) {
623 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
624 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
626 // handle E parameter as currently selected extruder ABC
627 if(gcode
->has_letter('E')) {
628 // find first selected extruder
629 int i
= get_active_extruder();
631 bm
|= (0x02<<i
); // set appropriate bit
635 THEKERNEL
->conveyor
->wait_for_idle();
636 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
641 THEKERNEL
->conveyor
->wait_for_idle();
642 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
645 case 82: e_absolute_mode
= true; break;
646 case 83: e_absolute_mode
= false; break;
648 case 92: // M92 - set steps per mm
649 for (int i
= 0; i
< n_motors
; ++i
) {
650 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
651 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
652 if(gcode
->has_letter(axis
)) {
653 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
655 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
657 gcode
->add_nl
= true;
658 check_max_actuator_speeds();
663 print_position(gcode
->subcode
, buf
, true); // ignore extruders as they will print E themselves
664 gcode
->txt_after_ok
.append(buf
);
668 case 120: // push state
672 case 121: // pop state
676 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
677 if(gcode
->get_num_args() == 0) {
678 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
679 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
681 if(gcode
->subcode
== 1) {
682 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
683 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
684 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
687 gcode
->stream
->printf(" S: %g ", this->max_speed
);
690 gcode
->add_nl
= true;
693 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
694 if (gcode
->has_letter('X' + i
)) {
695 float v
= gcode
->get_value('X'+i
);
696 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
697 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
701 if(gcode
->subcode
== 1) {
702 // ABC axis only handle actuator max speeds
703 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
704 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
705 int c
= 'A' + i
- A_AXIS
;
706 if(gcode
->has_letter(c
)) {
707 float v
= gcode
->get_value(c
);
708 actuators
[i
]->set_max_rate(v
);
713 if(gcode
->has_letter('S')) max_speed
= gcode
->get_value('S');
717 // this format is deprecated
718 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
719 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
720 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
721 if (gcode
->has_letter('A' + i
)) {
722 float v
= gcode
->get_value('A'+i
);
723 actuators
[i
]->set_max_rate(v
);
728 if(gcode
->subcode
== 1) check_max_actuator_speeds();
732 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
733 if (gcode
->has_letter('S')) {
734 float acc
= gcode
->get_value('S'); // mm/s^2
736 if (acc
< 1.0F
) acc
= 1.0F
;
737 this->default_acceleration
= acc
;
739 for (int i
= 0; i
< n_motors
; ++i
) {
740 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
741 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
742 if(gcode
->has_letter(axis
)) {
743 float acc
= gcode
->get_value(axis
); // mm/s^2
745 if (acc
<= 0.0F
) acc
= NAN
;
746 actuators
[i
]->set_acceleration(acc
);
751 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
752 if (gcode
->has_letter('X')) {
753 float jd
= gcode
->get_value('X');
757 THEKERNEL
->planner
->junction_deviation
= jd
;
759 if (gcode
->has_letter('Z')) {
760 float jd
= gcode
->get_value('Z');
761 // enforce minimum, -1 disables it and uses regular junction deviation
764 THEKERNEL
->planner
->z_junction_deviation
= jd
;
766 if (gcode
->has_letter('S')) {
767 float mps
= gcode
->get_value('S');
771 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
775 case 211: // M211 Sn turns soft endstops on/off
776 if(gcode
->has_letter('S')) {
777 soft_endstop_enabled
= gcode
->get_uint('S') == 1;
779 gcode
->stream
->printf("Soft endstops are %s", soft_endstop_enabled
? "Enabled" : "Disabled");
780 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
781 if(isnan(soft_endstop_min
[i
])) {
782 gcode
->stream
->printf(",%c min is disabled", 'X'+i
);
784 if(isnan(soft_endstop_max
[i
])) {
785 gcode
->stream
->printf(",%c max is disabled", 'X'+i
);
788 gcode
->stream
->printf(",%c axis is not homed", 'X'+i
);
791 gcode
->stream
->printf("\n");
795 case 220: // M220 - speed override percentage
796 if (gcode
->has_letter('S')) {
797 float factor
= gcode
->get_value('S');
798 // enforce minimum 10% speed
801 // enforce maximum 10x speed
802 if (factor
> 1000.0F
)
805 seconds_per_minute
= 6000.0F
/ factor
;
807 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
811 case 400: // wait until all moves are done up to this point
812 THEKERNEL
->conveyor
->wait_for_idle();
815 case 500: // M500 saves some volatile settings to config override file
816 case 503: { // M503 just prints the settings
817 gcode
->stream
->printf(";Steps per unit:\nM92 ");
818 for (int i
= 0; i
< n_motors
; ++i
) {
819 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
820 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
821 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
823 gcode
->stream
->printf("\n");
825 // only print if not NAN
826 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
827 for (int i
= 0; i
< n_motors
; ++i
) {
828 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
829 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
830 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
832 gcode
->stream
->printf("\n");
834 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
);
836 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
);
838 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
839 for (int i
= 0; i
< n_motors
; ++i
) {
840 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
841 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
842 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
844 gcode
->stream
->printf("\n");
846 // get or save any arm solution specific optional values
847 BaseSolution::arm_options_t options
;
848 if(arm_solution
->get_optional(options
) && !options
.empty()) {
849 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
850 for(auto &i
: options
) {
851 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
853 gcode
->stream
->printf("\n");
856 // save wcs_offsets and current_wcs
857 // TODO this may need to be done whenever they change to be compliant
859 gcode
->stream
->printf(";WCS settings\n");
860 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
862 for(auto &i
: wcs_offsets
) {
863 if(i
!= wcs_t(0, 0, 0)) {
865 std::tie(x
, y
, z
) = i
;
866 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
872 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
873 // 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
874 if(g92_offset
!= wcs_t(0, 0, 0)) {
876 std::tie(x
, y
, z
) = g92_offset
;
877 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
883 case 665: { // M665 set optional arm solution variables based on arm solution.
884 // the parameter args could be any letter each arm solution only accepts certain ones
885 BaseSolution::arm_options_t options
= gcode
->get_args();
886 options
.erase('S'); // don't include the S
887 options
.erase('U'); // don't include the U
888 if(options
.size() > 0) {
889 // set the specified options
890 arm_solution
->set_optional(options
);
893 if(arm_solution
->get_optional(options
)) {
894 // foreach optional value
895 for(auto &i
: options
) {
896 // print all current values of supported options
897 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
898 gcode
->add_nl
= true;
902 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
903 this->delta_segments_per_second
= gcode
->get_value('S');
904 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
906 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
907 this->mm_per_line_segment
= gcode
->get_value('U');
908 this->delta_segments_per_second
= 0;
909 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
917 if( motion_mode
!= NONE
) {
918 is_g123
= motion_mode
!= SEEK
;
919 process_move(gcode
, motion_mode
);
925 next_command_is_MCS
= false; // must be on same line as G0 or G1
928 int Robot::get_active_extruder() const
930 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
931 // find first selected extruder
932 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
937 // process a G0/G1/G2/G3
938 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
940 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
941 // get XYZ and one E (which goes to the selected extruder)
942 float param
[4]{NAN
, NAN
, NAN
, NAN
};
944 // process primary axis
945 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
947 if( gcode
->has_letter(letter
) ) {
948 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
952 float offset
[3]{0,0,0};
953 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
954 if( gcode
->has_letter(letter
) ) {
955 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
959 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
960 float target
[n_motors
];
961 memcpy(target
, machine_position
, n_motors
*sizeof(float));
963 if(!next_command_is_MCS
) {
964 if(this->absolute_mode
) {
965 // apply wcs offsets and g92 offset and tool offset
966 if(!isnan(param
[X_AXIS
])) {
967 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
);
970 if(!isnan(param
[Y_AXIS
])) {
971 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
);
974 if(!isnan(param
[Z_AXIS
])) {
975 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
);
979 // they are deltas from the machine_position if specified
980 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
981 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
986 // already in machine coordinates, we do not add wcs or tool offset for that
987 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
988 if(!isnan(param
[i
])) target
[i
] = param
[i
];
994 #if MAX_ROBOT_ACTUATORS > 3
995 // process extruder parameters, for active extruder only (only one active extruder at a time)
996 int selected_extruder
= 0;
997 if(gcode
->has_letter('E')) {
998 selected_extruder
= get_active_extruder();
999 param
[E_AXIS
]= gcode
->get_value('E');
1002 // do E for the selected extruder
1003 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
1004 if(this->e_absolute_mode
) {
1005 target
[selected_extruder
]= param
[E_AXIS
];
1006 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
1008 delta_e
= param
[E_AXIS
];
1009 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
1013 // 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
1014 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
1015 char letter
= 'A'+i
-A_AXIS
;
1016 if(gcode
->has_letter(letter
)) {
1017 float p
= gcode
->get_value(letter
);
1018 if(this->absolute_mode
) {
1021 target
[i
]= p
+ machine_position
[i
];
1027 if( gcode
->has_letter('F') ) {
1028 if( motion_mode
== SEEK
)
1029 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
1031 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
1034 // S is modal When specified on a G0/1/2/3 command
1035 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
1039 // Perform any physical actions
1040 switch(motion_mode
) {
1044 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
1048 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
1053 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
1054 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1058 // needed to act as start of next arc command
1059 memcpy(arc_milestone
, target
, sizeof(arc_milestone
));
1062 // set machine_position to the calculated target
1063 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1067 // reset the machine position for all axis. Used for homing.
1068 // after homing we supply the cartesian coordinates that the head is at when homed,
1069 // however for Z this is the compensated machine position (if enabled)
1070 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1071 // this will make the results from M114 and ? consistent after homing.
1072 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1073 void Robot::reset_axis_position(float x
, float y
, float z
)
1075 // set both the same initially
1076 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1077 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1078 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1080 if(compensationTransform
) {
1081 // apply inverse transform to get machine_position
1082 compensationTransform(machine_position
, true);
1085 // now set the actuator positions based on the supplied compensated position
1086 ActuatorCoordinates actuator_pos
;
1087 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1088 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1089 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1092 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1093 void Robot::reset_axis_position(float position
, int axis
)
1095 compensated_machine_position
[axis
] = position
;
1096 if(axis
<= Z_AXIS
) {
1097 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1099 #if MAX_ROBOT_ACTUATORS > 3
1100 }else if(axis
< n_motors
) {
1101 // ABC and/or extruders need to be set as there is no arm solution for them
1102 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1103 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1108 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1109 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1110 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1112 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1113 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1116 // now correct axis positions then recorrect actuator to account for rounding
1117 reset_position_from_current_actuator_position();
1120 // Use FK to find out where actuator is and reset to match
1121 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1122 void Robot::reset_position_from_current_actuator_position()
1124 ActuatorCoordinates actuator_pos
;
1125 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1126 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1127 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1130 // discover machine position from where actuators actually are
1131 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1132 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1134 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1135 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1137 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1138 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1139 // to get everything in perfect sync.
1140 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1141 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1142 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1145 // Handle extruders and/or ABC axis
1146 #if MAX_ROBOT_ACTUATORS > 3
1147 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1148 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1149 float ap
= actuator_pos
[i
];
1150 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
1151 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1152 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1157 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1158 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1159 // all transforms and is what we actually convert to actuator positions
1160 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1162 float deltas
[n_motors
];
1163 float transformed_target
[n_motors
]; // adjust target for bed compensation
1164 float unit_vec
[N_PRIMARY_AXIS
];
1166 // unity transform by default
1167 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1169 // check function pointer and call if set to transform the target to compensate for bed
1170 if(compensationTransform
) {
1171 // some compensation strategies can transform XYZ, some just change Z
1172 compensationTransform(transformed_target
, false);
1175 // check soft endstops only for homed axis that are enabled
1176 if(soft_endstop_enabled
) {
1177 for (int i
= 0; i
<= Z_AXIS
; ++i
) {
1178 if(!is_homed(i
)) continue;
1179 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
]) ) {
1180 if(soft_endstop_halt
) {
1181 if(THEKERNEL
->is_grbl_mode()) {
1182 THEKERNEL
->streams
->printf("error:");
1184 THEKERNEL
->streams
->printf("Error: ");
1187 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i
+'X');
1188 THEKERNEL
->call_event(ON_HALT
, nullptr);
1191 //} else if(soft_endstop_truncate) {
1192 // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
1193 // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
1197 if(THEKERNEL
->is_grbl_mode()) {
1198 THEKERNEL
->streams
->printf("error:");
1200 THEKERNEL
->streams
->printf("Error: ");
1202 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - entire move ignored\n", i
+'X');
1211 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1213 // find distance moved by each axis, use transformed target from the current compensated machine position
1214 for (size_t i
= 0; i
< n_motors
; i
++) {
1215 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1216 if(deltas
[i
] == 0) continue;
1217 // at least one non zero delta
1219 if(i
< N_PRIMARY_AXIS
) {
1220 sos
+= powf(deltas
[i
], 2);
1225 if(!move
) return false;
1227 // see if this is a primary axis move or not
1228 bool auxilliary_move
= true;
1229 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1230 if(deltas
[i
] != 0) {
1231 auxilliary_move
= false;
1236 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1237 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1239 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1240 // 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
1241 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1243 if(!auxilliary_move
) {
1244 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1245 // find distance unit vector for primary axis only
1246 unit_vec
[i
] = deltas
[i
] / distance
;
1248 // Do not move faster than the configured cartesian limits for XYZ
1249 if ( i
<= Z_AXIS
&& max_speeds
[i
] > 0 ) {
1250 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1252 if (axis_speed
> max_speeds
[i
])
1253 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1257 if(this->max_speed
> 0 && rate_mm_s
> this->max_speed
) {
1258 rate_mm_s
= this->max_speed
;
1262 // find actuator position given the machine position, use actual adjusted target
1263 ActuatorCoordinates actuator_pos
;
1264 if(!disable_arm_solution
) {
1265 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1268 // basically the same as cartesian, would be used for special homing situations like for scara
1269 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1270 actuator_pos
[i
] = transformed_target
[i
];
1274 #if MAX_ROBOT_ACTUATORS > 3
1276 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1277 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1278 actuator_pos
[i
]= transformed_target
[i
];
1279 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1280 // NOTE this relies on the fact only one extruder is active at a time
1281 // scale for volumetric or flow rate
1282 // TODO is this correct? scaling the absolute target? what if the scale changes?
1283 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1284 actuator_pos
[i
] *= get_e_scale_fnc();
1286 if(auxilliary_move
) {
1287 // for E only moves we need to use the scaled E to calculate the distance
1288 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1291 if(auxilliary_move
) {
1292 distance
= sqrtf(sos
); // distance in mm of the e move
1293 if(distance
< 0.00001F
) return false;
1297 // use default acceleration to start with
1298 float acceleration
= default_acceleration
;
1300 float isecs
= rate_mm_s
/ distance
;
1302 // check per-actuator speed limits
1303 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1304 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1305 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1307 float actuator_rate
= d
* isecs
;
1308 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1309 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1310 isecs
= rate_mm_s
/ distance
;
1313 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1314 // 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.
1315 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1316 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1317 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1318 float ca
= fabsf((d
/distance
) * acceleration
);
1320 acceleration
*= ( ma
/ ca
);
1326 // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
1327 while(THEKERNEL
->get_feed_hold()) {
1328 THEKERNEL
->call_event(ON_IDLE
, this);
1329 // if we also got a HALT then break out of this
1330 if(THEKERNEL
->is_halted()) return false;
1333 // Append the block to the planner
1334 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1335 // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
1336 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1337 // this is the new compensated machine position
1338 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1346 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1347 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1349 if(THEKERNEL
->is_halted()) return false;
1351 // catch negative or zero feed rates
1352 if(rate_mm_s
<= 0.0F
) {
1356 // get the absolute target position, default is current machine_position
1357 float target
[n_motors
];
1358 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1360 // add in the deltas to get new target
1361 for (int i
= 0; i
< naxis
; i
++) {
1362 target
[i
] += delta
[i
];
1365 // submit for planning and if moved update machine_position
1366 if(append_milestone(target
, rate_mm_s
)) {
1367 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1374 // Append a move to the queue ( cutting it into segments if needed )
1375 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1377 // catch negative or zero feed rates and return the same error as GRBL does
1378 if(rate_mm_s
<= 0.0F
) {
1379 gcode
->is_error
= true;
1380 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1384 // Find out the distance for this move in XYZ in MCS
1385 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 ));
1387 if(millimeters_of_travel
< 0.00001F
) {
1388 // we have no movement in XYZ, probably E only extrude or retract
1389 return this->append_milestone(target
, rate_mm_s
);
1393 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1394 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1395 We ask Extruder to do all the work but we need to pass in the relevant data.
1396 NOTE we need to do this before we segment the line (for deltas)
1398 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1399 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1400 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1401 rate_mm_s
*= data
[1]; // adjust the feedrate
1405 // 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.
1406 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1407 // 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
1410 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1413 } else if(this->delta_segments_per_second
> 1.0F
) {
1414 // enabled if set to something > 1, it is set to 0.0 by default
1415 // segment based on current speed and requested segments per second
1416 // the faster the travel speed the fewer segments needed
1417 // NOTE rate is mm/sec and we take into account any speed override
1418 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1419 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1420 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1423 if(this->mm_per_line_segment
== 0.0F
) {
1424 segments
= 1; // don't split it up
1426 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1432 // A vector to keep track of the endpoint of each segment
1433 float segment_delta
[n_motors
];
1434 float segment_end
[n_motors
];
1435 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1437 // How far do we move each segment?
1438 for (int i
= 0; i
< n_motors
; i
++)
1439 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1441 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1442 // We always add another point after this loop so we stop at segments-1, ie i < segments
1443 for (int i
= 1; i
< segments
; i
++) {
1444 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1445 for (int j
= 0; j
< n_motors
; j
++)
1446 segment_end
[j
] += segment_delta
[j
];
1448 // Append the end of this segment to the queue
1449 // this can block waiting for free block queue or if in feed hold
1450 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1455 // Append the end of this full move to the queue
1456 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1458 this->next_command_is_MCS
= false; // always reset this
1464 // Append an arc to the queue ( cutting it into segments as needed )
1465 // TODO does not support any E parameters so cannot be used for 3D printing.
1466 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1468 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1469 // catch negative or zero feed rates and return the same error as GRBL does
1470 if(rate_mm_s
<= 0.0F
) {
1471 gcode
->is_error
= true;
1472 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1477 // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
1478 float center_axis0
= this->arc_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1479 float center_axis1
= this->arc_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1480 float linear_travel
= target
[this->plane_axis_2
] - this->arc_milestone
[this->plane_axis_2
];
1481 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to start position
1482 float r_axis1
= -offset
[this->plane_axis_1
];
1483 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
1484 float rt_axis1
= target
[this->plane_axis_1
] - this->arc_milestone
[this->plane_axis_1
] - offset
[this->plane_axis_1
];
1485 float angular_travel
= 0;
1486 //check for condition where atan2 formula will fail due to everything canceling out exactly
1487 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
])) {
1488 if (is_clockwise
) { // set angular_travel to -2pi for a clockwise full circle
1489 angular_travel
= (-2 * PI
);
1490 } else { // set angular_travel to 2pi for a counterclockwise full circle
1491 angular_travel
= (2 * PI
);
1494 // Patch from GRBL Firmware - Christoph Baumann 04072015
1495 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1496 // Only run if not a full circle or angular travel will incorrectly result in 0.0f
1497 angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1498 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is reverse of other 2 planes
1499 if (is_clockwise
) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
1500 if (angular_travel
> 0) { angular_travel
-= (2 * PI
); }
1501 } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
1502 if (angular_travel
< 0) { angular_travel
+= (2 * PI
); }
1506 // Find the distance for this gcode
1507 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1509 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1510 if( millimeters_of_travel
< 0.000001F
) {
1514 // limit segments by maximum arc error
1515 float arc_segment
= this->mm_per_arc_segment
;
1516 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1517 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1518 if (this->mm_per_arc_segment
< min_err_segment
) {
1519 arc_segment
= min_err_segment
;
1523 // catch fall through on above
1524 if(arc_segment
< 0.0001F
) {
1525 arc_segment
= 0.5F
; /// the old default, so we avoid the divide by zero
1528 // Figure out how many segments for this gcode
1529 // 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
1530 uint16_t segments
= floorf(millimeters_of_travel
/ arc_segment
);
1534 float theta_per_segment
= angular_travel
/ segments
;
1535 float linear_per_segment
= linear_travel
/ segments
;
1537 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1538 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1539 r_T = [cos(phi) -sin(phi);
1540 sin(phi) cos(phi] * r ;
1541 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1542 defined from the circle center to the initial position. Each line segment is formed by successive
1543 vector rotations. This requires only two cos() and sin() computations to form the rotation
1544 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1545 all float numbers are single precision on the Arduino. (True float precision will not have
1546 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1547 tool precision in some cases. Therefore, arc path correction is implemented.
1549 Small angle approximation may be used to reduce computation overhead further. This approximation
1550 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1551 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1552 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1553 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1554 issue for CNC machines with the single precision Arduino calculations.
1555 This approximation also allows mc_arc to immediately insert a line segment into the planner
1556 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1557 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1558 This is important when there are successive arc motions.
1560 // Vector rotation matrix values
1561 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1562 float sin_T
= theta_per_segment
;
1564 // TODO we need to handle the ABC axis here by segmenting them
1565 float arc_target
[n_motors
];
1572 // init array for all axis
1573 memcpy(arc_target
, machine_position
, n_motors
*sizeof(float));
1575 // Initialize the linear axis
1576 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1578 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1579 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1581 if (count
< this->arc_correction
) {
1582 // Apply vector rotation matrix
1583 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1584 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1588 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1589 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1590 cos_Ti
= cosf(i
* theta_per_segment
);
1591 sin_Ti
= sinf(i
* theta_per_segment
);
1592 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1593 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1597 // Update arc_target location
1598 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1599 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1600 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1602 // Append this segment to the queue
1603 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1608 // Ensure last segment arrives at target location.
1609 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1614 // Do the math for an arc and add it to the queue
1615 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1619 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1621 // Set clockwise/counter-clockwise sign for mc_arc computations
1622 bool is_clockwise
= false;
1623 if( motion_mode
== CW_ARC
) {
1624 is_clockwise
= true;
1628 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1632 float Robot::theta(float x
, float y
)
1634 float t
= atanf(x
/ fabs(y
));
1646 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1648 this->plane_axis_0
= axis_0
;
1649 this->plane_axis_1
= axis_1
;
1650 this->plane_axis_2
= axis_2
;
1653 void Robot::clearToolOffset()
1655 this->tool_offset
= wcs_t(0,0,0);
1658 void Robot::setToolOffset(const float offset
[3])
1660 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1663 float Robot::get_feed_rate() const
1665 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;
1668 bool Robot::is_homed(uint8_t i
) const
1670 if(i
>= 3) return false; // safety
1672 // if we are homing we ignore soft endstops so return false
1674 bool ok
= PublicData::get_value(endstops_checksum
, get_homing_status_checksum
, 0, &homing
);
1675 if(!ok
|| homing
) return false;
1677 // check individual axis homing status
1679 ok
= PublicData::get_value(endstops_checksum
, get_homed_status_checksum
, 0, homed
);
1680 if(!ok
) return false;