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
);
595 if(gcode
->subcode
== 0 && gcode
->get_num_args() > 0) {
596 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
597 // ABC just need to set machine_position and compensated_machine_position if specified
599 if(!actuators
[i
]->is_extruder() && gcode
->has_letter(axis
)) {
600 float ap
= gcode
->get_value(axis
);
601 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
602 actuators
[i
]->change_last_milestone(ap
); // this updates the last_milestone in the actuator
612 } else if( gcode
->has_m
) {
614 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
615 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
618 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
619 if(!THEKERNEL
->is_grbl_mode()) break;
620 // fall through to M2
621 case 2: // M2 end of program
623 absolute_mode
= true;
626 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
629 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
630 if(gcode
->get_num_args() > 0) {
631 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
633 for (int i
= 0; i
< n_motors
; ++i
) {
634 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
635 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
637 // handle E parameter as currently selected extruder ABC
638 if(gcode
->has_letter('E')) {
639 // find first selected extruder
640 int i
= get_active_extruder();
642 bm
|= (0x02<<i
); // set appropriate bit
646 THEKERNEL
->conveyor
->wait_for_idle();
647 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
652 THEKERNEL
->conveyor
->wait_for_idle();
653 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
656 case 82: e_absolute_mode
= true; break;
657 case 83: e_absolute_mode
= false; break;
659 case 92: // M92 - set steps per mm
660 for (int i
= 0; i
< n_motors
; ++i
) {
661 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
662 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
663 if(gcode
->has_letter(axis
)) {
664 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
666 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
668 gcode
->add_nl
= true;
669 check_max_actuator_speeds();
674 print_position(gcode
->subcode
, buf
, true); // ignore extruders as they will print E themselves
675 gcode
->txt_after_ok
.append(buf
);
679 case 120: // push state
683 case 121: // pop state
687 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
688 if(gcode
->get_num_args() == 0) {
689 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
690 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
692 if(gcode
->subcode
== 1) {
693 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
694 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
695 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
698 gcode
->stream
->printf(" S: %g ", this->max_speed
);
701 gcode
->add_nl
= true;
704 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
705 if (gcode
->has_letter('X' + i
)) {
706 float v
= gcode
->get_value('X'+i
);
707 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
708 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
712 if(gcode
->subcode
== 1) {
713 // ABC axis only handle actuator max speeds
714 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
715 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
716 int c
= 'A' + i
- A_AXIS
;
717 if(gcode
->has_letter(c
)) {
718 float v
= gcode
->get_value(c
);
719 actuators
[i
]->set_max_rate(v
);
724 if(gcode
->has_letter('S')) max_speed
= gcode
->get_value('S');
728 // this format is deprecated
729 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
730 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
731 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
732 if (gcode
->has_letter('A' + i
)) {
733 float v
= gcode
->get_value('A'+i
);
734 actuators
[i
]->set_max_rate(v
);
739 if(gcode
->subcode
== 1) check_max_actuator_speeds();
743 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
744 if (gcode
->has_letter('S')) {
745 float acc
= gcode
->get_value('S'); // mm/s^2
747 if (acc
< 1.0F
) acc
= 1.0F
;
748 this->default_acceleration
= acc
;
750 for (int i
= 0; i
< n_motors
; ++i
) {
751 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
752 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
753 if(gcode
->has_letter(axis
)) {
754 float acc
= gcode
->get_value(axis
); // mm/s^2
756 if (acc
<= 0.0F
) acc
= NAN
;
757 actuators
[i
]->set_acceleration(acc
);
762 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
763 if (gcode
->has_letter('X')) {
764 float jd
= gcode
->get_value('X');
768 THEKERNEL
->planner
->junction_deviation
= jd
;
770 if (gcode
->has_letter('Z')) {
771 float jd
= gcode
->get_value('Z');
772 // enforce minimum, -1 disables it and uses regular junction deviation
775 THEKERNEL
->planner
->z_junction_deviation
= jd
;
777 if (gcode
->has_letter('S')) {
778 float mps
= gcode
->get_value('S');
782 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
786 case 211: // M211 Sn turns soft endstops on/off
787 if(gcode
->has_letter('S')) {
788 soft_endstop_enabled
= gcode
->get_uint('S') == 1;
790 gcode
->stream
->printf("Soft endstops are %s", soft_endstop_enabled
? "Enabled" : "Disabled");
791 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
792 if(isnan(soft_endstop_min
[i
])) {
793 gcode
->stream
->printf(",%c min is disabled", 'X'+i
);
795 if(isnan(soft_endstop_max
[i
])) {
796 gcode
->stream
->printf(",%c max is disabled", 'X'+i
);
799 gcode
->stream
->printf(",%c axis is not homed", 'X'+i
);
802 gcode
->stream
->printf("\n");
806 case 220: // M220 - speed override percentage
807 if (gcode
->has_letter('S')) {
808 float factor
= gcode
->get_value('S');
809 // enforce minimum 10% speed
812 // enforce maximum 10x speed
813 if (factor
> 1000.0F
)
816 seconds_per_minute
= 6000.0F
/ factor
;
818 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
822 case 400: // wait until all moves are done up to this point
823 THEKERNEL
->conveyor
->wait_for_idle();
826 case 500: // M500 saves some volatile settings to config override file
827 case 503: { // M503 just prints the settings
828 gcode
->stream
->printf(";Steps per unit:\nM92 ");
829 for (int i
= 0; i
< n_motors
; ++i
) {
830 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
831 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
832 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
834 gcode
->stream
->printf("\n");
836 // only print if not NAN
837 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
838 for (int i
= 0; i
< n_motors
; ++i
) {
839 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
840 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
841 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
843 gcode
->stream
->printf("\n");
845 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
);
847 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
);
849 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
850 for (int i
= 0; i
< n_motors
; ++i
) {
851 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
852 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
853 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
855 gcode
->stream
->printf("\n");
857 // get or save any arm solution specific optional values
858 BaseSolution::arm_options_t options
;
859 if(arm_solution
->get_optional(options
) && !options
.empty()) {
860 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
861 for(auto &i
: options
) {
862 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
864 gcode
->stream
->printf("\n");
867 // save wcs_offsets and current_wcs
868 // TODO this may need to be done whenever they change to be compliant
870 gcode
->stream
->printf(";WCS settings\n");
871 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
873 for(auto &i
: wcs_offsets
) {
874 if(i
!= wcs_t(0, 0, 0)) {
876 std::tie(x
, y
, z
) = i
;
877 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
883 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
884 // 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
885 if(g92_offset
!= wcs_t(0, 0, 0)) {
887 std::tie(x
, y
, z
) = g92_offset
;
888 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
894 case 665: { // M665 set optional arm solution variables based on arm solution.
895 // the parameter args could be any letter each arm solution only accepts certain ones
896 BaseSolution::arm_options_t options
= gcode
->get_args();
897 options
.erase('S'); // don't include the S
898 options
.erase('U'); // don't include the U
899 if(options
.size() > 0) {
900 // set the specified options
901 arm_solution
->set_optional(options
);
904 if(arm_solution
->get_optional(options
)) {
905 // foreach optional value
906 for(auto &i
: options
) {
907 // print all current values of supported options
908 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
909 gcode
->add_nl
= true;
913 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
914 this->delta_segments_per_second
= gcode
->get_value('S');
915 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
917 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
918 this->mm_per_line_segment
= gcode
->get_value('U');
919 this->delta_segments_per_second
= 0;
920 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
928 if( motion_mode
!= NONE
) {
929 is_g123
= motion_mode
!= SEEK
;
930 process_move(gcode
, motion_mode
);
936 next_command_is_MCS
= false; // must be on same line as G0 or G1
939 int Robot::get_active_extruder() const
941 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
942 // find first selected extruder
943 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
948 // process a G0/G1/G2/G3
949 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
951 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
952 // get XYZ and one E (which goes to the selected extruder)
953 float param
[4]{NAN
, NAN
, NAN
, NAN
};
955 // process primary axis
956 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
958 if( gcode
->has_letter(letter
) ) {
959 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
963 float offset
[3]{0,0,0};
964 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
965 if( gcode
->has_letter(letter
) ) {
966 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
970 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
971 float target
[n_motors
];
972 memcpy(target
, machine_position
, n_motors
*sizeof(float));
974 if(!next_command_is_MCS
) {
975 if(this->absolute_mode
) {
976 // apply wcs offsets and g92 offset and tool offset
977 if(!isnan(param
[X_AXIS
])) {
978 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
);
981 if(!isnan(param
[Y_AXIS
])) {
982 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
);
985 if(!isnan(param
[Z_AXIS
])) {
986 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
);
990 // they are deltas from the machine_position if specified
991 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
992 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
997 // already in machine coordinates, we do not add wcs or tool offset for that
998 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
999 if(!isnan(param
[i
])) target
[i
] = param
[i
];
1005 #if MAX_ROBOT_ACTUATORS > 3
1006 // process extruder parameters, for active extruder only (only one active extruder at a time)
1007 int selected_extruder
= 0;
1008 if(gcode
->has_letter('E')) {
1009 selected_extruder
= get_active_extruder();
1010 param
[E_AXIS
]= gcode
->get_value('E');
1013 // do E for the selected extruder
1014 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
1015 if(this->e_absolute_mode
) {
1016 target
[selected_extruder
]= param
[E_AXIS
];
1017 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
1019 delta_e
= param
[E_AXIS
];
1020 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
1024 // 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
1025 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
1026 char letter
= 'A'+i
-A_AXIS
;
1027 if(gcode
->has_letter(letter
)) {
1028 float p
= gcode
->get_value(letter
);
1029 if(this->absolute_mode
) {
1032 target
[i
]= p
+ machine_position
[i
];
1038 if( gcode
->has_letter('F') ) {
1039 if( motion_mode
== SEEK
)
1040 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
1042 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
1045 // S is modal When specified on a G0/1/2/3 command
1046 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
1050 // Perform any physical actions
1051 switch(motion_mode
) {
1055 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
1059 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
1064 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
1065 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1069 // needed to act as start of next arc command
1070 memcpy(arc_milestone
, target
, sizeof(arc_milestone
));
1073 // set machine_position to the calculated target
1074 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1078 // reset the machine position for all axis. Used for homing.
1079 // after homing we supply the cartesian coordinates that the head is at when homed,
1080 // however for Z this is the compensated machine position (if enabled)
1081 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1082 // this will make the results from M114 and ? consistent after homing.
1083 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1084 void Robot::reset_axis_position(float x
, float y
, float z
)
1086 // set both the same initially
1087 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1088 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1089 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1091 if(compensationTransform
) {
1092 // apply inverse transform to get machine_position
1093 compensationTransform(machine_position
, true);
1096 // now set the actuator positions based on the supplied compensated position
1097 ActuatorCoordinates actuator_pos
;
1098 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1099 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1100 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1103 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1104 void Robot::reset_axis_position(float position
, int axis
)
1106 compensated_machine_position
[axis
] = position
;
1107 if(axis
<= Z_AXIS
) {
1108 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1110 #if MAX_ROBOT_ACTUATORS > 3
1111 }else if(axis
< n_motors
) {
1112 // ABC and/or extruders need to be set as there is no arm solution for them
1113 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1114 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1119 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1120 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1121 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1123 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1124 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1127 // now correct axis positions then recorrect actuator to account for rounding
1128 reset_position_from_current_actuator_position();
1131 // Use FK to find out where actuator is and reset to match
1132 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1133 void Robot::reset_position_from_current_actuator_position()
1135 ActuatorCoordinates actuator_pos
;
1136 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1137 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1138 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1141 // discover machine position from where actuators actually are
1142 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1143 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1145 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1146 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1148 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1149 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1150 // to get everything in perfect sync.
1151 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1152 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1153 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1156 // Handle extruders and/or ABC axis
1157 #if MAX_ROBOT_ACTUATORS > 3
1158 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1159 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1160 float ap
= actuator_pos
[i
];
1161 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
1162 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1163 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1168 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1169 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1170 // all transforms and is what we actually convert to actuator positions
1171 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1173 float deltas
[n_motors
];
1174 float transformed_target
[n_motors
]; // adjust target for bed compensation
1175 float unit_vec
[N_PRIMARY_AXIS
];
1177 // unity transform by default
1178 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1180 // check function pointer and call if set to transform the target to compensate for bed
1181 if(compensationTransform
) {
1182 // some compensation strategies can transform XYZ, some just change Z
1183 compensationTransform(transformed_target
, false);
1186 // check soft endstops only for homed axis that are enabled
1187 if(soft_endstop_enabled
) {
1188 for (int i
= 0; i
<= Z_AXIS
; ++i
) {
1189 if(!is_homed(i
)) continue;
1190 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
]) ) {
1191 if(soft_endstop_halt
) {
1192 if(THEKERNEL
->is_grbl_mode()) {
1193 THEKERNEL
->streams
->printf("error:");
1195 THEKERNEL
->streams
->printf("Error: ");
1198 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i
+'X');
1199 THEKERNEL
->call_event(ON_HALT
, nullptr);
1202 //} else if(soft_endstop_truncate) {
1203 // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
1204 // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
1208 if(THEKERNEL
->is_grbl_mode()) {
1209 THEKERNEL
->streams
->printf("error:");
1211 THEKERNEL
->streams
->printf("Error: ");
1213 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - entire move ignored\n", i
+'X');
1222 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1224 // find distance moved by each axis, use transformed target from the current compensated machine position
1225 for (size_t i
= 0; i
< n_motors
; i
++) {
1226 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1227 if(deltas
[i
] == 0) continue;
1228 // at least one non zero delta
1230 if(i
< N_PRIMARY_AXIS
) {
1231 sos
+= powf(deltas
[i
], 2);
1236 if(!move
) return false;
1238 // see if this is a primary axis move or not
1239 bool auxilliary_move
= true;
1240 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1241 if(deltas
[i
] != 0) {
1242 auxilliary_move
= false;
1247 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1248 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1250 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1251 // 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
1252 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1254 if(!auxilliary_move
) {
1255 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1256 // find distance unit vector for primary axis only
1257 unit_vec
[i
] = deltas
[i
] / distance
;
1259 // Do not move faster than the configured cartesian limits for XYZ
1260 if ( i
<= Z_AXIS
&& max_speeds
[i
] > 0 ) {
1261 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1263 if (axis_speed
> max_speeds
[i
])
1264 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1268 if(this->max_speed
> 0 && rate_mm_s
> this->max_speed
) {
1269 rate_mm_s
= this->max_speed
;
1273 // find actuator position given the machine position, use actual adjusted target
1274 ActuatorCoordinates actuator_pos
;
1275 if(!disable_arm_solution
) {
1276 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1279 // basically the same as cartesian, would be used for special homing situations like for scara
1280 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1281 actuator_pos
[i
] = transformed_target
[i
];
1285 #if MAX_ROBOT_ACTUATORS > 3
1287 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1288 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1289 actuator_pos
[i
]= transformed_target
[i
];
1290 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1291 // NOTE this relies on the fact only one extruder is active at a time
1292 // scale for volumetric or flow rate
1293 // TODO is this correct? scaling the absolute target? what if the scale changes?
1294 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1295 actuator_pos
[i
] *= get_e_scale_fnc();
1297 if(auxilliary_move
) {
1298 // for E only moves we need to use the scaled E to calculate the distance
1299 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1302 if(auxilliary_move
) {
1303 distance
= sqrtf(sos
); // distance in mm of the e move
1304 if(distance
< 0.00001F
) return false;
1308 // use default acceleration to start with
1309 float acceleration
= default_acceleration
;
1311 float isecs
= rate_mm_s
/ distance
;
1313 // check per-actuator speed limits
1314 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1315 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1316 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1318 float actuator_rate
= d
* isecs
;
1319 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1320 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1321 isecs
= rate_mm_s
/ distance
;
1324 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1325 // 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.
1326 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1327 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1328 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1329 float ca
= fabsf((d
/distance
) * acceleration
);
1331 acceleration
*= ( ma
/ ca
);
1337 // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
1338 while(THEKERNEL
->get_feed_hold()) {
1339 THEKERNEL
->call_event(ON_IDLE
, this);
1340 // if we also got a HALT then break out of this
1341 if(THEKERNEL
->is_halted()) return false;
1344 // Append the block to the planner
1345 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1346 // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
1347 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1348 // this is the new compensated machine position
1349 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1357 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1358 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1360 if(THEKERNEL
->is_halted()) return false;
1362 // catch negative or zero feed rates
1363 if(rate_mm_s
<= 0.0F
) {
1367 // get the absolute target position, default is current machine_position
1368 float target
[n_motors
];
1369 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1371 // add in the deltas to get new target
1372 for (int i
= 0; i
< naxis
; i
++) {
1373 target
[i
] += delta
[i
];
1376 // submit for planning and if moved update machine_position
1377 if(append_milestone(target
, rate_mm_s
)) {
1378 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1385 // Append a move to the queue ( cutting it into segments if needed )
1386 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1388 // catch negative or zero feed rates and return the same error as GRBL does
1389 if(rate_mm_s
<= 0.0F
) {
1390 gcode
->is_error
= true;
1391 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1395 // Find out the distance for this move in XYZ in MCS
1396 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 ));
1398 if(millimeters_of_travel
< 0.00001F
) {
1399 // we have no movement in XYZ, probably E only extrude or retract
1400 return this->append_milestone(target
, rate_mm_s
);
1404 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1405 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1406 We ask Extruder to do all the work but we need to pass in the relevant data.
1407 NOTE we need to do this before we segment the line (for deltas)
1409 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1410 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1411 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1412 rate_mm_s
*= data
[1]; // adjust the feedrate
1416 // 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.
1417 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1418 // 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
1421 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1424 } else if(this->delta_segments_per_second
> 1.0F
) {
1425 // enabled if set to something > 1, it is set to 0.0 by default
1426 // segment based on current speed and requested segments per second
1427 // the faster the travel speed the fewer segments needed
1428 // NOTE rate is mm/sec and we take into account any speed override
1429 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1430 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1431 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1434 if(this->mm_per_line_segment
== 0.0F
) {
1435 segments
= 1; // don't split it up
1437 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1443 // A vector to keep track of the endpoint of each segment
1444 float segment_delta
[n_motors
];
1445 float segment_end
[n_motors
];
1446 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1448 // How far do we move each segment?
1449 for (int i
= 0; i
< n_motors
; i
++)
1450 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1452 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1453 // We always add another point after this loop so we stop at segments-1, ie i < segments
1454 for (int i
= 1; i
< segments
; i
++) {
1455 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1456 for (int j
= 0; j
< n_motors
; j
++)
1457 segment_end
[j
] += segment_delta
[j
];
1459 // Append the end of this segment to the queue
1460 // this can block waiting for free block queue or if in feed hold
1461 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1466 // Append the end of this full move to the queue
1467 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1469 this->next_command_is_MCS
= false; // always reset this
1475 // Append an arc to the queue ( cutting it into segments as needed )
1476 // TODO does not support any E parameters so cannot be used for 3D printing.
1477 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1479 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1480 // catch negative or zero feed rates and return the same error as GRBL does
1481 if(rate_mm_s
<= 0.0F
) {
1482 gcode
->is_error
= true;
1483 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1488 // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
1489 float center_axis0
= this->arc_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1490 float center_axis1
= this->arc_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1491 float linear_travel
= target
[this->plane_axis_2
] - this->arc_milestone
[this->plane_axis_2
];
1492 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to start position
1493 float r_axis1
= -offset
[this->plane_axis_1
];
1494 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
1495 float rt_axis1
= target
[this->plane_axis_1
] - this->arc_milestone
[this->plane_axis_1
] - offset
[this->plane_axis_1
];
1496 float angular_travel
= 0;
1497 //check for condition where atan2 formula will fail due to everything canceling out exactly
1498 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
])) {
1499 if (is_clockwise
) { // set angular_travel to -2pi for a clockwise full circle
1500 angular_travel
= (-2 * PI
);
1501 } else { // set angular_travel to 2pi for a counterclockwise full circle
1502 angular_travel
= (2 * PI
);
1505 // Patch from GRBL Firmware - Christoph Baumann 04072015
1506 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1507 // Only run if not a full circle or angular travel will incorrectly result in 0.0f
1508 angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1509 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is reverse of other 2 planes
1510 if (is_clockwise
) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
1511 if (angular_travel
> 0) { angular_travel
-= (2 * PI
); }
1512 } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
1513 if (angular_travel
< 0) { angular_travel
+= (2 * PI
); }
1517 // Find the distance for this gcode
1518 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1520 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1521 if( millimeters_of_travel
< 0.000001F
) {
1525 // limit segments by maximum arc error
1526 float arc_segment
= this->mm_per_arc_segment
;
1527 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1528 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1529 if (this->mm_per_arc_segment
< min_err_segment
) {
1530 arc_segment
= min_err_segment
;
1534 // catch fall through on above
1535 if(arc_segment
< 0.0001F
) {
1536 arc_segment
= 0.5F
; /// the old default, so we avoid the divide by zero
1539 // Figure out how many segments for this gcode
1540 // 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
1541 uint16_t segments
= floorf(millimeters_of_travel
/ arc_segment
);
1545 float theta_per_segment
= angular_travel
/ segments
;
1546 float linear_per_segment
= linear_travel
/ segments
;
1548 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1549 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1550 r_T = [cos(phi) -sin(phi);
1551 sin(phi) cos(phi] * r ;
1552 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1553 defined from the circle center to the initial position. Each line segment is formed by successive
1554 vector rotations. This requires only two cos() and sin() computations to form the rotation
1555 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1556 all float numbers are single precision on the Arduino. (True float precision will not have
1557 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1558 tool precision in some cases. Therefore, arc path correction is implemented.
1560 Small angle approximation may be used to reduce computation overhead further. This approximation
1561 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1562 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1563 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1564 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1565 issue for CNC machines with the single precision Arduino calculations.
1566 This approximation also allows mc_arc to immediately insert a line segment into the planner
1567 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1568 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1569 This is important when there are successive arc motions.
1571 // Vector rotation matrix values
1572 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1573 float sin_T
= theta_per_segment
;
1575 // TODO we need to handle the ABC axis here by segmenting them
1576 float arc_target
[n_motors
];
1583 // init array for all axis
1584 memcpy(arc_target
, machine_position
, n_motors
*sizeof(float));
1586 // Initialize the linear axis
1587 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1589 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1590 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1592 if (count
< this->arc_correction
) {
1593 // Apply vector rotation matrix
1594 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1595 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1599 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1600 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1601 cos_Ti
= cosf(i
* theta_per_segment
);
1602 sin_Ti
= sinf(i
* theta_per_segment
);
1603 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1604 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1608 // Update arc_target location
1609 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1610 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1611 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1613 // Append this segment to the queue
1614 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1619 // Ensure last segment arrives at target location.
1620 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1625 // Do the math for an arc and add it to the queue
1626 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1630 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1632 // Set clockwise/counter-clockwise sign for mc_arc computations
1633 bool is_clockwise
= false;
1634 if( motion_mode
== CW_ARC
) {
1635 is_clockwise
= true;
1639 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1643 float Robot::theta(float x
, float y
)
1645 float t
= atanf(x
/ fabs(y
));
1657 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1659 this->plane_axis_0
= axis_0
;
1660 this->plane_axis_1
= axis_1
;
1661 this->plane_axis_2
= axis_2
;
1664 void Robot::clearToolOffset()
1666 this->tool_offset
= wcs_t(0,0,0);
1669 void Robot::setToolOffset(const float offset
[3])
1671 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1674 float Robot::get_feed_rate() const
1676 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;
1679 bool Robot::is_homed(uint8_t i
) const
1681 if(i
>= 3) return false; // safety
1683 // if we are homing we ignore soft endstops so return false
1685 bool ok
= PublicData::get_value(endstops_checksum
, get_homing_status_checksum
, 0, &homing
);
1686 if(!ok
|| homing
) return false;
1688 // check individual axis homing status
1690 ok
= PublicData::get_value(endstops_checksum
, get_homed_status_checksum
, 0, homed
);
1691 if(!ok
) return false;