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/RotatableDeltaSolution.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 arc_correction_checksum CHECKSUM("arc_correction")
50 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
51 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
52 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
55 #define arm_solution_checksum CHECKSUM("arm_solution")
56 #define cartesian_checksum CHECKSUM("cartesian")
57 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
58 #define rostock_checksum CHECKSUM("rostock")
59 #define linear_delta_checksum CHECKSUM("linear_delta")
60 #define rotatable_delta_checksum CHECKSUM("rotatable_delta")
61 #define delta_checksum CHECKSUM("delta")
62 #define hbot_checksum CHECKSUM("hbot")
63 #define corexy_checksum CHECKSUM("corexy")
64 #define corexz_checksum CHECKSUM("corexz")
65 #define kossel_checksum CHECKSUM("kossel")
66 #define morgan_checksum CHECKSUM("morgan")
68 // new-style actuator stuff
69 #define actuator_checksum CHEKCSUM("actuator")
71 #define step_pin_checksum CHECKSUM("step_pin")
72 #define dir_pin_checksum CHEKCSUM("dir_pin")
73 #define en_pin_checksum CHECKSUM("en_pin")
75 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
76 #define max_rate_checksum CHECKSUM("max_rate")
78 #define alpha_checksum CHECKSUM("alpha")
79 #define beta_checksum CHECKSUM("beta")
80 #define gamma_checksum CHECKSUM("gamma")
82 #define NEXT_ACTION_DEFAULT 0
83 #define NEXT_ACTION_DWELL 1
84 #define NEXT_ACTION_GO_HOME 2
86 #define MOTION_MODE_SEEK 0 // G0
87 #define MOTION_MODE_LINEAR 1 // G1
88 #define MOTION_MODE_CW_ARC 2 // G2
89 #define MOTION_MODE_CCW_ARC 3 // G3
90 #define MOTION_MODE_CANCEL 4 // G80
92 #define PATH_CONTROL_MODE_EXACT_PATH 0
93 #define PATH_CONTROL_MODE_EXACT_STOP 1
94 #define PATH_CONTROL_MODE_CONTINOUS 2
96 #define PROGRAM_FLOW_RUNNING 0
97 #define PROGRAM_FLOW_PAUSED 1
98 #define PROGRAM_FLOW_COMPLETED 2
100 #define SPINDLE_DIRECTION_CW 0
101 #define SPINDLE_DIRECTION_CCW 1
103 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
105 // 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
106 // It takes care of cutting arcs into segments, same thing for line that are too long
110 this->inch_mode
= false;
111 this->absolute_mode
= true;
112 this->motion_mode
= MOTION_MODE_SEEK
;
113 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
114 clear_vector(this->last_milestone
);
115 clear_vector(this->last_machine_position
);
116 this->arm_solution
= NULL
;
117 seconds_per_minute
= 60.0F
;
118 this->clearToolOffset();
119 this->compensationTransform
= 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;
125 //Called when the module has just been loaded
126 void Robot::on_module_loaded()
128 this->register_for_event(ON_GCODE_RECEIVED
);
134 #define ACTUATOR_CHECKSUMS(X) { \
135 CHECKSUM(X "_step_pin"), \
136 CHECKSUM(X "_dir_pin"), \
137 CHECKSUM(X "_en_pin"), \
138 CHECKSUM(X "_steps_per_mm"), \
139 CHECKSUM(X "_max_rate") \
142 void Robot::load_config()
144 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
145 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
146 // To make adding those solution easier, they have their own, separate object.
147 // Here we read the config to find out which arm solution to use
148 if (this->arm_solution
) delete this->arm_solution
;
149 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
150 // Note checksums are not const expressions when in debug mode, so don't use switch
151 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
152 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
154 } else if(solution_checksum
== corexz_checksum
) {
155 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
157 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
158 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
160 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
161 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
163 } else if(solution_checksum
== rotatable_delta_checksum
) {
164 this->arm_solution
= new RotatableDeltaSolution(THEKERNEL
->config
);
166 } else if(solution_checksum
== morgan_checksum
) {
167 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
169 } else if(solution_checksum
== cartesian_checksum
) {
170 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
173 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
176 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
177 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
178 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
179 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
180 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
181 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
183 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
184 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
185 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
187 // Make our 3 StepperMotors
188 uint16_t const checksums
[][5] = {
189 ACTUATOR_CHECKSUMS("alpha"),
190 ACTUATOR_CHECKSUMS("beta"),
191 ACTUATOR_CHECKSUMS("gamma"),
192 #if MAX_ROBOT_ACTUATORS > 3
193 ACTUATOR_CHECKSUMS("delta"),
194 ACTUATOR_CHECKSUMS("epsilon"),
195 ACTUATOR_CHECKSUMS("zeta")
198 constexpr size_t actuator_checksum_count
= sizeof(checksums
) / sizeof(checksums
[0]);
199 static_assert(actuator_checksum_count
>= k_max_actuators
, "Robot checksum array too small for k_max_actuators");
201 size_t motor_count
= std::min(this->arm_solution
->get_actuator_count(), k_max_actuators
);
202 for (size_t a
= 0; a
< motor_count
; a
++) {
203 Pin pins
[3]; //step, dir, enable
204 for (size_t i
= 0; i
< 3; i
++) {
205 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
207 actuators
[a
] = new StepperMotor(pins
[0], pins
[1], pins
[2]);
209 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
210 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(checksums
[a
][4])->by_default(30000.0F
)->as_number());
213 check_max_actuator_speeds(); // check the configs are sane
215 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
216 // so the first move can be correct if homing is not performed
217 ActuatorCoordinates actuator_pos
;
218 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
219 for (size_t i
= 0; i
< actuators
.size(); i
++)
220 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
222 //this->clearToolOffset();
225 void Robot::push_state()
227 bool am
= this->absolute_mode
;
228 bool im
= this->inch_mode
;
229 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, im
, current_wcs
);
233 void Robot::pop_state()
235 if(!state_stack
.empty()) {
236 auto s
= state_stack
.top();
238 this->feed_rate
= std::get
<0>(s
);
239 this->seek_rate
= std::get
<1>(s
);
240 this->absolute_mode
= std::get
<2>(s
);
241 this->inch_mode
= std::get
<3>(s
);
242 this->current_wcs
= std::get
<4>(s
);
246 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
248 std::vector
<wcs_t
> v
;
249 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
250 for(auto& i
: wcs_offsets
) {
253 v
.push_back(g92_offset
);
254 v
.push_back(tool_offset
);
258 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
260 // M114.1 is a new way to do this (similar to how GRBL does it).
261 // it returns the realtime position based on the current step position of the actuators.
262 // this does require a FK to get a machine position from the actuator position
263 // and then invert all the transforms to get a workspace position from machine position
264 // M114 just does it the old way uses last_milestone and does inversse tranfroms to get the requested position
266 if(subcode
== 0) { // M114 print WCS
267 wcs_t pos
= mcs2wcs(last_milestone
);
268 n
= snprintf(buf
, bufsize
, "C: X:%1.3f Y:%1.3f Z:%1.3f", from_millimeters(std::get
<X_AXIS
>(pos
)), from_millimeters(std::get
<Y_AXIS
>(pos
)), from_millimeters(std::get
<Z_AXIS
>(pos
)));
270 } else if(subcode
== 4) { // M114.3 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
271 n
= snprintf(buf
, bufsize
, "LMS: X:%1.3f Y:%1.3f Z:%1.3f", last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
273 } else if(subcode
== 5) { // M114.4 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
274 n
= snprintf(buf
, bufsize
, "LMCS: X:%1.3f Y:%1.3f Z:%1.3f", last_machine_position
[X_AXIS
], last_machine_position
[Y_AXIS
], last_machine_position
[Z_AXIS
]);
277 // get real time positions
278 // current actuator position in mm
279 ActuatorCoordinates current_position
{
280 actuators
[X_AXIS
]->get_current_position(),
281 actuators
[Y_AXIS
]->get_current_position(),
282 actuators
[Z_AXIS
]->get_current_position()
285 // get machine position from the actuator position using FK
287 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
289 if(subcode
== 1) { // M114.1 print realtime WCS
290 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
291 wcs_t pos
= mcs2wcs(mpos
);
292 n
= snprintf(buf
, bufsize
, "C: X:%1.3f Y:%1.3f Z:%1.3f", from_millimeters(std::get
<X_AXIS
>(pos
)), from_millimeters(std::get
<Y_AXIS
>(pos
)), from_millimeters(std::get
<Z_AXIS
>(pos
)));
294 } else if(subcode
== 2) { // M114.1 print realtime Machine coordinate system
295 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.3f Y:%1.3f Z:%1.3f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
297 } else if(subcode
== 3) { // M114.2 print realtime actuator position
298 n
= snprintf(buf
, bufsize
, "APOS: A:%1.3f B:%1.3f C:%1.3f", current_position
[X_AXIS
], current_position
[Y_AXIS
], current_position
[Z_AXIS
]);
304 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
305 Robot::wcs_t
Robot::mcs2wcs(const float *pos
) const
307 return std::make_tuple(
308 pos
[X_AXIS
] - std::get
<X_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<X_AXIS
>(g92_offset
) - std::get
<X_AXIS
>(tool_offset
),
309 pos
[Y_AXIS
] - std::get
<Y_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<Y_AXIS
>(g92_offset
) - std::get
<Y_AXIS
>(tool_offset
),
310 pos
[Z_AXIS
] - std::get
<Z_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<Z_AXIS
>(g92_offset
) - std::get
<Z_AXIS
>(tool_offset
)
314 // this does a sanity check that actuator speeds do not exceed steps rate capability
315 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
316 void Robot::check_max_actuator_speeds()
318 for (size_t i
= 0; i
< actuators
.size(); i
++) {
319 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
320 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
321 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
322 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
);
327 //A GCode has been received
328 //See if the current Gcode line has some orders for us
329 void Robot::on_gcode_received(void *argument
)
331 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
333 this->motion_mode
= -1;
337 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
338 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
339 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
340 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
341 case 4: { // G4 pause
342 uint32_t delay_ms
= 0;
343 if (gcode
->has_letter('P')) {
344 delay_ms
= gcode
->get_int('P');
346 if (gcode
->has_letter('S')) {
347 delay_ms
+= gcode
->get_int('S') * 1000;
351 THEKERNEL
->conveyor
->wait_for_empty_queue();
352 // wait for specified time
353 uint32_t start
= us_ticker_read(); // mbed call
354 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
355 THEKERNEL
->call_event(ON_IDLE
, this);
356 if(THEKERNEL
->is_halted()) return;
362 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
363 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
364 size_t n
= gcode
->get_uint('P');
365 if(n
== 0) n
= current_wcs
; // set current coordinate system
369 std::tie(x
, y
, z
) = wcs_offsets
[n
];
370 if(gcode
->get_int('L') == 20) {
371 // this makes the current machine position (less compensation transform) the offset
372 // get current position in WCS
373 wcs_t pos
= mcs2wcs(last_milestone
);
375 if(gcode
->has_letter('X')){
376 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
379 if(gcode
->has_letter('Y')){
380 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
382 if(gcode
->has_letter('Z')) {
383 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
387 // the value is the offset from machine zero
388 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
389 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
390 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
392 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
397 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
398 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
399 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
400 case 20: this->inch_mode
= true; break;
401 case 21: this->inch_mode
= false; break;
403 case 54: case 55: case 56: case 57: case 58: case 59:
404 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
405 current_wcs
= gcode
->g
- 54;
406 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
407 current_wcs
+= gcode
->subcode
;
408 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
412 case 90: this->absolute_mode
= true; break;
413 case 91: this->absolute_mode
= false; break;
416 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
417 // reset G92 offsets to 0
418 g92_offset
= wcs_t(0, 0, 0);
421 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
423 std::tie(x
, y
, z
) = g92_offset
;
424 // get current position in WCS
425 wcs_t pos
= mcs2wcs(last_milestone
);
427 // adjust g92 offset to make the current wpos == the value requested
428 if(gcode
->has_letter('X')){
429 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
431 if(gcode
->has_letter('Y')){
432 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
434 if(gcode
->has_letter('Z')) {
435 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
437 g92_offset
= wcs_t(x
, y
, z
);
444 } else if( gcode
->has_m
) {
446 case 2: // M2 end of program
448 absolute_mode
= true;
451 case 92: // M92 - set steps per mm
452 if (gcode
->has_letter('X'))
453 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
454 if (gcode
->has_letter('Y'))
455 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
456 if (gcode
->has_letter('Z'))
457 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
459 gcode
->stream
->printf("X:%f Y:%f Z:%f ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
460 gcode
->add_nl
= true;
461 check_max_actuator_speeds();
466 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
467 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
471 case 120: // push state
475 case 121: // pop state
479 case 203: // M203 Set maximum feedrates in mm/sec
480 if (gcode
->has_letter('X'))
481 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
482 if (gcode
->has_letter('Y'))
483 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
484 if (gcode
->has_letter('Z'))
485 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
486 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
487 if (gcode
->has_letter('A' + i
))
488 actuators
[i
]->set_max_rate(gcode
->get_value('A' + i
));
490 check_max_actuator_speeds();
492 if(gcode
->get_num_args() == 0) {
493 gcode
->stream
->printf("X:%g Y:%g Z:%g",
494 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
495 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
496 gcode
->stream
->printf(" %c : %g", 'A' + i
, actuators
[i
]->get_max_rate()); //xxx
498 gcode
->add_nl
= true;
502 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
503 if (gcode
->has_letter('S')) {
504 float acc
= gcode
->get_value('S'); // mm/s^2
508 THEKERNEL
->planner
->acceleration
= acc
;
510 if (gcode
->has_letter('Z')) {
511 float acc
= gcode
->get_value('Z'); // mm/s^2
515 THEKERNEL
->planner
->z_acceleration
= acc
;
519 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
520 if (gcode
->has_letter('X')) {
521 float jd
= gcode
->get_value('X');
525 THEKERNEL
->planner
->junction_deviation
= jd
;
527 if (gcode
->has_letter('Z')) {
528 float jd
= gcode
->get_value('Z');
529 // enforce minimum, -1 disables it and uses regular junction deviation
532 THEKERNEL
->planner
->z_junction_deviation
= jd
;
534 if (gcode
->has_letter('S')) {
535 float mps
= gcode
->get_value('S');
539 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
541 if (gcode
->has_letter('Y')) {
542 actuators
[0]->default_minimum_actuator_rate
= gcode
->get_value('Y');
546 case 220: // M220 - speed override percentage
547 if (gcode
->has_letter('S')) {
548 float factor
= gcode
->get_value('S');
549 // enforce minimum 10% speed
552 // enforce maximum 10x speed
553 if (factor
> 1000.0F
)
556 seconds_per_minute
= 6000.0F
/ factor
;
558 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
562 case 400: // wait until all moves are done up to this point
563 THEKERNEL
->conveyor
->wait_for_empty_queue();
566 case 500: // M500 saves some volatile settings to config override file
567 case 503: { // M503 just prints the settings
568 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
);
569 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
570 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
);
571 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f",
572 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
573 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
574 gcode
->stream
->printf(" %c%1.5f", 'A' + i
, actuators
[i
]->get_max_rate());
576 gcode
->stream
->printf("\n");
578 // get or save any arm solution specific optional values
579 BaseSolution::arm_options_t options
;
580 if(arm_solution
->get_optional(options
) && !options
.empty()) {
581 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
582 for(auto &i
: options
) {
583 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
585 gcode
->stream
->printf("\n");
588 // save wcs_offsets and current_wcs
589 // TODO this may need to be done whenever they change to be compliant
590 gcode
->stream
->printf(";WCS settings\n");
591 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
593 for(auto &i
: wcs_offsets
) {
594 if(i
!= wcs_t(0, 0, 0)) {
596 std::tie(x
, y
, z
) = i
;
597 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
603 if(gcode
->m
== 503) {
604 // just print the G92 setting as it is not saved
605 // TODO linuxcnc does seem to save G92, so maybe we should here too
606 if(g92_offset
!= wcs_t(0, 0, 0)) {
608 std::tie(x
, y
, z
) = g92_offset
;
609 gcode
->stream
->printf("G92 X%f Y%f Z%f ; NOT SAVED\n", x
, y
, z
);
614 case 665: { // M665 set optional arm solution variables based on arm solution.
615 // the parameter args could be any letter each arm solution only accepts certain ones
616 BaseSolution::arm_options_t options
= gcode
->get_args();
617 options
.erase('S'); // don't include the S
618 options
.erase('U'); // don't include the U
619 if(options
.size() > 0) {
620 // set the specified options
621 arm_solution
->set_optional(options
);
624 if(arm_solution
->get_optional(options
)) {
625 // foreach optional value
626 for(auto &i
: options
) {
627 // print all current values of supported options
628 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
629 gcode
->add_nl
= true;
633 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
634 this->delta_segments_per_second
= gcode
->get_value('S');
635 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
637 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
638 this->mm_per_line_segment
= gcode
->get_value('U');
639 this->delta_segments_per_second
= 0;
640 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
648 if( this->motion_mode
>= 0) {
652 next_command_is_MCS
= false; // must be on same line as G0 or G1
655 // process a G0/G1/G2/G3
656 void Robot::process_move(Gcode
*gcode
)
658 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
659 float param
[3]{NAN
, NAN
, NAN
};
660 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
662 if( gcode
->has_letter(letter
) ) {
663 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
667 float offset
[3]{0,0,0};
668 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
669 if( gcode
->has_letter(letter
) ) {
670 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
674 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
675 float target
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
676 if(!next_command_is_MCS
) {
677 if(this->absolute_mode
) {
678 // apply wcs offsets and g92 offset and tool offset
679 if(!isnan(param
[X_AXIS
])) {
680 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
);
683 if(!isnan(param
[Y_AXIS
])) {
684 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
);
687 if(!isnan(param
[Z_AXIS
])) {
688 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
);
692 // they are deltas from the last_milestone if specified
693 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
694 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
699 // already in machine coordinates, we do not add tool offset for that
700 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
701 if(!isnan(param
[i
])) target
[i
] = param
[i
];
705 if( gcode
->has_letter('F') ) {
706 if( this->motion_mode
== MOTION_MODE_SEEK
)
707 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
709 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
713 //Perform any physical actions
714 switch(this->motion_mode
) {
715 case MOTION_MODE_CANCEL
:
717 case MOTION_MODE_SEEK
:
718 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
);
720 case MOTION_MODE_LINEAR
:
721 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
723 case MOTION_MODE_CW_ARC
:
724 case MOTION_MODE_CCW_ARC
:
725 moved
= this->compute_arc(gcode
, offset
, target
);
730 // set last_milestone to the calculated target
731 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
));
735 // We received a new gcode, and one of the functions
736 // determined the distance for that given gcode. So now we can attach this gcode to the right block
738 void Robot::distance_in_gcode_is_known(Gcode
* gcode
)
740 //If the queue is empty, execute immediatly, otherwise attach to the last added block
741 THEKERNEL
->conveyor
->append_gcode(gcode
);
744 // reset the machine position for all axis. Used for homing.
745 // During homing compensation is turned off (actually not used as it drives steppers directly)
746 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
747 // so in those cases the final position is compensated.
748 void Robot::reset_axis_position(float x
, float y
, float z
)
750 // these are set to the same as compensation was not used to get to the current position
751 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
752 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
753 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
755 // now set the actuator positions to match
756 ActuatorCoordinates actuator_pos
;
757 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
758 for (size_t i
= 0; i
< actuators
.size(); i
++)
759 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
762 // Reset the position for an axis (used in homing)
763 void Robot::reset_axis_position(float position
, int axis
)
765 last_milestone
[axis
] = position
;
766 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
769 // Use FK to find out where actuator is and reset to match
770 void Robot::reset_position_from_current_actuator_position()
772 ActuatorCoordinates actuator_pos
;
773 for (size_t i
= 0; i
< actuators
.size(); i
++) {
774 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
775 actuator_pos
[i
] = actuators
[i
]->get_current_position();
778 // discover machine position from where actuators actually are
779 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
780 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
781 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
783 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
784 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
785 // to get everything in perfect sync.
786 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
787 for (size_t i
= 0; i
< actuators
.size(); i
++)
788 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
791 // Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
792 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
793 // all transforms and is what we actually convert to actuator positions
794 bool Robot::append_milestone(Gcode
* gcode
, const float target
[], float rate_mm_s
)
798 ActuatorCoordinates actuator_pos
;
799 float transformed_target
[3]; // adjust target for bed compensation and WCS offsets
800 float millimeters_of_travel
;
802 // unity transform by default
803 memcpy(transformed_target
, target
, sizeof(transformed_target
));
805 // check function pointer and call if set to transform the target to compensate for bed
806 if(compensationTransform
) {
807 // some compensation strategies can transform XYZ, some just change Z
808 compensationTransform(transformed_target
);
811 // find distance moved by each axis, use transformed target from the current machine position
812 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
813 deltas
[axis
] = transformed_target
[axis
] - last_machine_position
[axis
];
816 // Compute how long this move moves, so we can attach it to the block for later use
817 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
819 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
820 // 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
821 if(millimeters_of_travel
< 0.00001F
) return false;
823 // this is the machine position
824 memcpy(this->last_machine_position
, transformed_target
, sizeof(this->last_machine_position
));
826 // find distance unit vector
827 for (int i
= 0; i
< 3; i
++)
828 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
830 // Do not move faster than the configured cartesian limits
831 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
832 if ( max_speeds
[axis
] > 0 ) {
833 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
835 if (axis_speed
> max_speeds
[axis
])
836 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
840 // find actuator position given the machine position, use actual adjusted target
841 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
843 float isecs
= rate_mm_s
/ millimeters_of_travel
;
844 // check per-actuator speed limits
845 for (size_t actuator
= 0; actuator
< actuators
.size(); actuator
++) {
846 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
847 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
848 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
849 isecs
= rate_mm_s
/ millimeters_of_travel
;
853 // Append the block to the planner
854 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
859 // Append a move to the queue ( cutting it into segments if needed )
860 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
)
862 // Find out the distance for this move in MCS
863 // 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
864 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 ));
866 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
867 if( gcode
->millimeters_of_travel
< 0.00001F
) return false;
869 // Mark the gcode as having a known distance
870 this->distance_in_gcode_is_known( gcode
);
872 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
873 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
874 // We also check we are not exceeding the E max_speed for the current extruder
875 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
876 // we need to ask it now passing in the relevant data.
877 // NOTE we need to do this before we segment the line (for deltas)
878 if(gcode
->has_letter('E')) {
880 data
[0] = gcode
->get_value('E'); // E target (maybe absolute or relative)
881 data
[1] = rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
882 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
883 rate_mm_s
*= data
[1];
884 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
888 // 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.
889 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
890 // 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
893 if(this->delta_segments_per_second
> 1.0F
) {
894 // enabled if set to something > 1, it is set to 0.0 by default
895 // segment based on current speed and requested segments per second
896 // the faster the travel speed the fewer segments needed
897 // NOTE rate is mm/sec and we take into account any speed override
898 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
899 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
900 // TODO if we are only moving in Z on a delta we don't really need to segment at all
903 if(this->mm_per_line_segment
== 0.0F
) {
904 segments
= 1; // don't split it up
906 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
912 // A vector to keep track of the endpoint of each segment
913 float segment_delta
[3];
914 float segment_end
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
916 // How far do we move each segment?
917 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
918 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
920 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
921 // We always add another point after this loop so we stop at segments-1, ie i < segments
922 for (int i
= 1; i
< segments
; i
++) {
923 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
924 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
925 segment_end
[axis
] += segment_delta
[axis
];
927 // Append the end of this segment to the queue
928 bool b
= this->append_milestone(gcode
, segment_end
, rate_mm_s
);
933 // Append the end of this full move to the queue
934 if(this->append_milestone(gcode
, target
, rate_mm_s
)) moved
= true;
936 this->next_command_is_MCS
= false; // always reset this
939 // if adding these blocks didn't start executing, do that now
940 THEKERNEL
->conveyor
->ensure_running();
947 // Append an arc to the queue ( cutting it into segments as needed )
948 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
952 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
953 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
954 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
955 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
956 float r_axis1
= -offset
[this->plane_axis_1
];
957 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
958 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
960 // Patch from GRBL Firmware - Christoph Baumann 04072015
961 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
962 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
963 if (is_clockwise
) { // Correct atan2 output per direction
964 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2 * M_PI
; }
966 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2 * M_PI
; }
969 // Find the distance for this gcode
970 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
972 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
973 if( gcode
->millimeters_of_travel
< 0.00001F
) {
977 // Mark the gcode as having a known distance
978 this->distance_in_gcode_is_known( gcode
);
980 // Figure out how many segments for this gcode
981 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
983 float theta_per_segment
= angular_travel
/ segments
;
984 float linear_per_segment
= linear_travel
/ segments
;
986 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
987 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
988 r_T = [cos(phi) -sin(phi);
989 sin(phi) cos(phi] * r ;
990 For arc generation, the center of the circle is the axis of rotation and the radius vector is
991 defined from the circle center to the initial position. Each line segment is formed by successive
992 vector rotations. This requires only two cos() and sin() computations to form the rotation
993 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
994 all float numbers are single precision on the Arduino. (True float precision will not have
995 round off issues for CNC applications.) Single precision error can accumulate to be greater than
996 tool precision in some cases. Therefore, arc path correction is implemented.
998 Small angle approximation may be used to reduce computation overhead further. This approximation
999 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1000 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1001 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1002 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1003 issue for CNC machines with the single precision Arduino calculations.
1004 This approximation also allows mc_arc to immediately insert a line segment into the planner
1005 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1006 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1007 This is important when there are successive arc motions.
1009 // Vector rotation matrix values
1010 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1011 float sin_T
= theta_per_segment
;
1013 float arc_target
[3];
1020 // Initialize the linear axis
1021 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1024 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1025 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1027 if (count
< this->arc_correction
) {
1028 // Apply vector rotation matrix
1029 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1030 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1034 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1035 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1036 cos_Ti
= cosf(i
* theta_per_segment
);
1037 sin_Ti
= sinf(i
* theta_per_segment
);
1038 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1039 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1043 // Update arc_target location
1044 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1045 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1046 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1048 // Append this segment to the queue
1049 bool b
= this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
1053 // Ensure last segment arrives at target location.
1054 if(this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
)) moved
= true;
1059 // Do the math for an arc and add it to the queue
1060 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[])
1064 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1066 // Set clockwise/counter-clockwise sign for mc_arc computations
1067 bool is_clockwise
= false;
1068 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
1069 is_clockwise
= true;
1073 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1077 float Robot::theta(float x
, float y
)
1079 float t
= atanf(x
/ fabs(y
));
1091 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1093 this->plane_axis_0
= axis_0
;
1094 this->plane_axis_1
= axis_1
;
1095 this->plane_axis_2
= axis_2
;
1098 void Robot::clearToolOffset()
1100 this->tool_offset
= wcs_t(0,0,0);
1103 void Robot::setToolOffset(const float offset
[3])
1105 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1108 float Robot::get_feed_rate() const
1110 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;