2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
11 #include "mbed.h" // for us_ticker_read()
20 #include "nuts_bolts.h"
22 #include "StepperMotor.h"
24 #include "PublicDataRequest.h"
25 #include "PublicData.h"
26 #include "arm_solutions/BaseSolution.h"
27 #include "arm_solutions/CartesianSolution.h"
28 #include "arm_solutions/RotatableCartesianSolution.h"
29 #include "arm_solutions/LinearDeltaSolution.h"
30 #include "arm_solutions/RotatableDeltaSolution.h"
31 #include "arm_solutions/HBotSolution.h"
32 #include "arm_solutions/CoreXZSolution.h"
33 #include "arm_solutions/MorganSCARASolution.h"
34 #include "StepTicker.h"
35 #include "checksumm.h"
37 #include "ConfigValue.h"
38 #include "libs/StreamOutput.h"
39 #include "StreamOutputPool.h"
40 #include "ExtruderPublicAccess.h"
42 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
43 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
44 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
45 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
46 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
47 #define arc_correction_checksum CHECKSUM("arc_correction")
48 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
49 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
50 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
53 #define arm_solution_checksum CHECKSUM("arm_solution")
54 #define cartesian_checksum CHECKSUM("cartesian")
55 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
56 #define rostock_checksum CHECKSUM("rostock")
57 #define linear_delta_checksum CHECKSUM("linear_delta")
58 #define rotatable_delta_checksum CHECKSUM("rotatable_delta")
59 #define delta_checksum CHECKSUM("delta")
60 #define hbot_checksum CHECKSUM("hbot")
61 #define corexy_checksum CHECKSUM("corexy")
62 #define corexz_checksum CHECKSUM("corexz")
63 #define kossel_checksum CHECKSUM("kossel")
64 #define morgan_checksum CHECKSUM("morgan")
66 // stepper motor stuff
67 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
68 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
69 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
70 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
71 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
72 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
73 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
74 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
75 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
77 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
78 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
79 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
81 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
82 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
83 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
86 // new-style actuator stuff
87 #define actuator_checksum CHEKCSUM("actuator")
89 #define step_pin_checksum CHECKSUM("step_pin")
90 #define dir_pin_checksum CHEKCSUM("dir_pin")
91 #define en_pin_checksum CHECKSUM("en_pin")
93 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
94 #define max_rate_checksum CHECKSUM("max_rate")
96 #define alpha_checksum CHECKSUM("alpha")
97 #define beta_checksum CHECKSUM("beta")
98 #define gamma_checksum CHECKSUM("gamma")
100 #define NEXT_ACTION_DEFAULT 0
101 #define NEXT_ACTION_DWELL 1
102 #define NEXT_ACTION_GO_HOME 2
104 #define MOTION_MODE_SEEK 0 // G0
105 #define MOTION_MODE_LINEAR 1 // G1
106 #define MOTION_MODE_CW_ARC 2 // G2
107 #define MOTION_MODE_CCW_ARC 3 // G3
108 #define MOTION_MODE_CANCEL 4 // G80
110 #define PATH_CONTROL_MODE_EXACT_PATH 0
111 #define PATH_CONTROL_MODE_EXACT_STOP 1
112 #define PATH_CONTROL_MODE_CONTINOUS 2
114 #define PROGRAM_FLOW_RUNNING 0
115 #define PROGRAM_FLOW_PAUSED 1
116 #define PROGRAM_FLOW_COMPLETED 2
118 #define SPINDLE_DIRECTION_CW 0
119 #define SPINDLE_DIRECTION_CCW 1
121 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
123 // 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
124 // It takes care of cutting arcs into segments, same thing for line that are too long
128 this->inch_mode
= false;
129 this->absolute_mode
= true;
130 this->motion_mode
= MOTION_MODE_SEEK
;
131 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
132 clear_vector(this->last_milestone
);
133 clear_vector(this->transformed_last_milestone
);
134 this->arm_solution
= NULL
;
135 seconds_per_minute
= 60.0F
;
136 this->clearToolOffset();
137 this->compensationTransform
= nullptr;
140 //Called when the module has just been loaded
141 void Robot::on_module_loaded()
143 this->register_for_event(ON_GCODE_RECEIVED
);
146 this->on_config_reload(this);
149 void Robot::on_config_reload(void *argument
)
152 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
153 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
154 // To make adding those solution easier, they have their own, separate object.
155 // Here we read the config to find out which arm solution to use
156 if (this->arm_solution
) delete this->arm_solution
;
157 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
158 // Note checksums are not const expressions when in debug mode, so don't use switch
159 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
160 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== corexz_checksum
) {
163 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
166 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
169 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
171 } else if(solution_checksum
== rotatable_delta_checksum
) {
172 this->arm_solution
= new RotatableDeltaSolution(THEKERNEL
->config
);
175 } else if(solution_checksum
== morgan_checksum
) {
176 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
178 } else if(solution_checksum
== cartesian_checksum
) {
179 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
182 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
186 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
187 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
188 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
189 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
190 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
191 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
193 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
194 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
195 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
207 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
208 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
209 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
210 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
211 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
212 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
213 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
214 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
215 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
217 float steps_per_mm
[3] = {
218 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
219 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
220 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
223 // TODO: delete or detect old steppermotors
224 // Make our 3 StepperMotors
225 this->alpha_stepper_motor
= new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
);
226 this->beta_stepper_motor
= new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
);
227 this->gamma_stepper_motor
= new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
);
229 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
230 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
231 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
233 alpha_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
234 beta_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
235 gamma_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
236 check_max_actuator_speeds(); // check the configs are sane
239 actuators
.push_back(alpha_stepper_motor
);
240 actuators
.push_back(beta_stepper_motor
);
241 actuators
.push_back(gamma_stepper_motor
);
244 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
245 // so the first move can be correct if homing is not performed
246 float actuator_pos
[3];
247 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
248 for (int i
= 0; i
< 3; i
++)
249 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
251 //this->clearToolOffset();
254 // this does a sanity check that actuator speeds do not exceed steps rate capability
255 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
256 void Robot::check_max_actuator_speeds()
258 float step_freq
= alpha_stepper_motor
->get_max_rate() * alpha_stepper_motor
->get_steps_per_mm();
259 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
260 alpha_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm()));
261 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
);
264 step_freq
= beta_stepper_motor
->get_max_rate() * beta_stepper_motor
->get_steps_per_mm();
265 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
266 beta_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm()));
267 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
);
270 step_freq
= gamma_stepper_motor
->get_max_rate() * gamma_stepper_motor
->get_steps_per_mm();
271 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
272 gamma_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm()));
273 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
);
277 //A GCode has been received
278 //See if the current Gcode line has some orders for us
279 void Robot::on_gcode_received(void *argument
)
281 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
283 this->motion_mode
= -1;
285 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
288 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
289 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
290 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
291 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
293 uint32_t delay_ms
= 0;
294 if (gcode
->has_letter('P')) {
295 delay_ms
= gcode
->get_int('P');
297 if (gcode
->has_letter('S')) {
298 delay_ms
+= gcode
->get_int('S') * 1000;
302 THEKERNEL
->conveyor
->wait_for_empty_queue();
303 // wait for specified time
304 uint32_t start
= us_ticker_read(); // mbed call
305 while ((us_ticker_read() - start
) < delay_ms
*1000) {
306 THEKERNEL
->call_event(ON_IDLE
, this);
311 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
312 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
313 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
314 case 20: this->inch_mode
= true; break;
315 case 21: this->inch_mode
= false; break;
316 case 90: this->absolute_mode
= true; break;
317 case 91: this->absolute_mode
= false; break;
319 if(gcode
->get_num_args() == 0) {
320 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
321 reset_axis_position(0, i
);
325 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
326 if ( gcode
->has_letter(letter
) ) {
327 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
334 } else if( gcode
->has_m
) {
336 case 92: // M92 - set steps per mm
337 if (gcode
->has_letter('X'))
338 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
339 if (gcode
->has_letter('Y'))
340 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
341 if (gcode
->has_letter('Z'))
342 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
343 if (gcode
->has_letter('F'))
344 seconds_per_minute
= gcode
->get_value('F');
346 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
);
347 gcode
->add_nl
= true;
348 check_max_actuator_speeds();
353 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 ",
354 from_millimeters(this->last_milestone
[0]),
355 from_millimeters(this->last_milestone
[1]),
356 from_millimeters(this->last_milestone
[2]),
357 actuators
[X_AXIS
]->get_current_position(),
358 actuators
[Y_AXIS
]->get_current_position(),
359 actuators
[Z_AXIS
]->get_current_position() );
360 gcode
->txt_after_ok
.append(buf
, n
);
364 case 120: { // push state
365 bool b
= this->absolute_mode
;
366 saved_state_t
s(this->feed_rate
, this->seek_rate
, b
);
371 case 121: // pop state
372 if(!state_stack
.empty()) {
373 auto s
= state_stack
.top();
375 this->feed_rate
= std::get
<0>(s
);
376 this->seek_rate
= std::get
<1>(s
);
377 this->absolute_mode
= std::get
<2>(s
);
381 case 203: // M203 Set maximum feedrates in mm/sec
382 if (gcode
->has_letter('X'))
383 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
384 if (gcode
->has_letter('Y'))
385 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
386 if (gcode
->has_letter('Z'))
387 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
388 if (gcode
->has_letter('A'))
389 alpha_stepper_motor
->set_max_rate(gcode
->get_value('A'));
390 if (gcode
->has_letter('B'))
391 beta_stepper_motor
->set_max_rate(gcode
->get_value('B'));
392 if (gcode
->has_letter('C'))
393 gamma_stepper_motor
->set_max_rate(gcode
->get_value('C'));
395 check_max_actuator_speeds();
397 if(gcode
->get_num_args() == 0) {
398 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
399 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
400 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
401 gcode
->add_nl
= true;
406 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
407 if (gcode
->has_letter('S')) {
408 float acc
= gcode
->get_value('S'); // mm/s^2
412 THEKERNEL
->planner
->acceleration
= acc
;
414 if (gcode
->has_letter('Z')) {
415 float acc
= gcode
->get_value('Z'); // mm/s^2
419 THEKERNEL
->planner
->z_acceleration
= acc
;
423 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
424 if (gcode
->has_letter('X')) {
425 float jd
= gcode
->get_value('X');
429 THEKERNEL
->planner
->junction_deviation
= jd
;
431 if (gcode
->has_letter('Z')) {
432 float jd
= gcode
->get_value('Z');
433 // enforce minimum, -1 disables it and uses regular junction deviation
436 THEKERNEL
->planner
->z_junction_deviation
= jd
;
438 if (gcode
->has_letter('S')) {
439 float mps
= gcode
->get_value('S');
443 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
445 if (gcode
->has_letter('Y')) {
446 alpha_stepper_motor
->default_minimum_actuator_rate
= gcode
->get_value('Y');
450 case 220: // M220 - speed override percentage
451 if (gcode
->has_letter('S')) {
452 float factor
= gcode
->get_value('S');
453 // enforce minimum 10% speed
456 // enforce maximum 10x speed
457 if (factor
> 1000.0F
)
460 seconds_per_minute
= 6000.0F
/ factor
;
462 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
466 case 400: // wait until all moves are done up to this point
467 THEKERNEL
->conveyor
->wait_for_empty_queue();
470 case 500: // M500 saves some volatile settings to config override file
471 case 503: { // M503 just prints the settings
472 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
);
473 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
474 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
);
475 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",
476 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
477 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
479 // get or save any arm solution specific optional values
480 BaseSolution::arm_options_t options
;
481 if(arm_solution
->get_optional(options
) && !options
.empty()) {
482 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
483 for(auto &i
: options
) {
484 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
486 gcode
->stream
->printf("\n");
492 case 665: { // M665 set optional arm solution variables based on arm solution.
493 // the parameter args could be any letter each arm solution only accepts certain ones
494 BaseSolution::arm_options_t options
= gcode
->get_args();
495 options
.erase('S'); // don't include the S
496 options
.erase('U'); // don't include the U
497 if(options
.size() > 0) {
498 // set the specified options
499 arm_solution
->set_optional(options
);
502 if(arm_solution
->get_optional(options
)) {
503 // foreach optional value
504 for(auto &i
: options
) {
505 // print all current values of supported options
506 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
507 gcode
->add_nl
= true;
511 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
512 this->delta_segments_per_second
= gcode
->get_value('S');
513 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
515 }else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
516 this->mm_per_line_segment
= gcode
->get_value('U');
517 this->delta_segments_per_second
= 0;
518 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
526 if( this->motion_mode
< 0)
530 float target
[3], offset
[3];
531 clear_vector(offset
);
533 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
535 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
536 if( gcode
->has_letter(letter
) ) {
537 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
540 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
541 if( gcode
->has_letter(letter
) ) {
542 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
546 if( gcode
->has_letter('F') ) {
547 if( this->motion_mode
== MOTION_MODE_SEEK
)
548 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
550 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
553 //Perform any physical actions
554 switch(this->motion_mode
) {
555 case MOTION_MODE_CANCEL
: break;
556 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
557 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
558 case MOTION_MODE_CW_ARC
:
559 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
562 // last_milestone was set to target in append_milestone, no need to do it again
566 // We received a new gcode, and one of the functions
567 // determined the distance for that given gcode. So now we can attach this gcode to the right block
569 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
571 //If the queue is empty, execute immediatly, otherwise attach to the last added block
572 THEKERNEL
->conveyor
->append_gcode(gcode
);
575 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
576 void Robot::reset_axis_position(float x
, float y
, float z
)
578 this->last_milestone
[X_AXIS
] = x
;
579 this->last_milestone
[Y_AXIS
] = y
;
580 this->last_milestone
[Z_AXIS
] = z
;
581 this->transformed_last_milestone
[X_AXIS
] = x
;
582 this->transformed_last_milestone
[Y_AXIS
] = y
;
583 this->transformed_last_milestone
[Z_AXIS
] = z
;
585 float actuator_pos
[3];
586 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
587 for (int i
= 0; i
< 3; i
++)
588 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
591 // Reset the position for an axis (used in homing and G92)
592 void Robot::reset_axis_position(float position
, int axis
)
594 this->last_milestone
[axis
] = position
;
595 this->transformed_last_milestone
[axis
] = position
;
597 float actuator_pos
[3];
598 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
600 for (int i
= 0; i
< 3; i
++)
601 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
604 // Use FK to find out where actuator is and reset lastmilestone to match
605 void Robot::reset_position_from_current_actuator_position()
607 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
608 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
609 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
611 // now reset actuator correctly, NOTE this may lose a little precision
612 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
613 for (int i
= 0; i
< 3; i
++)
614 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
617 // Convert target from millimeters to steps, and append this to the planner
618 void Robot::append_milestone(Gcode
*gcode
, float target
[], float rate_mm_s
)
622 float actuator_pos
[3];
623 float transformed_target
[3]; // adjust target for bed compensation
624 float millimeters_of_travel
;
626 // unity transform by default
627 memcpy(transformed_target
, target
, sizeof(transformed_target
));
629 // check function pointer and call if set to transform the target to compensate for bed
630 if(compensationTransform
) {
631 // some compensation strategies can transform XYZ, some just change Z
632 compensationTransform(transformed_target
);
635 // find distance moved by each axis, use transformed target from last_transformed_target
636 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
637 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
639 // store last transformed
640 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
642 // Compute how long this move moves, so we can attach it to the block for later use
643 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
645 // find distance unit vector
646 for (int i
= 0; i
< 3; i
++)
647 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
649 // Do not move faster than the configured cartesian limits
650 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
651 if ( max_speeds
[axis
] > 0 ) {
652 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
654 if (axis_speed
> max_speeds
[axis
])
655 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
659 // find actuator position given cartesian position, use actual adjusted target
660 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
662 float isecs
= rate_mm_s
/ millimeters_of_travel
;
663 // check per-actuator speed limits
664 for (int actuator
= 0; actuator
<= 2; actuator
++) {
665 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
666 if (actuator_rate
> actuators
[actuator
]->get_max_rate()){
667 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
668 isecs
= rate_mm_s
/ millimeters_of_travel
;
672 // Append the block to the planner
673 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
675 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
676 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
680 // Append a move to the queue ( cutting it into segments if needed )
681 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
683 // Find out the distance for this gcode
684 // 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
685 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 ));
687 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
688 if( gcode
->millimeters_of_travel
< 0.00001F
) {
692 // Mark the gcode as having a known distance
693 this->distance_in_gcode_is_known( gcode
);
695 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
696 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
697 // We also check we are not exceeding the E max_speed for the current extruder
698 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
699 // we need to ask it now passing in the relevant data.
700 // NOTE we need to do this before we segment the line (for deltas)
701 if(gcode
->has_letter('E')) {
703 data
[0]= gcode
->get_value('E'); // E target (maybe absolute or relative)
704 data
[1]= rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
705 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
706 rate_mm_s
*= data
[1];
707 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
711 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
712 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
713 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
714 // 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
717 if(this->delta_segments_per_second
> 1.0F
) {
718 // enabled if set to something > 1, it is set to 0.0 by default
719 // segment based on current speed and requested segments per second
720 // the faster the travel speed the fewer segments needed
721 // NOTE rate is mm/sec and we take into account any speed override
722 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
723 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
724 // TODO if we are only moving in Z on a delta we don't really need to segment at all
727 if(this->mm_per_line_segment
== 0.0F
) {
728 segments
= 1; // don't split it up
730 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
735 // A vector to keep track of the endpoint of each segment
736 float segment_delta
[3];
737 float segment_end
[3];
739 // How far do we move each segment?
740 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
741 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
743 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
744 // We always add another point after this loop so we stop at segments-1, ie i < segments
745 for (int i
= 1; i
< segments
; i
++) {
746 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
747 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
748 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
750 // Append the end of this segment to the queue
751 this->append_milestone(gcode
, segment_end
, rate_mm_s
);
755 // Append the end of this full move to the queue
756 this->append_milestone(gcode
, target
, rate_mm_s
);
758 // if adding these blocks didn't start executing, do that now
759 THEKERNEL
->conveyor
->ensure_running();
763 // Append an arc to the queue ( cutting it into segments as needed )
764 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
768 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
769 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
770 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
771 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
772 float r_axis1
= -offset
[this->plane_axis_1
];
773 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
774 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
776 // Patch from GRBL Firmware - Christoph Baumann 04072015
777 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
778 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
779 if (is_clockwise
) { // Correct atan2 output per direction
780 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2*M_PI
; }
782 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2*M_PI
; }
785 // Find the distance for this gcode
786 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
788 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
789 if( gcode
->millimeters_of_travel
< 0.00001F
) {
793 // Mark the gcode as having a known distance
794 this->distance_in_gcode_is_known( gcode
);
796 // Figure out how many segments for this gcode
797 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
799 float theta_per_segment
= angular_travel
/ segments
;
800 float linear_per_segment
= linear_travel
/ segments
;
802 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
803 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
804 r_T = [cos(phi) -sin(phi);
805 sin(phi) cos(phi] * r ;
806 For arc generation, the center of the circle is the axis of rotation and the radius vector is
807 defined from the circle center to the initial position. Each line segment is formed by successive
808 vector rotations. This requires only two cos() and sin() computations to form the rotation
809 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
810 all float numbers are single precision on the Arduino. (True float precision will not have
811 round off issues for CNC applications.) Single precision error can accumulate to be greater than
812 tool precision in some cases. Therefore, arc path correction is implemented.
814 Small angle approximation may be used to reduce computation overhead further. This approximation
815 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
816 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
817 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
818 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
819 issue for CNC machines with the single precision Arduino calculations.
820 This approximation also allows mc_arc to immediately insert a line segment into the planner
821 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
822 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
823 This is important when there are successive arc motions.
825 // Vector rotation matrix values
826 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
827 float sin_T
= theta_per_segment
;
836 // Initialize the linear axis
837 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
839 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
840 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
842 if (count
< this->arc_correction
) {
843 // Apply vector rotation matrix
844 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
845 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
849 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
850 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
851 cos_Ti
= cosf(i
* theta_per_segment
);
852 sin_Ti
= sinf(i
* theta_per_segment
);
853 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
854 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
858 // Update arc_target location
859 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
860 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
861 arc_target
[this->plane_axis_2
] += linear_per_segment
;
863 // Append this segment to the queue
864 this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
868 // Ensure last segment arrives at target location.
869 this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
872 // Do the math for an arc and add it to the queue
873 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
877 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
879 // Set clockwise/counter-clockwise sign for mc_arc computations
880 bool is_clockwise
= false;
881 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
886 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
891 float Robot::theta(float x
, float y
)
893 float t
= atanf(x
/ fabs(y
));
905 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
907 this->plane_axis_0
= axis_0
;
908 this->plane_axis_1
= axis_1
;
909 this->plane_axis_2
= axis_2
;
912 void Robot::clearToolOffset()
914 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
917 void Robot::setToolOffset(const float offset
[3])
919 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));