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 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 rotary_delta_checksum CHECKSUM("rotary_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;
123 this->disable_segmentation
= false;
126 //Called when the module has just been loaded
127 void Robot::on_module_loaded()
129 this->register_for_event(ON_GCODE_RECEIVED
);
135 #define ACTUATOR_CHECKSUMS(X) { \
136 CHECKSUM(X "_step_pin"), \
137 CHECKSUM(X "_dir_pin"), \
138 CHECKSUM(X "_en_pin"), \
139 CHECKSUM(X "_steps_per_mm"), \
140 CHECKSUM(X "_max_rate") \
143 void Robot::load_config()
145 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
146 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
147 // To make adding those solution easier, they have their own, separate object.
148 // Here we read the config to find out which arm solution to use
149 if (this->arm_solution
) delete this->arm_solution
;
150 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
151 // Note checksums are not const expressions when in debug mode, so don't use switch
152 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
153 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
155 } else if(solution_checksum
== corexz_checksum
) {
156 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
158 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
159 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
161 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
162 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
164 } else if(solution_checksum
== rotary_delta_checksum
) {
165 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
167 } else if(solution_checksum
== morgan_checksum
) {
168 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
170 } else if(solution_checksum
== cartesian_checksum
) {
171 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
174 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
177 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
178 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
179 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
180 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
181 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
182 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
184 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
185 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
186 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
188 // Make our 3 StepperMotors
189 uint16_t const checksums
[][5] = {
190 ACTUATOR_CHECKSUMS("alpha"),
191 ACTUATOR_CHECKSUMS("beta"),
192 ACTUATOR_CHECKSUMS("gamma"),
193 #if MAX_ROBOT_ACTUATORS > 3
194 ACTUATOR_CHECKSUMS("delta"),
195 ACTUATOR_CHECKSUMS("epsilon"),
196 ACTUATOR_CHECKSUMS("zeta")
199 constexpr size_t actuator_checksum_count
= sizeof(checksums
) / sizeof(checksums
[0]);
200 static_assert(actuator_checksum_count
>= k_max_actuators
, "Robot checksum array too small for k_max_actuators");
202 size_t motor_count
= std::min(this->arm_solution
->get_actuator_count(), k_max_actuators
);
203 for (size_t a
= 0; a
< motor_count
; a
++) {
204 Pin pins
[3]; //step, dir, enable
205 for (size_t i
= 0; i
< 3; i
++) {
206 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
208 actuators
[a
] = new StepperMotor(pins
[0], pins
[1], pins
[2]);
210 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
211 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(checksums
[a
][4])->by_default(30000.0F
)->as_number());
214 check_max_actuator_speeds(); // check the configs are sane
216 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
217 // so the first move can be correct if homing is not performed
218 ActuatorCoordinates actuator_pos
;
219 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
220 for (size_t i
= 0; i
< actuators
.size(); i
++)
221 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
223 //this->clearToolOffset();
226 void Robot::push_state()
228 bool am
= this->absolute_mode
;
229 bool im
= this->inch_mode
;
230 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, im
, current_wcs
);
234 void Robot::pop_state()
236 if(!state_stack
.empty()) {
237 auto s
= state_stack
.top();
239 this->feed_rate
= std::get
<0>(s
);
240 this->seek_rate
= std::get
<1>(s
);
241 this->absolute_mode
= std::get
<2>(s
);
242 this->inch_mode
= std::get
<3>(s
);
243 this->current_wcs
= std::get
<4>(s
);
247 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
249 std::vector
<wcs_t
> v
;
250 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
251 for(auto& i
: wcs_offsets
) {
254 v
.push_back(g92_offset
);
255 v
.push_back(tool_offset
);
259 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
261 // M114.1 is a new way to do this (similar to how GRBL does it).
262 // it returns the realtime position based on the current step position of the actuators.
263 // this does require a FK to get a machine position from the actuator position
264 // and then invert all the transforms to get a workspace position from machine position
265 // M114 just does it the old way uses last_milestone and does inversse tranfroms to get the requested position
267 if(subcode
== 0) { // M114 print WCS
268 wcs_t pos
= mcs2wcs(last_milestone
);
269 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
)));
271 } 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)
272 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
]);
274 } 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)
275 n
= snprintf(buf
, bufsize
, "LMCS: X:%1.4f Y:%1.4f Z:%1.4f", last_machine_position
[X_AXIS
], last_machine_position
[Y_AXIS
], last_machine_position
[Z_AXIS
]);
278 // get real time positions
279 // current actuator position in mm
280 ActuatorCoordinates current_position
{
281 actuators
[X_AXIS
]->get_current_position(),
282 actuators
[Y_AXIS
]->get_current_position(),
283 actuators
[Z_AXIS
]->get_current_position()
286 // get machine position from the actuator position using FK
288 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
290 if(subcode
== 1) { // M114.1 print realtime WCS
291 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
292 wcs_t pos
= mcs2wcs(mpos
);
293 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
)));
295 } else if(subcode
== 2) { // M114.1 print realtime Machine coordinate system
296 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
298 } else if(subcode
== 3) { // M114.2 print realtime actuator position
299 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
]);
305 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
306 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
308 return std::make_tuple(
309 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
),
310 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
),
311 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
)
315 // this does a sanity check that actuator speeds do not exceed steps rate capability
316 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
317 void Robot::check_max_actuator_speeds()
319 for (size_t i
= 0; i
< actuators
.size(); i
++) {
320 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
321 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
322 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
323 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
);
328 //A GCode has been received
329 //See if the current Gcode line has some orders for us
330 void Robot::on_gcode_received(void *argument
)
332 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
334 this->motion_mode
= -1;
338 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
339 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
340 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
341 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
342 case 4: { // G4 pause
343 uint32_t delay_ms
= 0;
344 if (gcode
->has_letter('P')) {
345 delay_ms
= gcode
->get_int('P');
347 if (gcode
->has_letter('S')) {
348 delay_ms
+= gcode
->get_int('S') * 1000;
352 THEKERNEL
->conveyor
->wait_for_empty_queue();
353 // wait for specified time
354 uint32_t start
= us_ticker_read(); // mbed call
355 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
356 THEKERNEL
->call_event(ON_IDLE
, this);
357 if(THEKERNEL
->is_halted()) return;
363 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
364 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
365 size_t n
= gcode
->get_uint('P');
366 if(n
== 0) n
= current_wcs
; // set current coordinate system
370 std::tie(x
, y
, z
) = wcs_offsets
[n
];
371 if(gcode
->get_int('L') == 20) {
372 // this makes the current machine position (less compensation transform) the offset
373 // get current position in WCS
374 wcs_t pos
= mcs2wcs(last_milestone
);
376 if(gcode
->has_letter('X')){
377 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
380 if(gcode
->has_letter('Y')){
381 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
383 if(gcode
->has_letter('Z')) {
384 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
388 // the value is the offset from machine zero
389 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
390 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
391 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
393 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
398 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
399 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
400 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
401 case 20: this->inch_mode
= true; break;
402 case 21: this->inch_mode
= false; break;
404 case 54: case 55: case 56: case 57: case 58: case 59:
405 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
406 current_wcs
= gcode
->g
- 54;
407 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
408 current_wcs
+= gcode
->subcode
;
409 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
413 case 90: this->absolute_mode
= true; break;
414 case 91: this->absolute_mode
= false; break;
417 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
418 // reset G92 offsets to 0
419 g92_offset
= wcs_t(0, 0, 0);
422 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
424 std::tie(x
, y
, z
) = g92_offset
;
425 // get current position in WCS
426 wcs_t pos
= mcs2wcs(last_milestone
);
428 // adjust g92 offset to make the current wpos == the value requested
429 if(gcode
->has_letter('X')){
430 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
432 if(gcode
->has_letter('Y')){
433 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
435 if(gcode
->has_letter('Z')) {
436 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
438 g92_offset
= wcs_t(x
, y
, z
);
445 } else if( gcode
->has_m
) {
447 case 0: // M0 feed hold
448 if(THEKERNEL
->is_grbl_mode()) THEKERNEL
->set_feed_hold(true);
450 case 2: // M2 end of program
451 case 30: // M30 end of program
453 absolute_mode
= true;
456 case 92: // M92 - set steps per mm
457 if (gcode
->has_letter('X'))
458 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
459 if (gcode
->has_letter('Y'))
460 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
461 if (gcode
->has_letter('Z'))
462 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
464 gcode
->stream
->printf("X:%f Y:%f Z:%f ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
465 gcode
->add_nl
= true;
466 check_max_actuator_speeds();
471 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
472 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
476 case 120: // push state
480 case 121: // pop state
484 case 203: // M203 Set maximum feedrates in mm/sec
485 if (gcode
->has_letter('X'))
486 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
487 if (gcode
->has_letter('Y'))
488 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
489 if (gcode
->has_letter('Z'))
490 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
491 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
492 if (gcode
->has_letter('A' + i
))
493 actuators
[i
]->set_max_rate(gcode
->get_value('A' + i
));
495 check_max_actuator_speeds();
497 if(gcode
->get_num_args() == 0) {
498 gcode
->stream
->printf("X:%g Y:%g Z:%g",
499 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
500 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
501 gcode
->stream
->printf(" %c : %g", 'A' + i
, actuators
[i
]->get_max_rate()); //xxx
503 gcode
->add_nl
= true;
507 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
508 if (gcode
->has_letter('S')) {
509 float acc
= gcode
->get_value('S'); // mm/s^2
513 THEKERNEL
->planner
->acceleration
= acc
;
515 if (gcode
->has_letter('Z')) {
516 float acc
= gcode
->get_value('Z'); // mm/s^2
520 THEKERNEL
->planner
->z_acceleration
= acc
;
524 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
525 if (gcode
->has_letter('X')) {
526 float jd
= gcode
->get_value('X');
530 THEKERNEL
->planner
->junction_deviation
= jd
;
532 if (gcode
->has_letter('Z')) {
533 float jd
= gcode
->get_value('Z');
534 // enforce minimum, -1 disables it and uses regular junction deviation
537 THEKERNEL
->planner
->z_junction_deviation
= jd
;
539 if (gcode
->has_letter('S')) {
540 float mps
= gcode
->get_value('S');
544 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
546 if (gcode
->has_letter('Y')) {
547 actuators
[0]->default_minimum_actuator_rate
= gcode
->get_value('Y');
551 case 220: // M220 - speed override percentage
552 if (gcode
->has_letter('S')) {
553 float factor
= gcode
->get_value('S');
554 // enforce minimum 10% speed
557 // enforce maximum 10x speed
558 if (factor
> 1000.0F
)
561 seconds_per_minute
= 6000.0F
/ factor
;
563 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
567 case 400: // wait until all moves are done up to this point
568 THEKERNEL
->conveyor
->wait_for_empty_queue();
571 case 500: // M500 saves some volatile settings to config override file
572 case 503: { // M503 just prints the settings
573 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
);
574 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
575 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
);
576 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f",
577 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
578 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
579 gcode
->stream
->printf(" %c%1.5f", 'A' + i
, actuators
[i
]->get_max_rate());
581 gcode
->stream
->printf("\n");
583 // get or save any arm solution specific optional values
584 BaseSolution::arm_options_t options
;
585 if(arm_solution
->get_optional(options
) && !options
.empty()) {
586 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
587 for(auto &i
: options
) {
588 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
590 gcode
->stream
->printf("\n");
593 // save wcs_offsets and current_wcs
594 // TODO this may need to be done whenever they change to be compliant
595 gcode
->stream
->printf(";WCS settings\n");
596 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
598 for(auto &i
: wcs_offsets
) {
599 if(i
!= wcs_t(0, 0, 0)) {
601 std::tie(x
, y
, z
) = i
;
602 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
608 if(gcode
->m
== 503) {
609 // just print the G92 setting as it is not saved
610 // TODO linuxcnc does seem to save G92, so maybe we should here too
611 if(g92_offset
!= wcs_t(0, 0, 0)) {
613 std::tie(x
, y
, z
) = g92_offset
;
614 gcode
->stream
->printf("G92 X%f Y%f Z%f ; NOT SAVED\n", x
, y
, z
);
619 case 665: { // M665 set optional arm solution variables based on arm solution.
620 // the parameter args could be any letter each arm solution only accepts certain ones
621 BaseSolution::arm_options_t options
= gcode
->get_args();
622 options
.erase('S'); // don't include the S
623 options
.erase('U'); // don't include the U
624 if(options
.size() > 0) {
625 // set the specified options
626 arm_solution
->set_optional(options
);
629 if(arm_solution
->get_optional(options
)) {
630 // foreach optional value
631 for(auto &i
: options
) {
632 // print all current values of supported options
633 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
634 gcode
->add_nl
= true;
638 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
639 this->delta_segments_per_second
= gcode
->get_value('S');
640 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
642 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
643 this->mm_per_line_segment
= gcode
->get_value('U');
644 this->delta_segments_per_second
= 0;
645 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
653 if( this->motion_mode
>= 0) {
657 next_command_is_MCS
= false; // must be on same line as G0 or G1
660 // process a G0/G1/G2/G3
661 void Robot::process_move(Gcode
*gcode
)
663 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
664 float param
[3]{NAN
, NAN
, NAN
};
665 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
667 if( gcode
->has_letter(letter
) ) {
668 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
672 float offset
[3]{0,0,0};
673 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
674 if( gcode
->has_letter(letter
) ) {
675 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
679 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
680 float target
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
681 if(!next_command_is_MCS
) {
682 if(this->absolute_mode
) {
683 // apply wcs offsets and g92 offset and tool offset
684 if(!isnan(param
[X_AXIS
])) {
685 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
);
688 if(!isnan(param
[Y_AXIS
])) {
689 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
);
692 if(!isnan(param
[Z_AXIS
])) {
693 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
);
697 // they are deltas from the last_milestone if specified
698 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
699 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
704 // already in machine coordinates, we do not add tool offset for that
705 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
706 if(!isnan(param
[i
])) target
[i
] = param
[i
];
710 if( gcode
->has_letter('F') ) {
711 if( this->motion_mode
== MOTION_MODE_SEEK
)
712 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
714 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
718 //Perform any physical actions
719 switch(this->motion_mode
) {
720 case MOTION_MODE_CANCEL
:
722 case MOTION_MODE_SEEK
:
723 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
);
725 case MOTION_MODE_LINEAR
:
726 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
728 case MOTION_MODE_CW_ARC
:
729 case MOTION_MODE_CCW_ARC
:
730 moved
= this->compute_arc(gcode
, offset
, target
);
735 // set last_milestone to the calculated target
736 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
));
740 // We received a new gcode, and one of the functions
741 // determined the distance for that given gcode. So now we can attach this gcode to the right block
743 void Robot::distance_in_gcode_is_known(Gcode
* gcode
)
745 //If the queue is empty, execute immediatly, otherwise attach to the last added block
746 THEKERNEL
->conveyor
->append_gcode(gcode
);
749 // reset the machine position for all axis. Used for homing.
750 // During homing compensation is turned off (actually not used as it drives steppers directly)
751 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
752 // so in those cases the final position is compensated.
753 void Robot::reset_axis_position(float x
, float y
, float z
)
755 // these are set to the same as compensation was not used to get to the current position
756 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
757 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
758 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
760 // now set the actuator positions to match
761 ActuatorCoordinates actuator_pos
;
762 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
763 for (size_t i
= 0; i
< actuators
.size(); i
++)
764 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
767 // Reset the position for an axis (used in homing)
768 void Robot::reset_axis_position(float position
, int axis
)
770 last_milestone
[axis
] = position
;
771 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
774 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
775 // then sets the axis positions to match. currently only called from Endstops.cpp
776 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
778 for (size_t i
= 0; i
< actuators
.size(); i
++)
779 actuators
[i
]->change_last_milestone(ac
[i
]);
781 // now correct axis positions then recorrect actuator to account for rounding
782 reset_position_from_current_actuator_position();
785 // Use FK to find out where actuator is and reset to match
786 void Robot::reset_position_from_current_actuator_position()
788 ActuatorCoordinates actuator_pos
;
789 for (size_t i
= 0; i
< actuators
.size(); i
++) {
790 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
791 actuator_pos
[i
] = actuators
[i
]->get_current_position();
794 // discover machine position from where actuators actually are
795 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
796 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
797 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
799 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
800 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
801 // to get everything in perfect sync.
802 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
803 for (size_t i
= 0; i
< actuators
.size(); i
++)
804 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
807 // Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
808 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
809 // all transforms and is what we actually convert to actuator positions
810 bool Robot::append_milestone(Gcode
* gcode
, const float target
[], float rate_mm_s
)
814 ActuatorCoordinates actuator_pos
;
815 float transformed_target
[3]; // adjust target for bed compensation and WCS offsets
816 float millimeters_of_travel
;
818 // unity transform by default
819 memcpy(transformed_target
, target
, sizeof(transformed_target
));
821 // check function pointer and call if set to transform the target to compensate for bed
822 if(compensationTransform
) {
823 // some compensation strategies can transform XYZ, some just change Z
824 compensationTransform(transformed_target
);
827 // find distance moved by each axis, use transformed target from the current machine position
828 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
829 deltas
[axis
] = transformed_target
[axis
] - last_machine_position
[axis
];
832 // Compute how long this move moves, so we can attach it to the block for later use
833 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
835 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
836 // 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
837 if(millimeters_of_travel
< 0.00001F
) return false;
839 // this is the machine position
840 memcpy(this->last_machine_position
, transformed_target
, sizeof(this->last_machine_position
));
842 // find distance unit vector
843 for (int i
= 0; i
< 3; i
++)
844 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
846 // Do not move faster than the configured cartesian limits
847 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
848 if ( max_speeds
[axis
] > 0 ) {
849 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
851 if (axis_speed
> max_speeds
[axis
])
852 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
856 // find actuator position given the machine position, use actual adjusted target
857 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
859 float isecs
= rate_mm_s
/ millimeters_of_travel
;
860 // check per-actuator speed limits
861 for (size_t actuator
= 0; actuator
< actuators
.size(); actuator
++) {
862 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
863 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
864 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
865 isecs
= rate_mm_s
/ millimeters_of_travel
;
869 // Append the block to the planner
870 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
875 // Append a move to the queue ( cutting it into segments if needed )
876 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
)
878 // Find out the distance for this move in MCS
879 // 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
880 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 ));
882 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
883 if( gcode
->millimeters_of_travel
< 0.00001F
) return false;
885 // Mark the gcode as having a known distance
886 this->distance_in_gcode_is_known( gcode
);
888 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
889 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
890 // We also check we are not exceeding the E max_speed for the current extruder
891 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
892 // we need to ask it now passing in the relevant data.
893 // NOTE we need to do this before we segment the line (for deltas)
894 if(gcode
->has_letter('E')) {
896 data
[0] = gcode
->get_value('E'); // E target (maybe absolute or relative)
897 data
[1] = rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
898 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
899 rate_mm_s
*= data
[1];
900 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
904 // 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.
905 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
906 // 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
909 if(this->disable_segmentation
) {
912 } else if(this->delta_segments_per_second
> 1.0F
) {
913 // enabled if set to something > 1, it is set to 0.0 by default
914 // segment based on current speed and requested segments per second
915 // the faster the travel speed the fewer segments needed
916 // NOTE rate is mm/sec and we take into account any speed override
917 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
918 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
919 // TODO if we are only moving in Z on a delta we don't really need to segment at all
922 if(this->mm_per_line_segment
== 0.0F
) {
923 segments
= 1; // don't split it up
925 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
931 // A vector to keep track of the endpoint of each segment
932 float segment_delta
[3];
933 float segment_end
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
935 // How far do we move each segment?
936 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
937 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
939 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
940 // We always add another point after this loop so we stop at segments-1, ie i < segments
941 for (int i
= 1; i
< segments
; i
++) {
942 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
943 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
944 segment_end
[axis
] += segment_delta
[axis
];
946 // Append the end of this segment to the queue
947 bool b
= this->append_milestone(gcode
, segment_end
, rate_mm_s
);
952 // Append the end of this full move to the queue
953 if(this->append_milestone(gcode
, target
, rate_mm_s
)) moved
= true;
955 this->next_command_is_MCS
= false; // always reset this
958 // if adding these blocks didn't start executing, do that now
959 THEKERNEL
->conveyor
->ensure_running();
966 // Append an arc to the queue ( cutting it into segments as needed )
967 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
971 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
972 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
973 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
974 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
975 float r_axis1
= -offset
[this->plane_axis_1
];
976 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
977 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
979 // Patch from GRBL Firmware - Christoph Baumann 04072015
980 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
981 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
982 if (is_clockwise
) { // Correct atan2 output per direction
983 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2 * M_PI
; }
985 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2 * M_PI
; }
988 // Find the distance for this gcode
989 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
991 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
992 if( gcode
->millimeters_of_travel
< 0.00001F
) {
996 // Mark the gcode as having a known distance
997 this->distance_in_gcode_is_known( gcode
);
999 // Figure out how many segments for this gcode
1000 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
1002 float theta_per_segment
= angular_travel
/ segments
;
1003 float linear_per_segment
= linear_travel
/ segments
;
1005 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1006 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1007 r_T = [cos(phi) -sin(phi);
1008 sin(phi) cos(phi] * r ;
1009 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1010 defined from the circle center to the initial position. Each line segment is formed by successive
1011 vector rotations. This requires only two cos() and sin() computations to form the rotation
1012 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1013 all float numbers are single precision on the Arduino. (True float precision will not have
1014 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1015 tool precision in some cases. Therefore, arc path correction is implemented.
1017 Small angle approximation may be used to reduce computation overhead further. This approximation
1018 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1019 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1020 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1021 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1022 issue for CNC machines with the single precision Arduino calculations.
1023 This approximation also allows mc_arc to immediately insert a line segment into the planner
1024 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1025 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1026 This is important when there are successive arc motions.
1028 // Vector rotation matrix values
1029 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1030 float sin_T
= theta_per_segment
;
1032 float arc_target
[3];
1039 // Initialize the linear axis
1040 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1043 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1044 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1046 if (count
< this->arc_correction
) {
1047 // Apply vector rotation matrix
1048 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1049 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1053 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1054 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1055 cos_Ti
= cosf(i
* theta_per_segment
);
1056 sin_Ti
= sinf(i
* theta_per_segment
);
1057 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1058 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1062 // Update arc_target location
1063 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1064 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1065 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1067 // Append this segment to the queue
1068 bool b
= this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
1072 // Ensure last segment arrives at target location.
1073 if(this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
)) moved
= true;
1078 // Do the math for an arc and add it to the queue
1079 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[])
1083 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1085 // Set clockwise/counter-clockwise sign for mc_arc computations
1086 bool is_clockwise
= false;
1087 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
1088 is_clockwise
= true;
1092 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1096 float Robot::theta(float x
, float y
)
1098 float t
= atanf(x
/ fabs(y
));
1110 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1112 this->plane_axis_0
= axis_0
;
1113 this->plane_axis_1
= axis_1
;
1114 this->plane_axis_2
= axis_2
;
1117 void Robot::clearToolOffset()
1119 this->tool_offset
= wcs_t(0,0,0);
1122 void Robot::setToolOffset(const float offset
[3])
1124 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1127 float Robot::get_feed_rate() const
1129 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;