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")
53 #define segment_z_moves_checksum CHECKSUM("segment_z_moves")
56 #define arm_solution_checksum CHECKSUM("arm_solution")
57 #define cartesian_checksum CHECKSUM("cartesian")
58 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
59 #define rostock_checksum CHECKSUM("rostock")
60 #define linear_delta_checksum CHECKSUM("linear_delta")
61 #define rotary_delta_checksum CHECKSUM("rotary_delta")
62 #define delta_checksum CHECKSUM("delta")
63 #define hbot_checksum CHECKSUM("hbot")
64 #define corexy_checksum CHECKSUM("corexy")
65 #define corexz_checksum CHECKSUM("corexz")
66 #define kossel_checksum CHECKSUM("kossel")
67 #define morgan_checksum CHECKSUM("morgan")
69 // new-style actuator stuff
70 #define actuator_checksum CHEKCSUM("actuator")
72 #define step_pin_checksum CHECKSUM("step_pin")
73 #define dir_pin_checksum CHEKCSUM("dir_pin")
74 #define en_pin_checksum CHECKSUM("en_pin")
76 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
77 #define max_rate_checksum CHECKSUM("max_rate")
79 #define alpha_checksum CHECKSUM("alpha")
80 #define beta_checksum CHECKSUM("beta")
81 #define gamma_checksum CHECKSUM("gamma")
83 #define NEXT_ACTION_DEFAULT 0
84 #define NEXT_ACTION_DWELL 1
85 #define NEXT_ACTION_GO_HOME 2
87 #define MOTION_MODE_SEEK 0 // G0
88 #define MOTION_MODE_LINEAR 1 // G1
89 #define MOTION_MODE_CW_ARC 2 // G2
90 #define MOTION_MODE_CCW_ARC 3 // G3
91 #define MOTION_MODE_CANCEL 4 // G80
93 #define PATH_CONTROL_MODE_EXACT_PATH 0
94 #define PATH_CONTROL_MODE_EXACT_STOP 1
95 #define PATH_CONTROL_MODE_CONTINOUS 2
97 #define PROGRAM_FLOW_RUNNING 0
98 #define PROGRAM_FLOW_PAUSED 1
99 #define PROGRAM_FLOW_COMPLETED 2
101 #define SPINDLE_DIRECTION_CW 0
102 #define SPINDLE_DIRECTION_CCW 1
104 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
106 // 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
107 // It takes care of cutting arcs into segments, same thing for line that are too long
111 this->inch_mode
= false;
112 this->absolute_mode
= true;
113 this->motion_mode
= MOTION_MODE_SEEK
;
114 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
115 clear_vector(this->last_milestone
);
116 clear_vector(this->last_machine_position
);
117 this->arm_solution
= NULL
;
118 seconds_per_minute
= 60.0F
;
119 this->clearToolOffset();
120 this->compensationTransform
= nullptr;
121 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
122 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
123 this->next_command_is_MCS
= false;
124 this->disable_segmentation
= false;
127 //Called when the module has just been loaded
128 void Robot::on_module_loaded()
130 this->register_for_event(ON_GCODE_RECEIVED
);
136 #define ACTUATOR_CHECKSUMS(X) { \
137 CHECKSUM(X "_step_pin"), \
138 CHECKSUM(X "_dir_pin"), \
139 CHECKSUM(X "_en_pin"), \
140 CHECKSUM(X "_steps_per_mm"), \
141 CHECKSUM(X "_max_rate") \
144 void Robot::load_config()
146 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
147 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
148 // To make adding those solution easier, they have their own, separate object.
149 // Here we read the config to find out which arm solution to use
150 if (this->arm_solution
) delete this->arm_solution
;
151 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
152 // Note checksums are not const expressions when in debug mode, so don't use switch
153 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
154 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== corexz_checksum
) {
157 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
160 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
163 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== rotary_delta_checksum
) {
166 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== morgan_checksum
) {
169 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
171 } else if(solution_checksum
== cartesian_checksum
) {
172 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
175 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
178 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
179 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
180 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
181 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
182 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
183 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
185 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
186 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
187 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
189 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
191 // Make our 3 StepperMotors
192 uint16_t const checksums
[][5] = {
193 ACTUATOR_CHECKSUMS("alpha"),
194 ACTUATOR_CHECKSUMS("beta"),
195 ACTUATOR_CHECKSUMS("gamma"),
196 #if MAX_ROBOT_ACTUATORS > 3
197 ACTUATOR_CHECKSUMS("delta"),
198 ACTUATOR_CHECKSUMS("epsilon"),
199 ACTUATOR_CHECKSUMS("zeta")
202 constexpr size_t actuator_checksum_count
= sizeof(checksums
) / sizeof(checksums
[0]);
203 static_assert(actuator_checksum_count
>= k_max_actuators
, "Robot checksum array too small for k_max_actuators");
205 size_t motor_count
= std::min(this->arm_solution
->get_actuator_count(), k_max_actuators
);
206 for (size_t a
= 0; a
< motor_count
; a
++) {
207 Pin pins
[3]; //step, dir, enable
208 for (size_t i
= 0; i
< 3; i
++) {
209 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
211 actuators
[a
] = new StepperMotor(pins
[0], pins
[1], pins
[2]);
213 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
214 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(checksums
[a
][4])->by_default(30000.0F
)->as_number());
217 check_max_actuator_speeds(); // check the configs are sane
219 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
220 // so the first move can be correct if homing is not performed
221 ActuatorCoordinates actuator_pos
;
222 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
223 for (size_t i
= 0; i
< actuators
.size(); i
++)
224 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
226 //this->clearToolOffset();
229 void Robot::push_state()
231 bool am
= this->absolute_mode
;
232 bool im
= this->inch_mode
;
233 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, im
, current_wcs
);
237 void Robot::pop_state()
239 if(!state_stack
.empty()) {
240 auto s
= state_stack
.top();
242 this->feed_rate
= std::get
<0>(s
);
243 this->seek_rate
= std::get
<1>(s
);
244 this->absolute_mode
= std::get
<2>(s
);
245 this->inch_mode
= std::get
<3>(s
);
246 this->current_wcs
= std::get
<4>(s
);
250 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
252 std::vector
<wcs_t
> v
;
253 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
254 for(auto& i
: wcs_offsets
) {
257 v
.push_back(g92_offset
);
258 v
.push_back(tool_offset
);
262 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
264 // M114.1 is a new way to do this (similar to how GRBL does it).
265 // it returns the realtime position based on the current step position of the actuators.
266 // this does require a FK to get a machine position from the actuator position
267 // and then invert all the transforms to get a workspace position from machine position
268 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
270 if(subcode
== 0) { // M114 print WCS
271 wcs_t pos
= mcs2wcs(last_milestone
);
272 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
)));
274 } 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)
275 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
]);
277 } 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)
278 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
]);
281 // get real time positions
282 // current actuator position in mm
283 ActuatorCoordinates current_position
{
284 actuators
[X_AXIS
]->get_current_position(),
285 actuators
[Y_AXIS
]->get_current_position(),
286 actuators
[Z_AXIS
]->get_current_position()
289 // get machine position from the actuator position using FK
291 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
293 if(subcode
== 1) { // M114.1 print realtime WCS
294 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
295 wcs_t pos
= mcs2wcs(mpos
);
296 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
)));
298 } else if(subcode
== 2) { // M114.1 print realtime Machine coordinate system
299 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
301 } else if(subcode
== 3) { // M114.2 print realtime actuator position
302 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
]);
308 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
309 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
311 return std::make_tuple(
312 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
),
313 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
),
314 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
)
318 // this does a sanity check that actuator speeds do not exceed steps rate capability
319 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
320 void Robot::check_max_actuator_speeds()
322 for (size_t i
= 0; i
< actuators
.size(); i
++) {
323 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
324 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
325 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
326 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
);
331 //A GCode has been received
332 //See if the current Gcode line has some orders for us
333 void Robot::on_gcode_received(void *argument
)
335 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
337 this->motion_mode
= -1;
341 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
342 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
343 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
344 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
345 case 4: { // G4 pause
346 uint32_t delay_ms
= 0;
347 if (gcode
->has_letter('P')) {
348 delay_ms
= gcode
->get_int('P');
350 if (gcode
->has_letter('S')) {
351 delay_ms
+= gcode
->get_int('S') * 1000;
355 THEKERNEL
->conveyor
->wait_for_empty_queue();
356 // wait for specified time
357 uint32_t start
= us_ticker_read(); // mbed call
358 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
359 THEKERNEL
->call_event(ON_IDLE
, this);
360 if(THEKERNEL
->is_halted()) return;
366 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
367 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
368 size_t n
= gcode
->get_uint('P');
369 if(n
== 0) n
= current_wcs
; // set current coordinate system
373 std::tie(x
, y
, z
) = wcs_offsets
[n
];
374 if(gcode
->get_int('L') == 20) {
375 // this makes the current machine position (less compensation transform) the offset
376 // get current position in WCS
377 wcs_t pos
= mcs2wcs(last_milestone
);
379 if(gcode
->has_letter('X')){
380 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
383 if(gcode
->has_letter('Y')){
384 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
386 if(gcode
->has_letter('Z')) {
387 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
391 // the value is the offset from machine zero
392 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
393 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
394 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
396 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
401 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
402 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
403 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
404 case 20: this->inch_mode
= true; break;
405 case 21: this->inch_mode
= false; break;
407 case 54: case 55: case 56: case 57: case 58: case 59:
408 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
409 current_wcs
= gcode
->g
- 54;
410 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
411 current_wcs
+= gcode
->subcode
;
412 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
416 case 90: this->absolute_mode
= true; break;
417 case 91: this->absolute_mode
= false; break;
420 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
421 // reset G92 offsets to 0
422 g92_offset
= wcs_t(0, 0, 0);
425 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
427 std::tie(x
, y
, z
) = g92_offset
;
428 // get current position in WCS
429 wcs_t pos
= mcs2wcs(last_milestone
);
431 // adjust g92 offset to make the current wpos == the value requested
432 if(gcode
->has_letter('X')){
433 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
435 if(gcode
->has_letter('Y')){
436 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
438 if(gcode
->has_letter('Z')) {
439 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
441 g92_offset
= wcs_t(x
, y
, z
);
448 } else if( gcode
->has_m
) {
450 case 0: // M0 feed hold
451 if(THEKERNEL
->is_grbl_mode()) THEKERNEL
->set_feed_hold(true);
454 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
455 if(!THEKERNEL
->is_grbl_mode()) break;
456 // fall through to M2
457 case 2: // M2 end of program
459 absolute_mode
= true;
462 case 92: // M92 - set steps per mm
463 if (gcode
->has_letter('X'))
464 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
465 if (gcode
->has_letter('Y'))
466 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
467 if (gcode
->has_letter('Z'))
468 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
470 gcode
->stream
->printf("X:%f Y:%f Z:%f ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
471 gcode
->add_nl
= true;
472 check_max_actuator_speeds();
477 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
478 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
482 case 120: // push state
486 case 121: // pop state
490 case 203: // M203 Set maximum feedrates in mm/sec
491 if (gcode
->has_letter('X'))
492 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
493 if (gcode
->has_letter('Y'))
494 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
495 if (gcode
->has_letter('Z'))
496 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
497 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
498 if (gcode
->has_letter('A' + i
))
499 actuators
[i
]->set_max_rate(gcode
->get_value('A' + i
));
501 check_max_actuator_speeds();
503 if(gcode
->get_num_args() == 0) {
504 gcode
->stream
->printf("X:%g Y:%g Z:%g",
505 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
506 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
507 gcode
->stream
->printf(" %c : %g", 'A' + i
, actuators
[i
]->get_max_rate()); //xxx
509 gcode
->add_nl
= true;
513 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
514 if (gcode
->has_letter('S')) {
515 float acc
= gcode
->get_value('S'); // mm/s^2
519 THEKERNEL
->planner
->acceleration
= acc
;
521 if (gcode
->has_letter('Z')) {
522 float acc
= gcode
->get_value('Z'); // mm/s^2
526 THEKERNEL
->planner
->z_acceleration
= acc
;
530 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
531 if (gcode
->has_letter('X')) {
532 float jd
= gcode
->get_value('X');
536 THEKERNEL
->planner
->junction_deviation
= jd
;
538 if (gcode
->has_letter('Z')) {
539 float jd
= gcode
->get_value('Z');
540 // enforce minimum, -1 disables it and uses regular junction deviation
543 THEKERNEL
->planner
->z_junction_deviation
= jd
;
545 if (gcode
->has_letter('S')) {
546 float mps
= gcode
->get_value('S');
550 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
552 if (gcode
->has_letter('Y')) {
553 actuators
[0]->default_minimum_actuator_rate
= gcode
->get_value('Y');
557 case 220: // M220 - speed override percentage
558 if (gcode
->has_letter('S')) {
559 float factor
= gcode
->get_value('S');
560 // enforce minimum 10% speed
563 // enforce maximum 10x speed
564 if (factor
> 1000.0F
)
567 seconds_per_minute
= 6000.0F
/ factor
;
569 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
573 case 400: // wait until all moves are done up to this point
574 THEKERNEL
->conveyor
->wait_for_empty_queue();
577 case 500: // M500 saves some volatile settings to config override file
578 case 503: { // M503 just prints the settings
579 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
);
580 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
581 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
);
582 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f",
583 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
584 for (size_t i
= 0; i
< 3 && i
< actuators
.size(); i
++) {
585 gcode
->stream
->printf(" %c%1.5f", 'A' + i
, actuators
[i
]->get_max_rate());
587 gcode
->stream
->printf("\n");
589 // get or save any arm solution specific optional values
590 BaseSolution::arm_options_t options
;
591 if(arm_solution
->get_optional(options
) && !options
.empty()) {
592 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
593 for(auto &i
: options
) {
594 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
596 gcode
->stream
->printf("\n");
599 // save wcs_offsets and current_wcs
600 // TODO this may need to be done whenever they change to be compliant
601 gcode
->stream
->printf(";WCS settings\n");
602 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
604 for(auto &i
: wcs_offsets
) {
605 if(i
!= wcs_t(0, 0, 0)) {
607 std::tie(x
, y
, z
) = i
;
608 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
612 // linuxcnc does seem to save G92, so we do too
613 // also it needs to be used to set Z0 on rotary deltas as M206/306 can't be used
614 if(g92_offset
!= wcs_t(0, 0, 0)) {
616 std::tie(x
, y
, z
) = g92_offset
;
617 gcode
->stream
->printf("G92 X%f Y%f Z%f\n", x
, y
, z
);
622 case 665: { // M665 set optional arm solution variables based on arm solution.
623 // the parameter args could be any letter each arm solution only accepts certain ones
624 BaseSolution::arm_options_t options
= gcode
->get_args();
625 options
.erase('S'); // don't include the S
626 options
.erase('U'); // don't include the U
627 if(options
.size() > 0) {
628 // set the specified options
629 arm_solution
->set_optional(options
);
632 if(arm_solution
->get_optional(options
)) {
633 // foreach optional value
634 for(auto &i
: options
) {
635 // print all current values of supported options
636 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
637 gcode
->add_nl
= true;
641 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
642 this->delta_segments_per_second
= gcode
->get_value('S');
643 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
645 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
646 this->mm_per_line_segment
= gcode
->get_value('U');
647 this->delta_segments_per_second
= 0;
648 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
656 if( this->motion_mode
>= 0) {
660 next_command_is_MCS
= false; // must be on same line as G0 or G1
663 // process a G0/G1/G2/G3
664 void Robot::process_move(Gcode
*gcode
)
666 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
667 float param
[3]{NAN
, NAN
, NAN
};
668 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
670 if( gcode
->has_letter(letter
) ) {
671 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
675 float offset
[3]{0,0,0};
676 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
677 if( gcode
->has_letter(letter
) ) {
678 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
682 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
683 float target
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
684 if(!next_command_is_MCS
) {
685 if(this->absolute_mode
) {
686 // apply wcs offsets and g92 offset and tool offset
687 if(!isnan(param
[X_AXIS
])) {
688 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
);
691 if(!isnan(param
[Y_AXIS
])) {
692 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
);
695 if(!isnan(param
[Z_AXIS
])) {
696 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
);
700 // they are deltas from the last_milestone if specified
701 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
702 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
707 // already in machine coordinates, we do not add tool offset for that
708 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
709 if(!isnan(param
[i
])) target
[i
] = param
[i
];
713 if( gcode
->has_letter('F') ) {
714 if( this->motion_mode
== MOTION_MODE_SEEK
)
715 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
717 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
721 //Perform any physical actions
722 switch(this->motion_mode
) {
723 case MOTION_MODE_CANCEL
:
725 case MOTION_MODE_SEEK
:
726 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
);
728 case MOTION_MODE_LINEAR
:
729 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
731 case MOTION_MODE_CW_ARC
:
732 case MOTION_MODE_CCW_ARC
:
733 moved
= this->compute_arc(gcode
, offset
, target
);
738 // set last_milestone to the calculated target
739 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
));
743 // We received a new gcode, and one of the functions
744 // determined the distance for that given gcode. So now we can attach this gcode to the right block
746 void Robot::distance_in_gcode_is_known(Gcode
* gcode
)
748 //If the queue is empty, execute immediately, otherwise attach to the last added block
749 THEKERNEL
->conveyor
->append_gcode(gcode
);
752 // reset the machine position for all axis. Used for homing.
753 // During homing compensation is turned off (actually not used as it drives steppers directly)
754 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
755 // so in those cases the final position is compensated.
756 void Robot::reset_axis_position(float x
, float y
, float z
)
758 // these are set to the same as compensation was not used to get to the current position
759 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
760 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
761 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
763 // now set the actuator positions to match
764 ActuatorCoordinates actuator_pos
;
765 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
766 for (size_t i
= 0; i
< actuators
.size(); i
++)
767 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
770 // Reset the position for an axis (used in homing)
771 void Robot::reset_axis_position(float position
, int axis
)
773 last_milestone
[axis
] = position
;
774 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
777 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
778 // then sets the axis positions to match. currently only called from Endstops.cpp
779 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
781 for (size_t i
= 0; i
< actuators
.size(); i
++)
782 actuators
[i
]->change_last_milestone(ac
[i
]);
784 // now correct axis positions then recorrect actuator to account for rounding
785 reset_position_from_current_actuator_position();
788 // Use FK to find out where actuator is and reset to match
789 void Robot::reset_position_from_current_actuator_position()
791 ActuatorCoordinates actuator_pos
;
792 for (size_t i
= 0; i
< actuators
.size(); i
++) {
793 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
794 actuator_pos
[i
] = actuators
[i
]->get_current_position();
797 // discover machine position from where actuators actually are
798 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
799 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
800 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
802 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
803 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
804 // to get everything in perfect sync.
805 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
806 for (size_t i
= 0; i
< actuators
.size(); i
++)
807 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
810 // Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
811 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
812 // all transforms and is what we actually convert to actuator positions
813 bool Robot::append_milestone(Gcode
* gcode
, const float target
[], float rate_mm_s
)
817 ActuatorCoordinates actuator_pos
;
818 float transformed_target
[3]; // adjust target for bed compensation and WCS offsets
819 float millimeters_of_travel
;
821 // unity transform by default
822 memcpy(transformed_target
, target
, sizeof(transformed_target
));
824 // check function pointer and call if set to transform the target to compensate for bed
825 if(compensationTransform
) {
826 // some compensation strategies can transform XYZ, some just change Z
827 compensationTransform(transformed_target
);
830 // find distance moved by each axis, use transformed target from the current machine position
831 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
832 deltas
[axis
] = transformed_target
[axis
] - last_machine_position
[axis
];
835 // Compute how long this move moves, so we can attach it to the block for later use
836 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
838 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
839 // 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
840 if(millimeters_of_travel
< 0.00001F
) return false;
842 // this is the machine position
843 memcpy(this->last_machine_position
, transformed_target
, sizeof(this->last_machine_position
));
845 // find distance unit vector
846 for (int i
= 0; i
< 3; i
++)
847 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
849 // Do not move faster than the configured cartesian limits
850 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
851 if ( max_speeds
[axis
] > 0 ) {
852 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
854 if (axis_speed
> max_speeds
[axis
])
855 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
859 // find actuator position given the machine position, use actual adjusted target
860 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
862 float isecs
= rate_mm_s
/ millimeters_of_travel
;
863 // check per-actuator speed limits
864 for (size_t actuator
= 0; actuator
< actuators
.size(); actuator
++) {
865 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
866 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
867 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
868 isecs
= rate_mm_s
/ millimeters_of_travel
;
872 // Append the block to the planner
873 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
878 // Append a move to the queue ( cutting it into segments if needed )
879 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
)
881 // Find out the distance for this move in MCS
882 // 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
883 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 ));
885 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
886 if( gcode
->millimeters_of_travel
< 0.00001F
) return false;
888 // Mark the gcode as having a known distance
889 this->distance_in_gcode_is_known( gcode
);
891 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
892 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
893 // We also check we are not exceeding the E max_speed for the current extruder
894 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
895 // we need to ask it now passing in the relevant data.
896 // NOTE we need to do this before we segment the line (for deltas)
897 if(gcode
->has_letter('E')) {
899 data
[0] = gcode
->get_value('E'); // E target (may be absolute or relative)
900 data
[1] = rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
901 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
902 rate_mm_s
*= data
[1];
903 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
907 // 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.
908 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
909 // 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
912 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
915 } else if(this->delta_segments_per_second
> 1.0F
) {
916 // enabled if set to something > 1, it is set to 0.0 by default
917 // segment based on current speed and requested segments per second
918 // the faster the travel speed the fewer segments needed
919 // NOTE rate is mm/sec and we take into account any speed override
920 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
921 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
922 // TODO if we are only moving in Z on a delta we don't really need to segment at all
925 if(this->mm_per_line_segment
== 0.0F
) {
926 segments
= 1; // don't split it up
928 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
934 // A vector to keep track of the endpoint of each segment
935 float segment_delta
[3];
936 float segment_end
[3]{last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]};
938 // How far do we move each segment?
939 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
940 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
942 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
943 // We always add another point after this loop so we stop at segments-1, ie i < segments
944 for (int i
= 1; i
< segments
; i
++) {
945 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
946 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
947 segment_end
[axis
] += segment_delta
[axis
];
949 // Append the end of this segment to the queue
950 bool b
= this->append_milestone(gcode
, segment_end
, rate_mm_s
);
955 // Append the end of this full move to the queue
956 if(this->append_milestone(gcode
, target
, rate_mm_s
)) moved
= true;
958 this->next_command_is_MCS
= false; // always reset this
961 // if adding these blocks didn't start executing, do that now
962 THEKERNEL
->conveyor
->ensure_running();
969 // Append an arc to the queue ( cutting it into segments as needed )
970 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
974 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
975 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
976 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
977 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
978 float r_axis1
= -offset
[this->plane_axis_1
];
979 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
980 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
982 // Patch from GRBL Firmware - Christoph Baumann 04072015
983 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
984 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
985 if (is_clockwise
) { // Correct atan2 output per direction
986 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2 * M_PI
; }
988 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2 * M_PI
; }
991 // Find the distance for this gcode
992 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
994 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
995 if( gcode
->millimeters_of_travel
< 0.00001F
) {
999 // Mark the gcode as having a known distance
1000 this->distance_in_gcode_is_known( gcode
);
1002 // Figure out how many segments for this gcode
1003 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
1005 float theta_per_segment
= angular_travel
/ segments
;
1006 float linear_per_segment
= linear_travel
/ segments
;
1008 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1009 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1010 r_T = [cos(phi) -sin(phi);
1011 sin(phi) cos(phi] * r ;
1012 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1013 defined from the circle center to the initial position. Each line segment is formed by successive
1014 vector rotations. This requires only two cos() and sin() computations to form the rotation
1015 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1016 all float numbers are single precision on the Arduino. (True float precision will not have
1017 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1018 tool precision in some cases. Therefore, arc path correction is implemented.
1020 Small angle approximation may be used to reduce computation overhead further. This approximation
1021 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1022 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1023 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1024 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1025 issue for CNC machines with the single precision Arduino calculations.
1026 This approximation also allows mc_arc to immediately insert a line segment into the planner
1027 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1028 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1029 This is important when there are successive arc motions.
1031 // Vector rotation matrix values
1032 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1033 float sin_T
= theta_per_segment
;
1035 float arc_target
[3];
1042 // Initialize the linear axis
1043 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1046 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1047 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1049 if (count
< this->arc_correction
) {
1050 // Apply vector rotation matrix
1051 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1052 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1056 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1057 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1058 cos_Ti
= cosf(i
* theta_per_segment
);
1059 sin_Ti
= sinf(i
* theta_per_segment
);
1060 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1061 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1065 // Update arc_target location
1066 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1067 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1068 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1070 // Append this segment to the queue
1071 bool b
= this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
1075 // Ensure last segment arrives at target location.
1076 if(this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
)) moved
= true;
1081 // Do the math for an arc and add it to the queue
1082 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[])
1086 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1088 // Set clockwise/counter-clockwise sign for mc_arc computations
1089 bool is_clockwise
= false;
1090 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
1091 is_clockwise
= true;
1095 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1099 float Robot::theta(float x
, float y
)
1101 float t
= atanf(x
/ fabs(y
));
1113 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1115 this->plane_axis_0
= axis_0
;
1116 this->plane_axis_1
= axis_1
;
1117 this->plane_axis_2
= axis_2
;
1120 void Robot::clearToolOffset()
1122 this->tool_offset
= wcs_t(0,0,0);
1125 void Robot::setToolOffset(const float offset
[3])
1127 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1130 float Robot::get_feed_rate() const
1132 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;