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/MorganSCARASolution.h"
33 #include "StepTicker.h"
34 #include "checksumm.h"
36 #include "ConfigValue.h"
37 #include "libs/StreamOutput.h"
38 #include "StreamOutputPool.h"
39 #include "ExtruderPublicAccess.h"
41 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
42 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
43 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
44 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
45 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
46 #define arc_correction_checksum CHECKSUM("arc_correction")
47 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
48 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
49 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
52 #define arm_solution_checksum CHECKSUM("arm_solution")
53 #define cartesian_checksum CHECKSUM("cartesian")
54 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
55 #define rostock_checksum CHECKSUM("rostock")
56 #define linear_delta_checksum CHECKSUM("linear_delta")
57 #define rotatable_delta_checksum CHECKSUM("rotatable_delta")
58 #define delta_checksum CHECKSUM("delta")
59 #define hbot_checksum CHECKSUM("hbot")
60 #define corexy_checksum CHECKSUM("corexy")
61 #define kossel_checksum CHECKSUM("kossel")
62 #define morgan_checksum CHECKSUM("morgan")
64 // stepper motor stuff
65 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
66 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
67 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
68 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
69 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
70 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
71 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
72 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
73 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
75 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
76 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
77 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
79 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
80 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
81 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
84 // new-style actuator stuff
85 #define actuator_checksum CHEKCSUM("actuator")
87 #define step_pin_checksum CHECKSUM("step_pin")
88 #define dir_pin_checksum CHEKCSUM("dir_pin")
89 #define en_pin_checksum CHECKSUM("en_pin")
91 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
92 #define max_rate_checksum CHECKSUM("max_rate")
94 #define alpha_checksum CHECKSUM("alpha")
95 #define beta_checksum CHECKSUM("beta")
96 #define gamma_checksum CHECKSUM("gamma")
98 #define NEXT_ACTION_DEFAULT 0
99 #define NEXT_ACTION_DWELL 1
100 #define NEXT_ACTION_GO_HOME 2
102 #define MOTION_MODE_SEEK 0 // G0
103 #define MOTION_MODE_LINEAR 1 // G1
104 #define MOTION_MODE_CW_ARC 2 // G2
105 #define MOTION_MODE_CCW_ARC 3 // G3
106 #define MOTION_MODE_CANCEL 4 // G80
108 #define PATH_CONTROL_MODE_EXACT_PATH 0
109 #define PATH_CONTROL_MODE_EXACT_STOP 1
110 #define PATH_CONTROL_MODE_CONTINOUS 2
112 #define PROGRAM_FLOW_RUNNING 0
113 #define PROGRAM_FLOW_PAUSED 1
114 #define PROGRAM_FLOW_COMPLETED 2
116 #define SPINDLE_DIRECTION_CW 0
117 #define SPINDLE_DIRECTION_CCW 1
119 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
121 // 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
122 // It takes care of cutting arcs into segments, same thing for line that are too long
126 this->inch_mode
= false;
127 this->absolute_mode
= true;
128 this->motion_mode
= MOTION_MODE_SEEK
;
129 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
130 clear_vector(this->last_milestone
);
131 clear_vector(this->transformed_last_milestone
);
132 this->arm_solution
= NULL
;
133 seconds_per_minute
= 60.0F
;
134 this->clearToolOffset();
135 this->compensationTransform
= nullptr;
138 //Called when the module has just been loaded
139 void Robot::on_module_loaded()
141 this->register_for_event(ON_GCODE_RECEIVED
);
144 this->on_config_reload(this);
147 void Robot::on_config_reload(void *argument
)
150 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
151 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
152 // To make adding those solution easier, they have their own, separate object.
153 // Here we read the config to find out which arm solution to use
154 if (this->arm_solution
) delete this->arm_solution
;
155 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
156 // Note checksums are not const expressions when in debug mode, so don't use switch
157 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
158 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
160 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
161 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
163 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
164 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
166 } else if(solution_checksum
== rotatable_delta_checksum
) {
167 this->arm_solution
= new RotatableDeltaSolution(THEKERNEL
->config
);
170 } else if(solution_checksum
== morgan_checksum
) {
171 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
173 } else if(solution_checksum
== cartesian_checksum
) {
174 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
177 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
181 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
182 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
183 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
184 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
185 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
186 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
188 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
189 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
190 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
202 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
203 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
204 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
205 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
206 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
207 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
208 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
209 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
210 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
212 float steps_per_mm
[3] = {
213 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
214 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
215 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
218 // TODO: delete or detect old steppermotors
219 // Make our 3 StepperMotors
220 this->alpha_stepper_motor
= new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
);
221 this->beta_stepper_motor
= new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
);
222 this->gamma_stepper_motor
= new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
);
224 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
225 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
226 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
228 alpha_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
229 beta_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
230 gamma_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
231 check_max_actuator_speeds(); // check the configs are sane
234 actuators
.push_back(alpha_stepper_motor
);
235 actuators
.push_back(beta_stepper_motor
);
236 actuators
.push_back(gamma_stepper_motor
);
239 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
240 // so the first move can be correct if homing is not performed
241 float actuator_pos
[3];
242 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
243 for (int i
= 0; i
< 3; i
++)
244 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
246 //this->clearToolOffset();
249 // this does a sanity check that actuator speeds do not exceed steps rate capability
250 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
251 void Robot::check_max_actuator_speeds()
253 float step_freq
= alpha_stepper_motor
->get_max_rate() * alpha_stepper_motor
->get_steps_per_mm();
254 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
255 alpha_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm()));
256 THEKERNEL
->streams
->printf("WARNING: alpha_max_rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", step_freq
, alpha_stepper_motor
->max_rate
);
259 step_freq
= beta_stepper_motor
->get_max_rate() * beta_stepper_motor
->get_steps_per_mm();
260 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
261 beta_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm()));
262 THEKERNEL
->streams
->printf("WARNING: beta_max_rate exceeds base_stepping_frequency * beta_steps_per_mm: %f, setting to %f\n", step_freq
, beta_stepper_motor
->max_rate
);
265 step_freq
= gamma_stepper_motor
->get_max_rate() * gamma_stepper_motor
->get_steps_per_mm();
266 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
267 gamma_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm()));
268 THEKERNEL
->streams
->printf("WARNING: gamma_max_rate exceeds base_stepping_frequency * gamma_steps_per_mm: %f, setting to %f\n", step_freq
, gamma_stepper_motor
->max_rate
);
272 //A GCode has been received
273 //See if the current Gcode line has some orders for us
274 void Robot::on_gcode_received(void *argument
)
276 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
278 this->motion_mode
= -1;
280 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
283 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
284 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
285 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
286 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
288 uint32_t delay_ms
= 0;
289 if (gcode
->has_letter('P')) {
290 delay_ms
= gcode
->get_int('P');
292 if (gcode
->has_letter('S')) {
293 delay_ms
+= gcode
->get_int('S') * 1000;
297 THEKERNEL
->conveyor
->wait_for_empty_queue();
298 // wait for specified time
299 uint32_t start
= us_ticker_read(); // mbed call
300 while ((us_ticker_read() - start
) < delay_ms
*1000) {
301 THEKERNEL
->call_event(ON_IDLE
, this);
306 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
307 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
308 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
309 case 20: this->inch_mode
= true; break;
310 case 21: this->inch_mode
= false; break;
311 case 90: this->absolute_mode
= true; break;
312 case 91: this->absolute_mode
= false; break;
314 if(gcode
->get_num_args() == 0) {
315 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
316 reset_axis_position(0, i
);
320 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
321 if ( gcode
->has_letter(letter
) ) {
322 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
329 } else if( gcode
->has_m
) {
331 case 92: // M92 - set steps per mm
332 if (gcode
->has_letter('X'))
333 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
334 if (gcode
->has_letter('Y'))
335 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
336 if (gcode
->has_letter('Z'))
337 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
338 if (gcode
->has_letter('F'))
339 seconds_per_minute
= gcode
->get_value('F');
341 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
, seconds_per_minute
);
342 gcode
->add_nl
= true;
343 check_max_actuator_speeds();
348 int n
= snprintf(buf
, sizeof(buf
), "C: X:%1.3f Y:%1.3f Z:%1.3f A:%1.3f B:%1.3f C:%1.3f ",
349 from_millimeters(this->last_milestone
[0]),
350 from_millimeters(this->last_milestone
[1]),
351 from_millimeters(this->last_milestone
[2]),
352 actuators
[X_AXIS
]->get_current_position(),
353 actuators
[Y_AXIS
]->get_current_position(),
354 actuators
[Z_AXIS
]->get_current_position() );
355 gcode
->txt_after_ok
.append(buf
, n
);
359 case 120: { // push state
360 bool b
= this->absolute_mode
;
361 saved_state_t
s(this->feed_rate
, this->seek_rate
, b
);
366 case 121: // pop state
367 if(!state_stack
.empty()) {
368 auto s
= state_stack
.top();
370 this->feed_rate
= std::get
<0>(s
);
371 this->seek_rate
= std::get
<1>(s
);
372 this->absolute_mode
= std::get
<2>(s
);
376 case 203: // M203 Set maximum feedrates in mm/sec
377 if (gcode
->has_letter('X'))
378 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
379 if (gcode
->has_letter('Y'))
380 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
381 if (gcode
->has_letter('Z'))
382 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
383 if (gcode
->has_letter('A'))
384 alpha_stepper_motor
->set_max_rate(gcode
->get_value('A'));
385 if (gcode
->has_letter('B'))
386 beta_stepper_motor
->set_max_rate(gcode
->get_value('B'));
387 if (gcode
->has_letter('C'))
388 gamma_stepper_motor
->set_max_rate(gcode
->get_value('C'));
390 check_max_actuator_speeds();
392 if(gcode
->get_num_args() == 0) {
393 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
394 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
395 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
396 gcode
->add_nl
= true;
401 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
402 if (gcode
->has_letter('S')) {
403 float acc
= gcode
->get_value('S'); // mm/s^2
407 THEKERNEL
->planner
->acceleration
= acc
;
409 if (gcode
->has_letter('Z')) {
410 float acc
= gcode
->get_value('Z'); // mm/s^2
414 THEKERNEL
->planner
->z_acceleration
= acc
;
418 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
419 if (gcode
->has_letter('X')) {
420 float jd
= gcode
->get_value('X');
424 THEKERNEL
->planner
->junction_deviation
= jd
;
426 if (gcode
->has_letter('Z')) {
427 float jd
= gcode
->get_value('Z');
428 // enforce minimum, -1 disables it and uses regular junction deviation
431 THEKERNEL
->planner
->z_junction_deviation
= jd
;
433 if (gcode
->has_letter('S')) {
434 float mps
= gcode
->get_value('S');
438 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
440 if (gcode
->has_letter('Y')) {
441 alpha_stepper_motor
->default_minimum_actuator_rate
= gcode
->get_value('Y');
445 case 220: // M220 - speed override percentage
446 if (gcode
->has_letter('S')) {
447 float factor
= gcode
->get_value('S');
448 // enforce minimum 10% speed
451 // enforce maximum 10x speed
452 if (factor
> 1000.0F
)
455 seconds_per_minute
= 6000.0F
/ factor
;
457 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
461 case 400: // wait until all moves are done up to this point
462 THEKERNEL
->conveyor
->wait_for_empty_queue();
465 case 500: // M500 saves some volatile settings to config override file
466 case 503: { // M503 just prints the settings
467 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
);
468 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
469 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
);
470 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n",
471 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
472 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
474 // get or save any arm solution specific optional values
475 BaseSolution::arm_options_t options
;
476 if(arm_solution
->get_optional(options
) && !options
.empty()) {
477 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
478 for(auto &i
: options
) {
479 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
481 gcode
->stream
->printf("\n");
487 case 665: { // M665 set optional arm solution variables based on arm solution.
488 // the parameter args could be any letter each arm solution only accepts certain ones
489 BaseSolution::arm_options_t options
= gcode
->get_args();
490 options
.erase('S'); // don't include the S
491 options
.erase('U'); // don't include the U
492 if(options
.size() > 0) {
493 // set the specified options
494 arm_solution
->set_optional(options
);
497 if(arm_solution
->get_optional(options
)) {
498 // foreach optional value
499 for(auto &i
: options
) {
500 // print all current values of supported options
501 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
502 gcode
->add_nl
= true;
506 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
507 this->delta_segments_per_second
= gcode
->get_value('S');
508 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
510 }else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
511 this->mm_per_line_segment
= gcode
->get_value('U');
512 this->delta_segments_per_second
= 0;
513 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
521 if( this->motion_mode
< 0)
525 float target
[3], offset
[3];
526 clear_vector(offset
);
528 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
530 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
531 if( gcode
->has_letter(letter
) ) {
532 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
535 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
536 if( gcode
->has_letter(letter
) ) {
537 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
541 if( gcode
->has_letter('F') ) {
542 if( this->motion_mode
== MOTION_MODE_SEEK
)
543 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
545 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
548 //Perform any physical actions
549 switch(this->motion_mode
) {
550 case MOTION_MODE_CANCEL
: break;
551 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
552 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
553 case MOTION_MODE_CW_ARC
:
554 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
557 // last_milestone was set to target in append_milestone, no need to do it again
561 // We received a new gcode, and one of the functions
562 // determined the distance for that given gcode. So now we can attach this gcode to the right block
564 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
566 //If the queue is empty, execute immediatly, otherwise attach to the last added block
567 THEKERNEL
->conveyor
->append_gcode(gcode
);
570 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
571 void Robot::reset_axis_position(float x
, float y
, float z
)
573 this->last_milestone
[X_AXIS
] = x
;
574 this->last_milestone
[Y_AXIS
] = y
;
575 this->last_milestone
[Z_AXIS
] = z
;
576 this->transformed_last_milestone
[X_AXIS
] = x
;
577 this->transformed_last_milestone
[Y_AXIS
] = y
;
578 this->transformed_last_milestone
[Z_AXIS
] = z
;
580 float actuator_pos
[3];
581 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
582 for (int i
= 0; i
< 3; i
++)
583 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
586 // Reset the position for an axis (used in homing and G92)
587 void Robot::reset_axis_position(float position
, int axis
)
589 this->last_milestone
[axis
] = position
;
590 this->transformed_last_milestone
[axis
] = position
;
592 float actuator_pos
[3];
593 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
595 for (int i
= 0; i
< 3; i
++)
596 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
599 // Use FK to find out where actuator is and reset lastmilestone to match
600 void Robot::reset_position_from_current_actuator_position()
602 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
603 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
604 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
606 // now reset actuator correctly, NOTE this may lose a little precision
607 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
608 for (int i
= 0; i
< 3; i
++)
609 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
612 // Convert target from millimeters to steps, and append this to the planner
613 void Robot::append_milestone(Gcode
*gcode
, float target
[], float rate_mm_s
)
617 float actuator_pos
[3];
618 float transformed_target
[3]; // adjust target for bed compensation
619 float millimeters_of_travel
;
621 // unity transform by default
622 memcpy(transformed_target
, target
, sizeof(transformed_target
));
624 // check function pointer and call if set to transform the target to compensate for bed
625 if(compensationTransform
) {
626 // some compensation strategies can transform XYZ, some just change Z
627 compensationTransform(transformed_target
);
630 // find distance moved by each axis, use transformed target from last_transformed_target
631 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
632 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
634 // store last transformed
635 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
637 // Compute how long this move moves, so we can attach it to the block for later use
638 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
640 // find distance unit vector
641 for (int i
= 0; i
< 3; i
++)
642 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
644 // Do not move faster than the configured cartesian limits
645 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
646 if ( max_speeds
[axis
] > 0 ) {
647 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
649 if (axis_speed
> max_speeds
[axis
])
650 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
654 // find actuator position given cartesian position, use actual adjusted target
655 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
657 float isecs
= rate_mm_s
/ millimeters_of_travel
;
658 // check per-actuator speed limits
659 for (int actuator
= 0; actuator
<= 2; actuator
++) {
660 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
661 if (actuator_rate
> actuators
[actuator
]->get_max_rate()){
662 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
663 isecs
= rate_mm_s
/ millimeters_of_travel
;
667 // Append the block to the planner
668 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
670 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
671 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
675 // Append a move to the queue ( cutting it into segments if needed )
676 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
678 // Find out the distance for this gcode
679 // 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
680 gcode
->millimeters_of_travel
= sqrtf(powf( target
[X_AXIS
] - this->last_milestone
[X_AXIS
], 2 ) + powf( target
[Y_AXIS
] - this->last_milestone
[Y_AXIS
], 2 ) + powf( target
[Z_AXIS
] - this->last_milestone
[Z_AXIS
], 2 ));
682 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
683 if( gcode
->millimeters_of_travel
< 0.00001F
) {
687 // Mark the gcode as having a known distance
688 this->distance_in_gcode_is_known( gcode
);
690 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
691 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
692 // We also check we are not exceeding the E max_speed for the current extruder
693 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
694 // we need to ask it now passing in the relevant data.
695 // NOTE we need to do this before we segment the line (for deltas)
696 if(gcode
->has_letter('E')) {
698 data
[0]= gcode
->get_value('E'); // E target (maybe absolute or relative)
699 data
[1]= rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
700 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
701 rate_mm_s
*= data
[1];
702 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
706 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
707 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
708 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
709 // 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
712 if(this->delta_segments_per_second
> 1.0F
) {
713 // enabled if set to something > 1, it is set to 0.0 by default
714 // segment based on current speed and requested segments per second
715 // the faster the travel speed the fewer segments needed
716 // NOTE rate is mm/sec and we take into account any speed override
717 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
718 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
719 // TODO if we are only moving in Z on a delta we don't really need to segment at all
722 if(this->mm_per_line_segment
== 0.0F
) {
723 segments
= 1; // don't split it up
725 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
730 // A vector to keep track of the endpoint of each segment
731 float segment_delta
[3];
732 float segment_end
[3];
734 // How far do we move each segment?
735 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
736 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
738 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
739 // We always add another point after this loop so we stop at segments-1, ie i < segments
740 for (int i
= 1; i
< segments
; i
++) {
741 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
742 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
743 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
745 // Append the end of this segment to the queue
746 this->append_milestone(gcode
, segment_end
, rate_mm_s
);
750 // Append the end of this full move to the queue
751 this->append_milestone(gcode
, target
, rate_mm_s
);
753 // if adding these blocks didn't start executing, do that now
754 THEKERNEL
->conveyor
->ensure_running();
758 // Append an arc to the queue ( cutting it into segments as needed )
759 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
763 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
764 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
765 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
766 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
767 float r_axis1
= -offset
[this->plane_axis_1
];
768 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
769 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
771 // Patch from GRBL Firmware - Christoph Baumann 04072015
772 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
773 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
774 if (is_clockwise
) { // Correct atan2 output per direction
775 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2*M_PI
; }
777 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2*M_PI
; }
780 // Find the distance for this gcode
781 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
783 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
784 if( gcode
->millimeters_of_travel
< 0.00001F
) {
788 // Mark the gcode as having a known distance
789 this->distance_in_gcode_is_known( gcode
);
791 // Figure out how many segments for this gcode
792 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
794 float theta_per_segment
= angular_travel
/ segments
;
795 float linear_per_segment
= linear_travel
/ segments
;
797 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
798 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
799 r_T = [cos(phi) -sin(phi);
800 sin(phi) cos(phi] * r ;
801 For arc generation, the center of the circle is the axis of rotation and the radius vector is
802 defined from the circle center to the initial position. Each line segment is formed by successive
803 vector rotations. This requires only two cos() and sin() computations to form the rotation
804 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
805 all float numbers are single precision on the Arduino. (True float precision will not have
806 round off issues for CNC applications.) Single precision error can accumulate to be greater than
807 tool precision in some cases. Therefore, arc path correction is implemented.
809 Small angle approximation may be used to reduce computation overhead further. This approximation
810 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
811 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
812 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
813 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
814 issue for CNC machines with the single precision Arduino calculations.
815 This approximation also allows mc_arc to immediately insert a line segment into the planner
816 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
817 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
818 This is important when there are successive arc motions.
820 // Vector rotation matrix values
821 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
822 float sin_T
= theta_per_segment
;
831 // Initialize the linear axis
832 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
834 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
835 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
837 if (count
< this->arc_correction
) {
838 // Apply vector rotation matrix
839 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
840 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
844 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
845 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
846 cos_Ti
= cosf(i
* theta_per_segment
);
847 sin_Ti
= sinf(i
* theta_per_segment
);
848 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
849 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
853 // Update arc_target location
854 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
855 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
856 arc_target
[this->plane_axis_2
] += linear_per_segment
;
858 // Append this segment to the queue
859 this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
863 // Ensure last segment arrives at target location.
864 this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
867 // Do the math for an arc and add it to the queue
868 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
872 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
874 // Set clockwise/counter-clockwise sign for mc_arc computations
875 bool is_clockwise
= false;
876 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
881 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
886 float Robot::theta(float x
, float y
)
888 float t
= atanf(x
/ fabs(y
));
900 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
902 this->plane_axis_0
= axis_0
;
903 this->plane_axis_1
= axis_1
;
904 this->plane_axis_2
= axis_2
;
907 void Robot::clearToolOffset()
909 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
912 void Robot::setToolOffset(const float offset
[3])
914 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));