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"
11 #include "mbed.h" // for us_ticker_read()
20 #include "nuts_bolts.h"
22 #include "StepperMotor.h"
24 #include "PublicDataRequest.h"
25 #include "PublicData.h"
26 #include "arm_solutions/BaseSolution.h"
27 #include "arm_solutions/CartesianSolution.h"
28 #include "arm_solutions/RotatableCartesianSolution.h"
29 #include "arm_solutions/LinearDeltaSolution.h"
30 #include "arm_solutions/RotaryDeltaSolution.h"
31 #include "arm_solutions/HBotSolution.h"
32 #include "arm_solutions/CoreXZSolution.h"
33 #include "arm_solutions/MorganSCARASolution.h"
34 #include "StepTicker.h"
35 #include "checksumm.h"
37 #include "ConfigValue.h"
38 #include "libs/StreamOutput.h"
39 #include "StreamOutputPool.h"
40 #include "ExtruderPublicAccess.h"
41 #include "GcodeDispatch.h"
44 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
45 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
46 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
47 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
48 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
49 #define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
50 #define arc_correction_checksum CHECKSUM("arc_correction")
51 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
52 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
53 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
54 #define segment_z_moves_checksum CHECKSUM("segment_z_moves")
55 #define save_g92_checksum CHECKSUM("save_g92")
58 #define arm_solution_checksum CHECKSUM("arm_solution")
59 #define cartesian_checksum CHECKSUM("cartesian")
60 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
61 #define rostock_checksum CHECKSUM("rostock")
62 #define linear_delta_checksum CHECKSUM("linear_delta")
63 #define rotary_delta_checksum CHECKSUM("rotary_delta")
64 #define delta_checksum CHECKSUM("delta")
65 #define hbot_checksum CHECKSUM("hbot")
66 #define corexy_checksum CHECKSUM("corexy")
67 #define corexz_checksum CHECKSUM("corexz")
68 #define kossel_checksum CHECKSUM("kossel")
69 #define morgan_checksum CHECKSUM("morgan")
71 // new-style actuator stuff
72 #define actuator_checksum CHEKCSUM("actuator")
74 #define step_pin_checksum CHECKSUM("step_pin")
75 #define dir_pin_checksum CHEKCSUM("dir_pin")
76 #define en_pin_checksum CHECKSUM("en_pin")
78 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
79 #define max_rate_checksum CHECKSUM("max_rate")
81 #define alpha_checksum CHECKSUM("alpha")
82 #define beta_checksum CHECKSUM("beta")
83 #define gamma_checksum CHECKSUM("gamma")
85 #define NEXT_ACTION_DEFAULT 0
86 #define NEXT_ACTION_DWELL 1
87 #define NEXT_ACTION_GO_HOME 2
89 #define MOTION_MODE_SEEK 0 // G0
90 #define MOTION_MODE_LINEAR 1 // G1
91 #define MOTION_MODE_CW_ARC 2 // G2
92 #define MOTION_MODE_CCW_ARC 3 // G3
93 #define MOTION_MODE_CANCEL 4 // G80
95 #define PATH_CONTROL_MODE_EXACT_PATH 0
96 #define PATH_CONTROL_MODE_EXACT_STOP 1
97 #define PATH_CONTROL_MODE_CONTINOUS 2
99 #define PROGRAM_FLOW_RUNNING 0
100 #define PROGRAM_FLOW_PAUSED 1
101 #define PROGRAM_FLOW_COMPLETED 2
103 #define SPINDLE_DIRECTION_CW 0
104 #define SPINDLE_DIRECTION_CCW 1
106 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
108 // 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
109 // It takes care of cutting arcs into segments, same thing for line that are too long
113 this->inch_mode
= false;
114 this->absolute_mode
= true;
115 this->motion_mode
= MOTION_MODE_SEEK
;
116 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
117 clear_vector(this->last_milestone
);
118 clear_vector(this->last_machine_position
);
119 this->arm_solution
= NULL
;
120 seconds_per_minute
= 60.0F
;
121 this->clearToolOffset();
122 this->compensationTransform
= nullptr;
123 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
124 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
125 this->next_command_is_MCS
= false;
126 this->disable_segmentation
= false;
129 //Called when the module has just been loaded
130 void Robot::on_module_loaded()
132 this->register_for_event(ON_GCODE_RECEIVED
);
138 #define ACTUATOR_CHECKSUMS(X) { \
139 CHECKSUM(X "_step_pin"), \
140 CHECKSUM(X "_dir_pin"), \
141 CHECKSUM(X "_en_pin"), \
142 CHECKSUM(X "_steps_per_mm"), \
143 CHECKSUM(X "_max_rate") \
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.5f
)->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 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
;
192 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
193 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
195 // Make our 3 StepperMotors
196 uint16_t const checksums
[][5] = {
197 ACTUATOR_CHECKSUMS("alpha"),
198 ACTUATOR_CHECKSUMS("beta"),
199 ACTUATOR_CHECKSUMS("gamma"),
200 #if MAX_ROBOT_ACTUATORS > 3
201 ACTUATOR_CHECKSUMS("delta"),
202 ACTUATOR_CHECKSUMS("epsilon"),
203 ACTUATOR_CHECKSUMS("zeta")
206 constexpr size_t actuator_checksum_count
= sizeof(checksums
) / sizeof(checksums
[0]);
207 static_assert(actuator_checksum_count
>= k_max_actuators
, "Robot checksum array too small for k_max_actuators");
209 size_t motor_count
= std::min(this->arm_solution
->get_actuator_count(), k_max_actuators
);
210 for (size_t a
= 0; a
< motor_count
; a
++) {
211 Pin pins
[3]; //step, dir, enable
212 for (size_t i
= 0; i
< 3; i
++) {
213 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
215 actuators
[a
] = new StepperMotor(pins
[0], pins
[1], pins
[2]);
217 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
218 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(checksums
[a
][4])->by_default(30000.0F
)->as_number());
221 check_max_actuator_speeds(); // check the configs are sane
223 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
224 // so the first move can be correct if homing is not performed
225 ActuatorCoordinates actuator_pos
;
226 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
227 for (size_t i
= 0; i
< actuators
.size(); i
++)
228 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
230 //this->clearToolOffset();
233 void Robot::push_state()
235 bool am
= this->absolute_mode
;
236 bool im
= this->inch_mode
;
237 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, im
, current_wcs
);
241 void Robot::pop_state()
243 if(!state_stack
.empty()) {
244 auto s
= state_stack
.top();
246 this->feed_rate
= std::get
<0>(s
);
247 this->seek_rate
= std::get
<1>(s
);
248 this->absolute_mode
= std::get
<2>(s
);
249 this->inch_mode
= std::get
<3>(s
);
250 this->current_wcs
= std::get
<4>(s
);
254 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
256 std::vector
<wcs_t
> v
;
257 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
258 for(auto& i
: wcs_offsets
) {
261 v
.push_back(g92_offset
);
262 v
.push_back(tool_offset
);
266 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
268 // M114.1 is a new way to do this (similar to how GRBL does it).
269 // it returns the realtime position based on the current step position of the actuators.
270 // this does require a FK to get a machine position from the actuator position
271 // and then invert all the transforms to get a workspace position from machine position
272 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
274 if(subcode
== 0) { // M114 print WCS
275 wcs_t pos
= mcs2wcs(last_milestone
);
276 n
= snprintf(buf
, bufsize
, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get
<X_AXIS
>(pos
)), from_millimeters(std::get
<Y_AXIS
>(pos
)), from_millimeters(std::get
<Z_AXIS
>(pos
)));
278 } else if(subcode
== 4) { // M114.4 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
279 n
= snprintf(buf
, bufsize
, "LMS: X:%1.4f Y:%1.4f Z:%1.4f", last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
281 } else if(subcode
== 5) { // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
282 n
= snprintf(buf
, bufsize
, "LMP: X:%1.4f Y:%1.4f Z:%1.4f", last_machine_position
[X_AXIS
], last_machine_position
[Y_AXIS
], last_machine_position
[Z_AXIS
]);
285 // get real time positions
286 // current actuator position in mm
287 ActuatorCoordinates current_position
{
288 actuators
[X_AXIS
]->get_current_position(),
289 actuators
[Y_AXIS
]->get_current_position(),
290 actuators
[Z_AXIS
]->get_current_position()
293 // get machine position from the actuator position using FK
295 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
297 if(subcode
== 1) { // M114.1 print realtime WCS
298 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
299 wcs_t pos
= mcs2wcs(mpos
);
300 n
= snprintf(buf
, bufsize
, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get
<X_AXIS
>(pos
)), from_millimeters(std::get
<Y_AXIS
>(pos
)), from_millimeters(std::get
<Z_AXIS
>(pos
)));
302 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
303 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
305 } else if(subcode
== 3) { // M114.3 print realtime actuator position
306 n
= snprintf(buf
, bufsize
, "APOS: A:%1.4f B:%1.4f C:%1.4f", current_position
[X_AXIS
], current_position
[Y_AXIS
], current_position
[Z_AXIS
]);
312 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
313 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
315 return std::make_tuple(
316 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
),
317 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
),
318 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
)
322 // this does a sanity check that actuator speeds do not exceed steps rate capability
323 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
324 void Robot::check_max_actuator_speeds()
326 for (size_t i
= 0; i
< actuators
.size(); i
++) {
327 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
328 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
329 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
330 THEKERNEL
->streams
->printf("WARNING: actuator %c rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", 'A' + i
, step_freq
, actuators
[i
]->max_rate
);
335 //A GCode has been received
336 //See if the current Gcode line has some orders for us
337 void Robot::on_gcode_received(void *argument
)
339 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
341 this->motion_mode
= -1;
345 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
346 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
347 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
348 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
349 case 4: { // G4 pause
350 uint32_t delay_ms
= 0;
351 if (gcode
->has_letter('P')) {
352 delay_ms
= gcode
->get_int('P');
354 if (gcode
->has_letter('S')) {
355 delay_ms
+= gcode
->get_int('S') * 1000;
359 THEKERNEL
->conveyor
->wait_for_empty_queue();
360 // wait for specified time
361 uint32_t start
= us_ticker_read(); // mbed call
362 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
363 THEKERNEL
->call_event(ON_IDLE
, this);
364 if(THEKERNEL
->is_halted()) return;
370 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
371 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
372 size_t n
= gcode
->get_uint('P');
373 if(n
== 0) n
= current_wcs
; // set current coordinate system
377 std::tie(x
, y
, z
) = wcs_offsets
[n
];
378 if(gcode
->get_int('L') == 20) {
379 // this makes the current machine position (less compensation transform) the offset
380 // get current position in WCS
381 wcs_t pos
= mcs2wcs(last_milestone
);
383 if(gcode
->has_letter('X')){
384 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
387 if(gcode
->has_letter('Y')){
388 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
390 if(gcode
->has_letter('Z')) {
391 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
395 // the value is the offset from machine zero
396 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
397 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
398 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
400 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
405 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
406 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
407 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
408 case 20: this->inch_mode
= true; break;
409 case 21: this->inch_mode
= false; break;
411 case 54: case 55: case 56: case 57: case 58: case 59:
412 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
413 current_wcs
= gcode
->g
- 54;
414 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
415 current_wcs
+= gcode
->subcode
;
416 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
420 case 90: this->absolute_mode
= true; break;
421 case 91: this->absolute_mode
= false; break;
424 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
425 // reset G92 offsets to 0
426 g92_offset
= wcs_t(0, 0, 0);
428 } else if(gcode
->subcode
== 3) {
429 // initialize G92 to the specified values, only used for saving it with M500
430 float x
= 0, y
= 0, z
= 0;
431 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
432 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
433 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
434 g92_offset
= wcs_t(x
, y
, z
);
437 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
439 std::tie(x
, y
, z
) = g92_offset
;
440 // get current position in WCS
441 wcs_t pos
= mcs2wcs(last_milestone
);
443 // adjust g92 offset to make the current wpos == the value requested
444 if(gcode
->has_letter('X')){
445 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
447 if(gcode
->has_letter('Y')){
448 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
450 if(gcode
->has_letter('Z')) {
451 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
453 g92_offset
= wcs_t(x
, y
, z
);
460 } else if( gcode
->has_m
) {
462 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
463 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
466 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
467 if(!THEKERNEL
->is_grbl_mode()) break;
468 // fall through to M2
469 case 2: // M2 end of program
471 absolute_mode
= true;
474 case 92: // M92 - set steps per mm
475 if (gcode
->has_letter('X'))
476 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
477 if (gcode
->has_letter('Y'))
478 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
479 if (gcode
->has_letter('Z'))
480 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
482 gcode
->stream
->printf("X:%f Y:%f Z:%f ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
483 gcode
->add_nl
= true;
484 check_max_actuator_speeds();
489 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
490 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
494 case 120: // push state
498 case 121: // pop state
502 case 203: // M203 Set maximum feedrates in mm/sec
503 if (gcode
->has_letter('X'))
504 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
505 if (gcode
->has_letter('Y'))
506 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
507 if (gcode
->has_letter('Z'))
508 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
509 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
510 if (gcode
->has_letter('A' + i
))
511 actuators
[i
]->set_max_rate(gcode
->get_value('A' + i
));
513 check_max_actuator_speeds();
515 if(gcode
->get_num_args() == 0) {
516 gcode
->stream
->printf("X:%g Y:%g Z:%g",
517 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
518 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
519 gcode
->stream
->printf(" %c : %g", 'A' + i
, actuators
[i
]->get_max_rate()); //xxx
521 gcode
->add_nl
= true;
525 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
526 if (gcode
->has_letter('S')) {
527 float acc
= gcode
->get_value('S'); // mm/s^2
531 THEKERNEL
->planner
->acceleration
= acc
;
533 if (gcode
->has_letter('Z')) {
534 float acc
= gcode
->get_value('Z'); // mm/s^2
538 THEKERNEL
->planner
->z_acceleration
= acc
;
542 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
543 if (gcode
->has_letter('X')) {
544 float jd
= gcode
->get_value('X');
548 THEKERNEL
->planner
->junction_deviation
= jd
;
550 if (gcode
->has_letter('Z')) {
551 float jd
= gcode
->get_value('Z');
552 // enforce minimum, -1 disables it and uses regular junction deviation
555 THEKERNEL
->planner
->z_junction_deviation
= jd
;
557 if (gcode
->has_letter('S')) {
558 float mps
= gcode
->get_value('S');
562 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
564 if (gcode
->has_letter('Y')) {
565 actuators
[0]->default_minimum_actuator_rate
= gcode
->get_value('Y');
569 case 220: // M220 - speed override percentage
570 if (gcode
->has_letter('S')) {
571 float factor
= gcode
->get_value('S');
572 // enforce minimum 10% speed
575 // enforce maximum 10x speed
576 if (factor
> 1000.0F
)
579 seconds_per_minute
= 6000.0F
/ factor
;
581 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
585 case 400: // wait until all moves are done up to this point
586 THEKERNEL
->conveyor
->wait_for_empty_queue();
589 case 500: // M500 saves some volatile settings to config override file
590 case 503: { // M503 just prints the settings
591 gcode
->stream
->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
592 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
593 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
, THEKERNEL
->planner
->z_junction_deviation
, THEKERNEL
->planner
->minimum_planner_speed
);
594 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f",
595 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
596 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
597 gcode
->stream
->printf(" %c%1.5f", 'A' + i
, actuators
[i
]->get_max_rate());
599 gcode
->stream
->printf("\n");
601 // get or save any arm solution specific optional values
602 BaseSolution::arm_options_t options
;
603 if(arm_solution
->get_optional(options
) && !options
.empty()) {
604 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
605 for(auto &i
: options
) {
606 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
608 gcode
->stream
->printf("\n");
611 // save wcs_offsets and current_wcs
612 // TODO this may need to be done whenever they change to be compliant
613 gcode
->stream
->printf(";WCS settings\n");
614 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
616 for(auto &i
: wcs_offsets
) {
617 if(i
!= wcs_t(0, 0, 0)) {
619 std::tie(x
, y
, z
) = i
;
620 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
625 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
626 // 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
627 if(g92_offset
!= wcs_t(0, 0, 0)) {
629 std::tie(x
, y
, z
) = g92_offset
;
630 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
636 case 665: { // M665 set optional arm solution variables based on arm solution.
637 // the parameter args could be any letter each arm solution only accepts certain ones
638 BaseSolution::arm_options_t options
= gcode
->get_args();
639 options
.erase('S'); // don't include the S
640 options
.erase('U'); // don't include the U
641 if(options
.size() > 0) {
642 // set the specified options
643 arm_solution
->set_optional(options
);
646 if(arm_solution
->get_optional(options
)) {
647 // foreach optional value
648 for(auto &i
: options
) {
649 // print all current values of supported options
650 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
651 gcode
->add_nl
= true;
655 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
656 this->delta_segments_per_second
= gcode
->get_value('S');
657 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
659 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
660 this->mm_per_line_segment
= gcode
->get_value('U');
661 this->delta_segments_per_second
= 0;
662 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
670 if( this->motion_mode
>= 0) {
674 next_command_is_MCS
= false; // must be on same line as G0 or G1
677 // process a G0/G1/G2/G3
678 void Robot::process_move(Gcode
*gcode
)
680 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
681 float param
[3]{NAN
, NAN
, NAN
};
682 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
684 if( gcode
->has_letter(letter
) ) {
685 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
689 float offset
[3]{0,0,0};
690 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
691 if( gcode
->has_letter(letter
) ) {
692 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
696 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
697 float target
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
698 if(!next_command_is_MCS
) {
699 if(this->absolute_mode
) {
700 // apply wcs offsets and g92 offset and tool offset
701 if(!isnan(param
[X_AXIS
])) {
702 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
);
705 if(!isnan(param
[Y_AXIS
])) {
706 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
);
709 if(!isnan(param
[Z_AXIS
])) {
710 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
);
714 // they are deltas from the last_milestone if specified
715 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
716 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
721 // already in machine coordinates, we do not add tool offset for that
722 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
723 if(!isnan(param
[i
])) target
[i
] = param
[i
];
727 if( gcode
->has_letter('F') ) {
728 if( this->motion_mode
== MOTION_MODE_SEEK
)
729 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
731 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
735 //Perform any physical actions
736 switch(this->motion_mode
) {
737 case MOTION_MODE_CANCEL
:
739 case MOTION_MODE_SEEK
:
740 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
);
742 case MOTION_MODE_LINEAR
:
743 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
745 case MOTION_MODE_CW_ARC
:
746 case MOTION_MODE_CCW_ARC
:
747 moved
= this->compute_arc(gcode
, offset
, target
);
752 // set last_milestone to the calculated target
753 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
));
757 // We received a new gcode, and one of the functions
758 // determined the distance for that given gcode. So now we can attach this gcode to the right block
760 void Robot::distance_in_gcode_is_known(Gcode
* gcode
)
762 //If the queue is empty, execute immediately, otherwise attach to the last added block
763 THEKERNEL
->conveyor
->append_gcode(gcode
);
766 // reset the machine position for all axis. Used for homing.
767 // During homing compensation is turned off (actually not used as it drives steppers directly)
768 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
769 // so in those cases the final position is compensated.
770 void Robot::reset_axis_position(float x
, float y
, float z
)
772 // these are set to the same as compensation was not used to get to the current position
773 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
774 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
775 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
777 // now set the actuator positions to match
778 ActuatorCoordinates actuator_pos
;
779 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
780 for (size_t i
= 0; i
< actuators
.size(); i
++)
781 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
784 // Reset the position for an axis (used in homing)
785 void Robot::reset_axis_position(float position
, int axis
)
787 last_milestone
[axis
] = position
;
788 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
791 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
792 // then sets the axis positions to match. currently only called from Endstops.cpp
793 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
795 for (size_t i
= 0; i
< actuators
.size(); i
++)
796 actuators
[i
]->change_last_milestone(ac
[i
]);
798 // now correct axis positions then recorrect actuator to account for rounding
799 reset_position_from_current_actuator_position();
802 // Use FK to find out where actuator is and reset to match
803 void Robot::reset_position_from_current_actuator_position()
805 ActuatorCoordinates actuator_pos
;
806 for (size_t i
= 0; i
< actuators
.size(); i
++) {
807 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
808 actuator_pos
[i
] = actuators
[i
]->get_current_position();
811 // discover machine position from where actuators actually are
812 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
813 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
814 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
816 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
817 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
818 // to get everything in perfect sync.
819 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
820 for (size_t i
= 0; i
< actuators
.size(); i
++)
821 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
824 // Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
825 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
826 // all transforms and is what we actually convert to actuator positions
827 bool Robot::append_milestone(Gcode
* gcode
, const float target
[], float rate_mm_s
)
831 ActuatorCoordinates actuator_pos
;
832 float transformed_target
[3]; // adjust target for bed compensation and WCS offsets
833 float millimeters_of_travel
;
835 // catch negative or zero feed rates and return the same error as GRBL does
836 if(rate_mm_s
<= 0.0F
) {
837 gcode
->is_error
= true;
838 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
842 // unity transform by default
843 memcpy(transformed_target
, target
, sizeof(transformed_target
));
845 // check function pointer and call if set to transform the target to compensate for bed
846 if(compensationTransform
) {
847 // some compensation strategies can transform XYZ, some just change Z
848 compensationTransform(transformed_target
);
851 // find distance moved by each axis, use transformed target from the current machine position
852 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
853 deltas
[axis
] = transformed_target
[axis
] - last_machine_position
[axis
];
856 // Compute how long this move moves, so we can attach it to the block for later use
857 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
859 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
860 // 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
861 if(millimeters_of_travel
< 0.00001F
) return false;
863 // this is the machine position
864 memcpy(this->last_machine_position
, transformed_target
, sizeof(this->last_machine_position
));
866 // find distance unit vector
867 for (int i
= 0; i
< 3; i
++)
868 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
870 // Do not move faster than the configured cartesian limits
871 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
872 if ( max_speeds
[axis
] > 0 ) {
873 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
875 if (axis_speed
> max_speeds
[axis
])
876 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
880 // find actuator position given the machine position, use actual adjusted target
881 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
883 float isecs
= rate_mm_s
/ millimeters_of_travel
;
884 // check per-actuator speed limits
885 for (size_t actuator
= 0; actuator
< actuators
.size(); actuator
++) {
886 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
887 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
888 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
889 isecs
= rate_mm_s
/ millimeters_of_travel
;
893 // Append the block to the planner
894 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
899 // Append a move to the queue ( cutting it into segments if needed )
900 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
)
902 // Find out the distance for this move in MCS
903 // NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
904 gcode
->millimeters_of_travel
= sqrtf(powf( target
[X_AXIS
] - last_milestone
[X_AXIS
], 2 ) + powf( target
[Y_AXIS
] - last_milestone
[Y_AXIS
], 2 ) + powf( target
[Z_AXIS
] - last_milestone
[Z_AXIS
], 2 ));
906 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
907 if( gcode
->millimeters_of_travel
< 0.00001F
) return false;
909 // Mark the gcode as having a known distance
910 this->distance_in_gcode_is_known( gcode
);
912 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
913 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
914 // We also check we are not exceeding the E max_speed for the current extruder
915 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
916 // we need to ask it now passing in the relevant data.
917 // NOTE we need to do this before we segment the line (for deltas)
918 if(gcode
->has_letter('E')) {
920 data
[0] = gcode
->get_value('E'); // E target (may be absolute or relative)
921 data
[1] = rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
922 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
923 rate_mm_s
*= data
[1];
924 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
928 // 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.
929 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
930 // 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
933 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
936 } else if(this->delta_segments_per_second
> 1.0F
) {
937 // enabled if set to something > 1, it is set to 0.0 by default
938 // segment based on current speed and requested segments per second
939 // the faster the travel speed the fewer segments needed
940 // NOTE rate is mm/sec and we take into account any speed override
941 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
942 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
943 // TODO if we are only moving in Z on a delta we don't really need to segment at all
946 if(this->mm_per_line_segment
== 0.0F
) {
947 segments
= 1; // don't split it up
949 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
955 // A vector to keep track of the endpoint of each segment
956 float segment_delta
[3];
957 float segment_end
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
959 // How far do we move each segment?
960 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
961 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
963 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
964 // We always add another point after this loop so we stop at segments-1, ie i < segments
965 for (int i
= 1; i
< segments
; i
++) {
966 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
967 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
968 segment_end
[axis
] += segment_delta
[axis
];
970 // Append the end of this segment to the queue
971 bool b
= this->append_milestone(gcode
, segment_end
, rate_mm_s
);
976 // Append the end of this full move to the queue
977 if(this->append_milestone(gcode
, target
, rate_mm_s
)) moved
= true;
979 this->next_command_is_MCS
= false; // always reset this
982 // if adding these blocks didn't start executing, do that now
983 THEKERNEL
->conveyor
->ensure_running();
990 // Append an arc to the queue ( cutting it into segments as needed )
991 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
995 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
996 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
997 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
998 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
999 float r_axis1
= -offset
[this->plane_axis_1
];
1000 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1001 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1003 // Patch from GRBL Firmware - Christoph Baumann 04072015
1004 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1005 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1006 if (is_clockwise
) { // Correct atan2 output per direction
1007 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * (float)M_PI
); }
1009 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * (float)M_PI
); }
1012 // Find the distance for this gcode
1013 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1015 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1016 if( gcode
->millimeters_of_travel
< 0.00001F
) {
1020 // Mark the gcode as having a known distance
1021 this->distance_in_gcode_is_known( gcode
);
1023 // limit segments by maximum arc error
1024 float arc_segment
= this->mm_per_arc_segment
;
1025 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1026 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1027 if (this->mm_per_arc_segment
< min_err_segment
) {
1028 arc_segment
= min_err_segment
;
1031 // Figure out how many segments for this gcode
1032 uint16_t segments
= ceilf((gcode
->millimeters_of_travel
/ arc_segment
));
1034 printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius
,arc_segment
,segments
); // Testing Purposes ONLY
1035 float theta_per_segment
= angular_travel
/ segments
;
1036 float linear_per_segment
= linear_travel
/ segments
;
1038 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1039 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1040 r_T = [cos(phi) -sin(phi);
1041 sin(phi) cos(phi] * r ;
1042 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1043 defined from the circle center to the initial position. Each line segment is formed by successive
1044 vector rotations. This requires only two cos() and sin() computations to form the rotation
1045 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1046 all float numbers are single precision on the Arduino. (True float precision will not have
1047 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1048 tool precision in some cases. Therefore, arc path correction is implemented.
1050 Small angle approximation may be used to reduce computation overhead further. This approximation
1051 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1052 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1053 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1054 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1055 issue for CNC machines with the single precision Arduino calculations.
1056 This approximation also allows mc_arc to immediately insert a line segment into the planner
1057 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1058 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1059 This is important when there are successive arc motions.
1061 // Vector rotation matrix values
1062 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1063 float sin_T
= theta_per_segment
;
1065 float arc_target
[3];
1072 // Initialize the linear axis
1073 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1076 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1077 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1079 if (count
< this->arc_correction
) {
1080 // Apply vector rotation matrix
1081 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1082 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1086 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1087 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1088 cos_Ti
= cosf(i
* theta_per_segment
);
1089 sin_Ti
= sinf(i
* theta_per_segment
);
1090 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1091 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1095 // Update arc_target location
1096 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1097 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1098 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1100 // Append this segment to the queue
1101 bool b
= this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
1105 // Ensure last segment arrives at target location.
1106 if(this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
)) moved
= true;
1111 // Do the math for an arc and add it to the queue
1112 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[])
1116 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1118 // Set clockwise/counter-clockwise sign for mc_arc computations
1119 bool is_clockwise
= false;
1120 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
1121 is_clockwise
= true;
1125 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1129 float Robot::theta(float x
, float y
)
1131 float t
= atanf(x
/ fabs(y
));
1143 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1145 this->plane_axis_0
= axis_0
;
1146 this->plane_axis_1
= axis_1
;
1147 this->plane_axis_2
= axis_2
;
1150 void Robot::clearToolOffset()
1152 this->tool_offset
= wcs_t(0,0,0);
1155 void Robot::setToolOffset(const float offset
[3])
1157 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1160 float Robot::get_feed_rate() const
1162 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;