2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
15 #include "StepperMotor.h"
17 #include "PublicDataRequest.h"
18 #include "PublicData.h"
19 #include "arm_solutions/BaseSolution.h"
20 #include "arm_solutions/CartesianSolution.h"
21 #include "arm_solutions/RotatableCartesianSolution.h"
22 #include "arm_solutions/LinearDeltaSolution.h"
23 #include "arm_solutions/RotaryDeltaSolution.h"
24 #include "arm_solutions/HBotSolution.h"
25 #include "arm_solutions/CoreXZSolution.h"
26 #include "arm_solutions/MorganSCARASolution.h"
27 #include "StepTicker.h"
28 #include "checksumm.h"
30 #include "ConfigValue.h"
31 #include "libs/StreamOutput.h"
32 #include "StreamOutputPool.h"
33 #include "ExtruderPublicAccess.h"
34 #include "GcodeDispatch.h"
35 #include "ActuatorCoordinates.h"
36 #include "EndstopsPublicAccess.h"
38 #include "mbed.h" // for us_ticker_read()
45 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
46 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
47 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
48 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
49 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
50 #define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
51 #define arc_correction_checksum CHECKSUM("arc_correction")
52 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
53 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
54 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
55 #define segment_z_moves_checksum CHECKSUM("segment_z_moves")
56 #define save_g92_checksum CHECKSUM("save_g92")
57 #define set_g92_checksum CHECKSUM("set_g92")
60 #define arm_solution_checksum CHECKSUM("arm_solution")
61 #define cartesian_checksum CHECKSUM("cartesian")
62 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
63 #define rostock_checksum CHECKSUM("rostock")
64 #define linear_delta_checksum CHECKSUM("linear_delta")
65 #define rotary_delta_checksum CHECKSUM("rotary_delta")
66 #define delta_checksum CHECKSUM("delta")
67 #define hbot_checksum CHECKSUM("hbot")
68 #define corexy_checksum CHECKSUM("corexy")
69 #define corexz_checksum CHECKSUM("corexz")
70 #define kossel_checksum CHECKSUM("kossel")
71 #define morgan_checksum CHECKSUM("morgan")
73 // new-style actuator stuff
74 #define actuator_checksum CHEKCSUM("actuator")
76 #define step_pin_checksum CHECKSUM("step_pin")
77 #define dir_pin_checksum CHEKCSUM("dir_pin")
78 #define en_pin_checksum CHECKSUM("en_pin")
80 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
81 #define max_rate_checksum CHECKSUM("max_rate")
82 #define acceleration_checksum CHECKSUM("acceleration")
83 #define z_acceleration_checksum CHECKSUM("z_acceleration")
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 ARC_ANGULAR_TRAVEL_EPSILON 5E-9F // Float (radians)
102 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
104 // 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
105 // It takes care of cutting arcs into segments, same thing for line that are too long
109 this->inch_mode
= false;
110 this->absolute_mode
= true;
111 this->e_absolute_mode
= true;
112 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
113 memset(this->machine_position
, 0, sizeof machine_position
);
114 memset(this->compensated_machine_position
, 0, sizeof compensated_machine_position
);
115 this->arm_solution
= NULL
;
116 seconds_per_minute
= 60.0F
;
117 this->clearToolOffset();
118 this->compensationTransform
= nullptr;
119 this->get_e_scale_fnc
= nullptr;
120 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
121 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
122 this->next_command_is_MCS
= false;
123 this->disable_segmentation
= false;
124 this->disable_arm_solution
= false;
128 //Called when the module has just been loaded
129 void Robot::on_module_loaded()
131 this->register_for_event(ON_GCODE_RECEIVED
);
137 #define ACTUATOR_CHECKSUMS(X) { \
138 CHECKSUM(X "_step_pin"), \
139 CHECKSUM(X "_dir_pin"), \
140 CHECKSUM(X "_en_pin"), \
141 CHECKSUM(X "_steps_per_mm"), \
142 CHECKSUM(X "_max_rate"), \
143 CHECKSUM(X "_acceleration") \
146 void Robot::load_config()
148 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
149 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
150 // To make adding those solution easier, they have their own, separate object.
151 // Here we read the config to find out which arm solution to use
152 if (this->arm_solution
) delete this->arm_solution
;
153 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
154 // Note checksums are not const expressions when in debug mode, so don't use switch
155 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
156 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
158 } else if(solution_checksum
== corexz_checksum
) {
159 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
161 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
162 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
164 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
165 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
167 } else if(solution_checksum
== rotary_delta_checksum
) {
168 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
170 } else if(solution_checksum
== morgan_checksum
) {
171 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
173 } else if(solution_checksum
== cartesian_checksum
) {
174 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
177 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
180 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
181 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
182 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
183 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
184 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
185 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
186 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
188 // in mm/sec but specified in config as mm/min
189 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
190 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
191 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.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 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
197 // optional setting for a fixed G92 offset
198 std::vector
<float> t
= parse_number_list(g92
.c_str());
200 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
204 // default s value for laser
205 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
207 // Make our Primary XYZ StepperMotors, and potentially A B C
208 uint16_t const motor_checksums
[][6] = {
209 ACTUATOR_CHECKSUMS("alpha"), // X
210 ACTUATOR_CHECKSUMS("beta"), // Y
211 ACTUATOR_CHECKSUMS("gamma"), // Z
212 #if MAX_ROBOT_ACTUATORS > 3
213 ACTUATOR_CHECKSUMS("delta"), // A
214 #if MAX_ROBOT_ACTUATORS > 4
215 ACTUATOR_CHECKSUMS("epsilon"), // B
216 #if MAX_ROBOT_ACTUATORS > 5
217 ACTUATOR_CHECKSUMS("zeta") // C
223 // default acceleration setting, can be overriden with newer per axis settings
224 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
227 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
228 Pin pins
[3]; //step, dir, enable
229 for (size_t i
= 0; i
< 3; i
++) {
230 pins
[i
].from_string(THEKERNEL
->config
->value(motor_checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
233 if(!pins
[0].connected() || !pins
[1].connected()) { // step and dir must be defined, but enable is optional
235 THEKERNEL
->streams
->printf("FATAL: motor %c is not defined in config\n", 'X'+a
);
236 n_motors
= a
; // we only have this number of motors
239 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
242 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
243 // register this motor (NB This must be 0,1,2) of the actuators array
244 uint8_t n
= register_motor(sm
);
246 // this is a fatal error
247 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
251 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(motor_checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
252 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
253 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(motor_checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
256 check_max_actuator_speeds(); // check the configs are sane
258 // if we have not specified a z acceleration see if the legacy config was set
259 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
260 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
262 actuators
[Z_AXIS
]->set_acceleration(acc
);
266 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
267 // so the first move can be correct if homing is not performed
268 ActuatorCoordinates actuator_pos
;
269 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
270 for (size_t i
= 0; i
< n_motors
; i
++)
271 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
273 //this->clearToolOffset();
275 soft_endstop_enabled
= THEKERNEL
->config
->value(soft_endstop_checksum
, enable_checksum
)->by_default(false)->as_bool();
276 soft_endstop_halt
= THEKERNEL
->config
->value(soft_endstop_checksum
, halt_checksum
)->by_default(true)->as_bool();
278 soft_endstop_min
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmin_checksum
)->by_default(NAN
)->as_number();
279 soft_endstop_min
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymin_checksum
)->by_default(NAN
)->as_number();
280 soft_endstop_min
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmin_checksum
)->by_default(NAN
)->as_number();
281 soft_endstop_max
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmax_checksum
)->by_default(NAN
)->as_number();
282 soft_endstop_max
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymax_checksum
)->by_default(NAN
)->as_number();
283 soft_endstop_max
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmax_checksum
)->by_default(NAN
)->as_number();
286 uint8_t Robot::register_motor(StepperMotor
*motor
)
288 // register this motor with the step ticker
289 THEKERNEL
->step_ticker
->register_motor(motor
);
290 if(n_motors
>= k_max_actuators
) {
291 // this is a fatal error
292 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
295 actuators
.push_back(motor
);
296 motor
->set_motor_id(n_motors
);
300 void Robot::push_state()
302 bool am
= this->absolute_mode
;
303 bool em
= this->e_absolute_mode
;
304 bool im
= this->inch_mode
;
305 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
309 void Robot::pop_state()
311 if(!state_stack
.empty()) {
312 auto s
= state_stack
.top();
314 this->feed_rate
= std::get
<0>(s
);
315 this->seek_rate
= std::get
<1>(s
);
316 this->absolute_mode
= std::get
<2>(s
);
317 this->e_absolute_mode
= std::get
<3>(s
);
318 this->inch_mode
= std::get
<4>(s
);
319 this->current_wcs
= std::get
<5>(s
);
323 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
325 std::vector
<wcs_t
> v
;
326 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
327 for(auto& i
: wcs_offsets
) {
330 v
.push_back(g92_offset
);
331 v
.push_back(tool_offset
);
335 void Robot::get_current_machine_position(float *pos
) const
337 // get real time current actuator position in mm
338 ActuatorCoordinates current_position
{
339 actuators
[X_AXIS
]->get_current_position(),
340 actuators
[Y_AXIS
]->get_current_position(),
341 actuators
[Z_AXIS
]->get_current_position()
344 // get machine position from the actuator position using FK
345 arm_solution
->actuator_to_cartesian(current_position
, pos
);
348 void Robot::print_position(uint8_t subcode
, std::string
& res
, bool ignore_extruders
) const
350 // M114.1 is a new way to do this (similar to how GRBL does it).
351 // it returns the realtime position based on the current step position of the actuators.
352 // this does require a FK to get a machine position from the actuator position
353 // and then invert all the transforms to get a workspace position from machine position
354 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
357 if(subcode
== 0) { // M114 print WCS
358 wcs_t pos
= mcs2wcs(machine_position
);
359 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
)));
361 } else if(subcode
== 4) {
362 // M114.4 print last milestone
363 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
]);
365 } else if(subcode
== 5) {
366 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
367 // will differ from LMS by the compensation at the current position otherwise
368 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
]);
371 // get real time positions
373 get_current_machine_position(mpos
);
375 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
376 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
378 if(subcode
== 1) { // M114.1 print realtime WCS
379 wcs_t pos
= mcs2wcs(mpos
);
380 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
)));
382 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
383 n
= snprintf(buf
, sizeof(buf
), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
385 } else if(subcode
== 3) { // M114.3 print realtime actuator position
386 // get real time current actuator position in mm
387 ActuatorCoordinates current_position
{
388 actuators
[X_AXIS
]->get_current_position(),
389 actuators
[Y_AXIS
]->get_current_position(),
390 actuators
[Z_AXIS
]->get_current_position()
392 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
]);
396 if(n
> sizeof(buf
)) n
= sizeof(buf
);
399 #if MAX_ROBOT_ACTUATORS > 3
400 // deal with the ABC axis
401 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
403 if(ignore_extruders
&& actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
404 if(subcode
== 4) { // M114.4 print last milestone
405 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
407 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
408 // current actuator position
409 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
411 if(n
> sizeof(buf
)) n
= sizeof(buf
);
412 if(n
> 0) res
.append(buf
, n
);
417 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
418 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
420 return std::make_tuple(
421 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
),
422 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
),
423 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
)
427 // this does a sanity check that actuator speeds do not exceed steps rate capability
428 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
429 void Robot::check_max_actuator_speeds()
431 for (size_t i
= 0; i
< n_motors
; i
++) {
432 if(actuators
[i
]->is_extruder()) continue; //extruders are not included in this check
434 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
435 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
436 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
437 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());
442 //A GCode has been received
443 //See if the current Gcode line has some orders for us
444 void Robot::on_gcode_received(void *argument
)
446 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
448 enum MOTION_MODE_T motion_mode
= NONE
;
452 case 0: motion_mode
= SEEK
; break;
453 case 1: motion_mode
= LINEAR
; break;
454 case 2: motion_mode
= CW_ARC
; break;
455 case 3: motion_mode
= CCW_ARC
; break;
456 case 4: { // G4 Dwell
457 uint32_t delay_ms
= 0;
458 if (gcode
->has_letter('P')) {
459 if(THEKERNEL
->is_grbl_mode()) {
460 // in grbl mode (and linuxcnc) P is decimal seconds
461 float f
= gcode
->get_value('P');
462 delay_ms
= f
* 1000.0F
;
465 // in reprap P is milliseconds, they always have to be different!
466 delay_ms
= gcode
->get_int('P');
469 if (gcode
->has_letter('S')) {
470 delay_ms
+= gcode
->get_int('S') * 1000;
474 THEKERNEL
->conveyor
->wait_for_idle();
475 // wait for specified time
476 uint32_t start
= us_ticker_read(); // mbed call
477 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
478 THEKERNEL
->call_event(ON_IDLE
, this);
479 if(THEKERNEL
->is_halted()) return;
485 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
486 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
487 size_t n
= gcode
->get_uint('P');
488 if(n
== 0) n
= current_wcs
; // set current coordinate system
492 std::tie(x
, y
, z
) = wcs_offsets
[n
];
493 if(gcode
->get_int('L') == 20) {
494 // this makes the current machine position (less compensation transform) the offset
495 // get current position in WCS
496 wcs_t pos
= mcs2wcs(machine_position
);
498 if(gcode
->has_letter('X')){
499 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
502 if(gcode
->has_letter('Y')){
503 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
505 if(gcode
->has_letter('Z')) {
506 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
511 // the value is the offset from machine zero
512 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
513 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
514 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
516 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
517 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
518 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
521 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
526 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
527 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
528 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
529 case 20: this->inch_mode
= true; break;
530 case 21: this->inch_mode
= false; break;
532 case 54: case 55: case 56: case 57: case 58: case 59:
533 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
534 current_wcs
= gcode
->g
- 54;
535 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
536 current_wcs
+= gcode
->subcode
;
537 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
541 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
542 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
545 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
546 // reset G92 offsets to 0
547 g92_offset
= wcs_t(0, 0, 0);
549 } else if(gcode
->subcode
== 3) {
550 // initialize G92 to the specified values, only used for saving it with M500
551 float x
= 0, y
= 0, z
= 0;
552 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
553 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
554 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
555 g92_offset
= wcs_t(x
, y
, z
);
558 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
560 std::tie(x
, y
, z
) = g92_offset
;
561 // get current position in WCS
562 wcs_t pos
= mcs2wcs(machine_position
);
564 // adjust g92 offset to make the current wpos == the value requested
565 if(gcode
->has_letter('X')){
566 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
568 if(gcode
->has_letter('Y')){
569 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
571 if(gcode
->has_letter('Z')) {
572 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
574 g92_offset
= wcs_t(x
, y
, z
);
577 #if MAX_ROBOT_ACTUATORS > 3
578 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
579 // reset the E position, legacy for 3d Printers to be reprap compatible
580 // find the selected extruder
581 int selected_extruder
= get_active_extruder();
582 if(selected_extruder
> 0) {
583 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
584 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
585 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
594 } else if( gcode
->has_m
) {
596 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
597 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
600 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
601 if(!THEKERNEL
->is_grbl_mode()) break;
602 // fall through to M2
603 case 2: // M2 end of program
605 absolute_mode
= true;
608 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
611 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
612 if(gcode
->get_num_args() > 0) {
613 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
615 for (int i
= 0; i
< n_motors
; ++i
) {
616 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
617 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
619 // handle E parameter as currently selected extruder ABC
620 if(gcode
->has_letter('E')) {
621 // find first selected extruder
622 int i
= get_active_extruder();
624 bm
|= (0x02<<i
); // set appropriate bit
628 THEKERNEL
->conveyor
->wait_for_idle();
629 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
634 THEKERNEL
->conveyor
->wait_for_idle();
635 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
638 case 82: e_absolute_mode
= true; break;
639 case 83: e_absolute_mode
= false; break;
641 case 92: // M92 - set steps per mm
642 for (int i
= 0; i
< n_motors
; ++i
) {
643 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
644 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
645 if(gcode
->has_letter(axis
)) {
646 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
648 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
650 gcode
->add_nl
= true;
651 check_max_actuator_speeds();
656 print_position(gcode
->subcode
, buf
, true); // ignore extruders as they will print E themselves
657 gcode
->txt_after_ok
.append(buf
);
661 case 120: // push state
665 case 121: // pop state
669 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
670 if(gcode
->get_num_args() == 0) {
671 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
672 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
674 if(gcode
->subcode
== 1) {
675 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
676 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
677 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
681 gcode
->add_nl
= true;
684 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
685 if (gcode
->has_letter('X' + i
)) {
686 float v
= gcode
->get_value('X'+i
);
687 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
688 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
692 if(gcode
->subcode
== 1) {
693 // ABC axis only handle actuator max speeds
694 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
695 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
696 int c
= 'A' + i
- A_AXIS
;
697 if(gcode
->has_letter(c
)) {
698 float v
= gcode
->get_value(c
);
699 actuators
[i
]->set_max_rate(v
);
705 // this format is deprecated
706 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
707 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
708 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
709 if (gcode
->has_letter('A' + i
)) {
710 float v
= gcode
->get_value('A'+i
);
711 actuators
[i
]->set_max_rate(v
);
716 if(gcode
->subcode
== 1) check_max_actuator_speeds();
720 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
721 if (gcode
->has_letter('S')) {
722 float acc
= gcode
->get_value('S'); // mm/s^2
724 if (acc
< 1.0F
) acc
= 1.0F
;
725 this->default_acceleration
= acc
;
727 for (int i
= 0; i
< n_motors
; ++i
) {
728 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
729 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
730 if(gcode
->has_letter(axis
)) {
731 float acc
= gcode
->get_value(axis
); // mm/s^2
733 if (acc
<= 0.0F
) acc
= NAN
;
734 actuators
[i
]->set_acceleration(acc
);
739 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
740 if (gcode
->has_letter('X')) {
741 float jd
= gcode
->get_value('X');
745 THEKERNEL
->planner
->junction_deviation
= jd
;
747 if (gcode
->has_letter('Z')) {
748 float jd
= gcode
->get_value('Z');
749 // enforce minimum, -1 disables it and uses regular junction deviation
752 THEKERNEL
->planner
->z_junction_deviation
= jd
;
754 if (gcode
->has_letter('S')) {
755 float mps
= gcode
->get_value('S');
759 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
763 case 211: // M211 Sn turns soft endstops on/off
764 if(gcode
->has_letter('S')) {
765 soft_endstop_enabled
= gcode
->get_uint('S') == 1;
767 gcode
->stream
->printf("Soft endstops are %s", soft_endstop_enabled
? "Enabled" : "Disabled");
768 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
769 if(isnan(soft_endstop_min
[i
])) {
770 gcode
->stream
->printf(",%c min is disabled", 'X'+i
);
772 if(isnan(soft_endstop_max
[i
])) {
773 gcode
->stream
->printf(",%c max is disabled", 'X'+i
);
776 gcode
->stream
->printf(",%c axis is not homed", 'X'+i
);
779 gcode
->stream
->printf("\n");
783 case 220: // M220 - speed override percentage
784 if (gcode
->has_letter('S')) {
785 float factor
= gcode
->get_value('S');
786 // enforce minimum 10% speed
789 // enforce maximum 10x speed
790 if (factor
> 1000.0F
)
793 seconds_per_minute
= 6000.0F
/ factor
;
795 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
799 case 400: // wait until all moves are done up to this point
800 THEKERNEL
->conveyor
->wait_for_idle();
803 case 500: // M500 saves some volatile settings to config override file
804 case 503: { // M503 just prints the settings
805 gcode
->stream
->printf(";Steps per unit:\nM92 ");
806 for (int i
= 0; i
< n_motors
; ++i
) {
807 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
808 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
809 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
811 gcode
->stream
->printf("\n");
813 // only print if not NAN
814 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
815 for (int i
= 0; i
< n_motors
; ++i
) {
816 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
817 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
818 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
820 gcode
->stream
->printf("\n");
822 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
);
824 gcode
->stream
->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f\n", this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
826 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
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 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
832 gcode
->stream
->printf("\n");
834 // get or save any arm solution specific optional values
835 BaseSolution::arm_options_t options
;
836 if(arm_solution
->get_optional(options
) && !options
.empty()) {
837 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
838 for(auto &i
: options
) {
839 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
841 gcode
->stream
->printf("\n");
844 // save wcs_offsets and current_wcs
845 // TODO this may need to be done whenever they change to be compliant
846 gcode
->stream
->printf(";WCS settings\n");
847 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
849 for(auto &i
: wcs_offsets
) {
850 if(i
!= wcs_t(0, 0, 0)) {
852 std::tie(x
, y
, z
) = i
;
853 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
858 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
859 // 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
860 if(g92_offset
!= wcs_t(0, 0, 0)) {
862 std::tie(x
, y
, z
) = g92_offset
;
863 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
869 case 665: { // M665 set optional arm solution variables based on arm solution.
870 // the parameter args could be any letter each arm solution only accepts certain ones
871 BaseSolution::arm_options_t options
= gcode
->get_args();
872 options
.erase('S'); // don't include the S
873 options
.erase('U'); // don't include the U
874 if(options
.size() > 0) {
875 // set the specified options
876 arm_solution
->set_optional(options
);
879 if(arm_solution
->get_optional(options
)) {
880 // foreach optional value
881 for(auto &i
: options
) {
882 // print all current values of supported options
883 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
884 gcode
->add_nl
= true;
888 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
889 this->delta_segments_per_second
= gcode
->get_value('S');
890 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
892 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
893 this->mm_per_line_segment
= gcode
->get_value('U');
894 this->delta_segments_per_second
= 0;
895 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
903 if( motion_mode
!= NONE
) {
904 is_g123
= motion_mode
!= SEEK
;
905 process_move(gcode
, motion_mode
);
911 next_command_is_MCS
= false; // must be on same line as G0 or G1
914 int Robot::get_active_extruder() const
916 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
917 // find first selected extruder
918 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
923 // process a G0/G1/G2/G3
924 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
926 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
927 // get XYZ and one E (which goes to the selected extruder)
928 float param
[4]{NAN
, NAN
, NAN
, NAN
};
930 // process primary axis
931 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
933 if( gcode
->has_letter(letter
) ) {
934 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
938 float offset
[3]{0,0,0};
939 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
940 if( gcode
->has_letter(letter
) ) {
941 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
945 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
946 float target
[n_motors
];
947 memcpy(target
, machine_position
, n_motors
*sizeof(float));
949 if(!next_command_is_MCS
) {
950 if(this->absolute_mode
) {
951 // apply wcs offsets and g92 offset and tool offset
952 if(!isnan(param
[X_AXIS
])) {
953 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
);
956 if(!isnan(param
[Y_AXIS
])) {
957 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
);
960 if(!isnan(param
[Z_AXIS
])) {
961 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
);
965 // they are deltas from the machine_position if specified
966 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
967 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
972 // already in machine coordinates, we do not add wcs or tool offset for that
973 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
974 if(!isnan(param
[i
])) target
[i
] = param
[i
];
980 #if MAX_ROBOT_ACTUATORS > 3
981 // process extruder parameters, for active extruder only (only one active extruder at a time)
982 int selected_extruder
= 0;
983 if(gcode
->has_letter('E')) {
984 selected_extruder
= get_active_extruder();
985 param
[E_AXIS
]= gcode
->get_value('E');
988 // do E for the selected extruder
989 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
990 if(this->e_absolute_mode
) {
991 target
[selected_extruder
]= param
[E_AXIS
];
992 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
994 delta_e
= param
[E_AXIS
];
995 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
999 // 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
1000 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
1001 char letter
= 'A'+i
-A_AXIS
;
1002 if(gcode
->has_letter(letter
)) {
1003 float p
= gcode
->get_value(letter
);
1004 if(this->absolute_mode
) {
1007 target
[i
]= p
+ machine_position
[i
];
1013 if( gcode
->has_letter('F') ) {
1014 if( motion_mode
== SEEK
)
1015 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
1017 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
1020 // S is modal When specified on a G0/1/2/3 command
1021 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
1025 // Perform any physical actions
1026 switch(motion_mode
) {
1030 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
1034 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
1039 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
1040 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1045 // set machine_position to the calculated target
1046 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1050 // reset the machine position for all axis. Used for homing.
1051 // after homing we supply the cartesian coordinates that the head is at when homed,
1052 // however for Z this is the compensated machine position (if enabled)
1053 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1054 // this will make the results from M114 and ? consistent after homing.
1055 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1056 void Robot::reset_axis_position(float x
, float y
, float z
)
1058 // set both the same initially
1059 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1060 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1061 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1063 if(compensationTransform
) {
1064 // apply inverse transform to get machine_position
1065 compensationTransform(machine_position
, true);
1068 // now set the actuator positions based on the supplied compensated position
1069 ActuatorCoordinates actuator_pos
;
1070 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1071 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1072 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1075 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1076 void Robot::reset_axis_position(float position
, int axis
)
1078 compensated_machine_position
[axis
] = position
;
1079 if(axis
<= Z_AXIS
) {
1080 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1082 #if MAX_ROBOT_ACTUATORS > 3
1083 }else if(axis
< n_motors
) {
1084 // ABC and/or extruders need to be set as there is no arm solution for them
1085 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1086 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1091 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1092 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1093 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1095 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1096 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1099 // now correct axis positions then recorrect actuator to account for rounding
1100 reset_position_from_current_actuator_position();
1103 // Use FK to find out where actuator is and reset to match
1104 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1105 void Robot::reset_position_from_current_actuator_position()
1107 ActuatorCoordinates actuator_pos
;
1108 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1109 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1110 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1113 // discover machine position from where actuators actually are
1114 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1115 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1117 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1118 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1120 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1121 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1122 // to get everything in perfect sync.
1123 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1124 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1125 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1128 // Handle extruders and/or ABC axis
1129 #if MAX_ROBOT_ACTUATORS > 3
1130 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1131 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1132 float ap
= actuator_pos
[i
];
1133 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
1134 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1135 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1140 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1141 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1142 // all transforms and is what we actually convert to actuator positions
1143 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1145 float deltas
[n_motors
];
1146 float transformed_target
[n_motors
]; // adjust target for bed compensation
1147 float unit_vec
[N_PRIMARY_AXIS
];
1149 // unity transform by default
1150 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1152 // check function pointer and call if set to transform the target to compensate for bed
1153 if(compensationTransform
) {
1154 // some compensation strategies can transform XYZ, some just change Z
1155 compensationTransform(transformed_target
, false);
1158 // check soft endstops only for homed axis that are enabled
1159 if(soft_endstop_enabled
) {
1160 for (int i
= 0; i
<= Z_AXIS
; ++i
) {
1161 if(!is_homed(i
)) continue;
1162 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
]) ) {
1163 if(soft_endstop_halt
) {
1164 if(THEKERNEL
->is_grbl_mode()) {
1165 THEKERNEL
->streams
->printf("error: ");
1167 THEKERNEL
->streams
->printf("Error: ");
1170 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i
+'X');
1171 THEKERNEL
->call_event(ON_HALT
, nullptr);
1174 //} else if(soft_endstop_truncate) {
1175 // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
1176 // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
1180 if(THEKERNEL
->is_grbl_mode()) {
1181 THEKERNEL
->streams
->printf("error: ");
1183 THEKERNEL
->streams
->printf("Error: ");
1185 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - entire move ignored\n", i
+'X');
1194 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1196 // find distance moved by each axis, use transformed target from the current compensated machine position
1197 for (size_t i
= 0; i
< n_motors
; i
++) {
1198 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1199 if(deltas
[i
] == 0) continue;
1200 // at least one non zero delta
1202 if(i
< N_PRIMARY_AXIS
) {
1203 sos
+= powf(deltas
[i
], 2);
1208 if(!move
) return false;
1210 // see if this is a primary axis move or not
1211 bool auxilliary_move
= true;
1212 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1213 if(deltas
[i
] != 0) {
1214 auxilliary_move
= false;
1219 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1220 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1222 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1223 // 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
1224 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1227 if(!auxilliary_move
) {
1228 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1229 // find distance unit vector for primary axis only
1230 unit_vec
[i
] = deltas
[i
] / distance
;
1232 // Do not move faster than the configured cartesian limits for XYZ
1233 if ( i
<= Z_AXIS
&& max_speeds
[i
] > 0 ) {
1234 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1236 if (axis_speed
> max_speeds
[i
])
1237 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1242 // find actuator position given the machine position, use actual adjusted target
1243 ActuatorCoordinates actuator_pos
;
1244 if(!disable_arm_solution
) {
1245 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1248 // basically the same as cartesian, would be used for special homing situations like for scara
1249 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1250 actuator_pos
[i
] = transformed_target
[i
];
1254 #if MAX_ROBOT_ACTUATORS > 3
1256 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1257 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1258 actuator_pos
[i
]= transformed_target
[i
];
1259 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1260 // NOTE this relies on the fact only one extruder is active at a time
1261 // scale for volumetric or flow rate
1262 // TODO is this correct? scaling the absolute target? what if the scale changes?
1263 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1264 actuator_pos
[i
] *= get_e_scale_fnc();
1266 if(auxilliary_move
) {
1267 // for E only moves we need to use the scaled E to calculate the distance
1268 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1271 if(auxilliary_move
) {
1272 distance
= sqrtf(sos
); // distance in mm of the e move
1273 if(distance
< 0.00001F
) return false;
1277 // use default acceleration to start with
1278 float acceleration
= default_acceleration
;
1280 float isecs
= rate_mm_s
/ distance
;
1282 // check per-actuator speed limits
1283 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1284 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1285 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1287 float actuator_rate
= d
* isecs
;
1288 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1289 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1290 isecs
= rate_mm_s
/ distance
;
1293 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1294 // 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.
1295 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1296 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1297 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1298 float ca
= fabsf((d
/distance
) * acceleration
);
1300 acceleration
*= ( ma
/ ca
);
1306 // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
1307 while(THEKERNEL
->get_feed_hold()) {
1308 THEKERNEL
->call_event(ON_IDLE
, this);
1309 // if we also got a HALT then break out of this
1310 if(THEKERNEL
->is_halted()) return false;
1313 // Append the block to the planner
1314 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1315 // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
1316 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1317 // this is the new compensated machine position
1318 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1326 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1327 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1329 if(THEKERNEL
->is_halted()) return false;
1331 // catch negative or zero feed rates
1332 if(rate_mm_s
<= 0.0F
) {
1336 // get the absolute target position, default is current machine_position
1337 float target
[n_motors
];
1338 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1340 // add in the deltas to get new target
1341 for (int i
= 0; i
< naxis
; i
++) {
1342 target
[i
] += delta
[i
];
1345 // submit for planning and if moved update machine_position
1346 if(append_milestone(target
, rate_mm_s
)) {
1347 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1354 // Append a move to the queue ( cutting it into segments if needed )
1355 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1357 // catch negative or zero feed rates and return the same error as GRBL does
1358 if(rate_mm_s
<= 0.0F
) {
1359 gcode
->is_error
= true;
1360 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1364 // Find out the distance for this move in XYZ in MCS
1365 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 ));
1367 if(millimeters_of_travel
< 0.00001F
) {
1368 // we have no movement in XYZ, probably E only extrude or retract
1369 return this->append_milestone(target
, rate_mm_s
);
1373 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1374 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1375 We ask Extruder to do all the work but we need to pass in the relevant data.
1376 NOTE we need to do this before we segment the line (for deltas)
1378 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1379 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1380 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1381 rate_mm_s
*= data
[1]; // adjust the feedrate
1385 // 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.
1386 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1387 // 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
1390 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1393 } else if(this->delta_segments_per_second
> 1.0F
) {
1394 // enabled if set to something > 1, it is set to 0.0 by default
1395 // segment based on current speed and requested segments per second
1396 // the faster the travel speed the fewer segments needed
1397 // NOTE rate is mm/sec and we take into account any speed override
1398 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1399 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1400 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1403 if(this->mm_per_line_segment
== 0.0F
) {
1404 segments
= 1; // don't split it up
1406 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1412 // A vector to keep track of the endpoint of each segment
1413 float segment_delta
[n_motors
];
1414 float segment_end
[n_motors
];
1415 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1417 // How far do we move each segment?
1418 for (int i
= 0; i
< n_motors
; i
++)
1419 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1421 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1422 // We always add another point after this loop so we stop at segments-1, ie i < segments
1423 for (int i
= 1; i
< segments
; i
++) {
1424 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1425 for (int j
= 0; j
< n_motors
; j
++)
1426 segment_end
[j
] += segment_delta
[j
];
1428 // Append the end of this segment to the queue
1429 // this can block waiting for free block queue or if in feed hold
1430 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1435 // Append the end of this full move to the queue
1436 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1438 this->next_command_is_MCS
= false; // always reset this
1444 // Append an arc to the queue ( cutting it into segments as needed )
1445 // TODO does not support any E parameters so cannot be used for 3D printing.
1446 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1448 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1449 // catch negative or zero feed rates and return the same error as GRBL does
1450 if(rate_mm_s
<= 0.0F
) {
1451 gcode
->is_error
= true;
1452 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1457 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1458 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1459 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1460 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1461 float r_axis1
= -offset
[this->plane_axis_1
];
1462 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1463 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1464 float angular_travel
= 0;
1466 gcode
->stream
->printf("Mpos Plane_Axis_0: %8.34f\r\n", machine_position
[this->plane_axis_0
]);
1467 gcode
->stream
->printf("Mpos Plane_Axis_1: %8.34f\r\n", machine_position
[this->plane_axis_1
]);
1468 gcode
->stream
->printf("Offset Plane_Axis_0: %8.34f\r\n", offset
[this->plane_axis_0
]);
1469 gcode
->stream
->printf("Offset Plane_Axis_1: %8.34f\r\n", offset
[this->plane_axis_1
]);
1470 gcode
->stream
->printf("Target Plane_Axis_0: %8.34f\r\n", target
[this->plane_axis_0
]);
1471 gcode
->stream
->printf("Target Plane_Axis_1: %8.34f\r\n", target
[this->plane_axis_1
]);
1472 gcode
->stream
->printf("center_axis0: %8.34f\r\n", center_axis0
);
1473 gcode
->stream
->printf("center_axis1: %8.34f\r\n", center_axis1
);
1474 gcode
->stream
->printf("Radius: %8.34f\r\n",radius
);
1475 gcode
->stream
->printf("r_axis0: %8.34f\r\n",r_axis0
);
1476 gcode
->stream
->printf("rt_axis0: %8.34f\r\n",rt_axis0
);
1477 gcode
->stream
->printf("r_axis1: %8.34f\r\n",r_axis1
);
1478 gcode
->stream
->printf("rt_axis1: %8.34f\r\n",rt_axis1
);
1479 gcode
->stream
->printf("ARC_ANGULAR_TRAVEL_EPSILON: %8.64f\r\n",ARC_ANGULAR_TRAVEL_EPSILON
);
1481 if((this->machine_position
[this->plane_axis_0
]==target
[this->plane_axis_0
]) and(this->machine_position
[this->plane_axis_1
]==target
[this->plane_axis_1
])) {
1482 gcode
->stream
->printf("Full Circle: True\r\n");
1484 angular_travel
= (-2 * PI
);
1486 angular_travel
= (2 * PI
);
1488 gcode
->stream
->printf("Full Circle angular_travel: %8.34f\r\n",angular_travel
);
1490 gcode
->stream
->printf("Full Circle: False\r\n");
1491 // Patch from GRBL Firmware - Christoph Baumann 04072015
1492 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1493 // Only run if not a full circle
1494 angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1495 gcode
->stream
->printf("initial angular_travel1: %8.34f\r\n",angular_travel
);
1496 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is reverse of other 2 planes
1497 if (is_clockwise
) { // Correct atan2 output per direction
1498 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1500 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1502 gcode
->stream
->printf("old angular_travel2: %8.34f\r\n",angular_travel
);
1504 angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1506 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is reverse of other 2 planes
1507 if (is_clockwise
) { // Correct atan2 output per direction
1508 if (angular_travel
> 0) { angular_travel
-= (2 * PI
); }
1510 if (angular_travel
< 0) { angular_travel
+= (2 * PI
); }
1512 gcode
->stream
->printf("new angular_travel2: %8.34f\r\n",angular_travel
);
1516 // Find the distance for this gcode
1517 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1518 gcode
->stream
->printf("millimeters_of_travel:%8.34f\r\n",millimeters_of_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
]);
1631 // Set clockwise/counter-clockwise sign for mc_arc computations
1632 bool is_clockwise
= false;
1633 if( motion_mode
== CW_ARC
) {
1634 is_clockwise
= true;
1638 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1642 float Robot::theta(float x
, float y
)
1644 float t
= atanf(x
/ fabs(y
));
1656 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1658 this->plane_axis_0
= axis_0
;
1659 this->plane_axis_1
= axis_1
;
1660 this->plane_axis_2
= axis_2
;
1663 void Robot::clearToolOffset()
1665 this->tool_offset
= wcs_t(0,0,0);
1668 void Robot::setToolOffset(const float offset
[3])
1670 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1673 float Robot::get_feed_rate() const
1675 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;
1678 bool Robot::is_homed(uint8_t i
) const
1680 if(i
>= 3) return false; // safety
1682 // if we are homing we ignore soft endstops so return false
1684 bool ok
= PublicData::get_value(endstops_checksum
, get_homing_status_checksum
, 0, &homing
);
1685 if(!ok
|| homing
) return false;
1687 // check individual axis homing status
1689 ok
= PublicData::get_value(endstops_checksum
, get_homed_status_checksum
, 0, homed
);
1690 if(!ok
) return false;