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 "RobotPublicAccess.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"
40 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
41 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
42 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
43 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
44 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
45 #define arc_correction_checksum CHECKSUM("arc_correction")
46 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
47 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
48 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
51 #define arm_solution_checksum CHECKSUM("arm_solution")
52 #define cartesian_checksum CHECKSUM("cartesian")
53 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
54 #define rostock_checksum CHECKSUM("rostock")
55 #define linear_delta_checksum CHECKSUM("linear_delta")
56 #define rotatable_delta_checksum CHECKSUM("rotatable_delta")
57 #define delta_checksum CHECKSUM("delta")
58 #define hbot_checksum CHECKSUM("hbot")
59 #define corexy_checksum CHECKSUM("corexy")
60 #define kossel_checksum CHECKSUM("kossel")
61 #define morgan_checksum CHECKSUM("morgan")
63 // stepper motor stuff
64 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
65 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
66 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
67 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
68 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
69 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
70 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
71 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
72 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
74 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
75 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
76 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
78 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
79 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
80 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
83 // new-style actuator stuff
84 #define actuator_checksum CHEKCSUM("actuator")
86 #define step_pin_checksum CHECKSUM("step_pin")
87 #define dir_pin_checksum CHEKCSUM("dir_pin")
88 #define en_pin_checksum CHECKSUM("en_pin")
90 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
91 #define max_rate_checksum CHECKSUM("max_rate")
93 #define alpha_checksum CHECKSUM("alpha")
94 #define beta_checksum CHECKSUM("beta")
95 #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;
139 //Called when the module has just been loaded
140 void Robot::on_module_loaded()
142 this->register_for_event(ON_GCODE_RECEIVED
);
143 this->register_for_event(ON_GET_PUBLIC_DATA
);
144 this->register_for_event(ON_SET_PUBLIC_DATA
);
145 this->register_for_event(ON_HALT
);
148 this->on_config_reload(this);
151 void Robot::on_config_reload(void *argument
)
154 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
155 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
156 // To make adding those solution easier, they have their own, separate object.
157 // Here we read the config to find out which arm solution to use
158 if (this->arm_solution
) delete this->arm_solution
;
159 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
160 // Note checksums are not const expressions when in debug mode, so don't use switch
161 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
162 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
164 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
165 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
167 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
168 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
170 } else if(solution_checksum
== rotatable_delta_checksum
) {
171 this->arm_solution
= new RotatableDeltaSolution(THEKERNEL
->config
);
174 } else if(solution_checksum
== morgan_checksum
) {
175 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
177 } else if(solution_checksum
== cartesian_checksum
) {
178 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
181 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
185 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
186 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
187 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
188 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
189 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
190 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
192 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
193 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
194 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
206 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
207 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
208 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
209 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
210 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
211 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
212 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
213 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
214 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
216 float steps_per_mm
[3] = {
217 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
218 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
219 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
222 // TODO: delete or detect old steppermotors
223 // Make our 3 StepperMotors
224 this->alpha_stepper_motor
= new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
);
225 this->beta_stepper_motor
= new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
);
226 this->gamma_stepper_motor
= new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
);
228 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
229 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
230 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
232 alpha_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
233 beta_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
234 gamma_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
235 check_max_actuator_speeds(); // check the configs are sane
238 actuators
.push_back(alpha_stepper_motor
);
239 actuators
.push_back(beta_stepper_motor
);
240 actuators
.push_back(gamma_stepper_motor
);
243 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
244 // so the first move can be correct if homing is not performed
245 float actuator_pos
[3];
246 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
247 for (int i
= 0; i
< 3; i
++)
248 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
250 //this->clearToolOffset();
253 // this does a sanity check that actuator speeds do not exceed steps rate capability
254 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
255 void Robot::check_max_actuator_speeds()
257 float step_freq
= alpha_stepper_motor
->get_max_rate() * alpha_stepper_motor
->get_steps_per_mm();
258 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
259 alpha_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm()));
260 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
);
263 step_freq
= beta_stepper_motor
->get_max_rate() * beta_stepper_motor
->get_steps_per_mm();
264 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
265 beta_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm()));
266 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
);
269 step_freq
= gamma_stepper_motor
->get_max_rate() * gamma_stepper_motor
->get_steps_per_mm();
270 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
271 gamma_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm()));
272 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
);
276 void Robot::on_halt(void *arg
)
278 halted
= (arg
== nullptr);
281 void Robot::on_get_public_data(void *argument
)
283 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
285 if(!pdr
->starts_with(robot_checksum
)) return;
287 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
288 static float return_data
;
289 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
290 pdr
->set_data_ptr(&return_data
);
293 } else if(pdr
->second_element_is(current_position_checksum
)) {
294 static float return_data
[3];
295 return_data
[0] = from_millimeters(this->last_milestone
[0]);
296 return_data
[1] = from_millimeters(this->last_milestone
[1]);
297 return_data
[2] = from_millimeters(this->last_milestone
[2]);
299 pdr
->set_data_ptr(&return_data
);
304 void Robot::on_set_public_data(void *argument
)
306 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
308 if(!pdr
->starts_with(robot_checksum
)) return;
310 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
311 // NOTE do not use this while printing!
312 float t
= *static_cast<float *>(pdr
->get_data_ptr());
313 // enforce minimum 10% speed
314 if (t
< 10.0F
) t
= 10.0F
;
316 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
318 } else if(pdr
->second_element_is(current_position_checksum
)) {
319 float *t
= static_cast<float *>(pdr
->get_data_ptr());
320 for (int i
= 0; i
< 3; i
++) {
321 this->last_milestone
[i
] = this->to_millimeters(t
[i
]);
324 float actuator_pos
[3];
325 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
326 for (int i
= 0; i
< 3; i
++)
327 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
333 //A GCode has been received
334 //See if the current Gcode line has some orders for us
335 void Robot::on_gcode_received(void *argument
)
337 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
339 this->motion_mode
= -1;
341 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
344 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
345 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
346 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
347 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
349 uint32_t delay_ms
= 0;
350 if (gcode
->has_letter('P')) {
351 delay_ms
= gcode
->get_int('P');
353 if (gcode
->has_letter('S')) {
354 delay_ms
+= gcode
->get_int('S') * 1000;
358 THEKERNEL
->conveyor
->wait_for_empty_queue();
359 // wait for specified time
360 uint32_t start
= us_ticker_read(); // mbed call
361 while ((us_ticker_read() - start
) < delay_ms
*1000) {
362 THEKERNEL
->call_event(ON_IDLE
, this);
365 gcode
->mark_as_taken();
368 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
369 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
370 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
371 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
372 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
373 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
374 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
376 if(gcode
->get_num_args() == 0) {
377 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
378 reset_axis_position(0, i
);
382 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
383 if ( gcode
->has_letter(letter
) ) {
384 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
389 gcode
->mark_as_taken();
393 } else if( gcode
->has_m
) {
395 case 92: // M92 - set steps per mm
396 if (gcode
->has_letter('X'))
397 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
398 if (gcode
->has_letter('Y'))
399 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
400 if (gcode
->has_letter('Z'))
401 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
402 if (gcode
->has_letter('F'))
403 seconds_per_minute
= gcode
->get_value('F');
405 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
);
406 gcode
->add_nl
= true;
407 gcode
->mark_as_taken();
408 check_max_actuator_speeds();
413 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 ",
414 from_millimeters(this->last_milestone
[0]),
415 from_millimeters(this->last_milestone
[1]),
416 from_millimeters(this->last_milestone
[2]),
417 actuators
[X_AXIS
]->get_current_position(),
418 actuators
[Y_AXIS
]->get_current_position(),
419 actuators
[Z_AXIS
]->get_current_position() );
420 gcode
->txt_after_ok
.append(buf
, n
);
421 gcode
->mark_as_taken();
425 case 120: { // push state
426 gcode
->mark_as_taken();
427 bool b
= this->absolute_mode
;
428 saved_state_t
s(this->feed_rate
, this->seek_rate
, b
);
433 case 121: // pop state
434 gcode
->mark_as_taken();
435 if(!state_stack
.empty()) {
436 auto s
= state_stack
.top();
438 this->feed_rate
= std::get
<0>(s
);
439 this->seek_rate
= std::get
<1>(s
);
440 this->absolute_mode
= std::get
<2>(s
);
444 case 203: // M203 Set maximum feedrates in mm/sec
445 if (gcode
->has_letter('X'))
446 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
447 if (gcode
->has_letter('Y'))
448 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
449 if (gcode
->has_letter('Z'))
450 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
451 if (gcode
->has_letter('A'))
452 alpha_stepper_motor
->set_max_rate(gcode
->get_value('A'));
453 if (gcode
->has_letter('B'))
454 beta_stepper_motor
->set_max_rate(gcode
->get_value('B'));
455 if (gcode
->has_letter('C'))
456 gamma_stepper_motor
->set_max_rate(gcode
->get_value('C'));
458 check_max_actuator_speeds();
460 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
461 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
462 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
463 gcode
->add_nl
= true;
464 gcode
->mark_as_taken();
467 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
468 gcode
->mark_as_taken();
470 if (gcode
->has_letter('S')) {
471 float acc
= gcode
->get_value('S'); // mm/s^2
475 THEKERNEL
->planner
->acceleration
= acc
;
477 if (gcode
->has_letter('Z')) {
478 float acc
= gcode
->get_value('Z'); // mm/s^2
482 THEKERNEL
->planner
->z_acceleration
= acc
;
486 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
487 gcode
->mark_as_taken();
488 if (gcode
->has_letter('X')) {
489 float jd
= gcode
->get_value('X');
493 THEKERNEL
->planner
->junction_deviation
= jd
;
495 if (gcode
->has_letter('Z')) {
496 float jd
= gcode
->get_value('Z');
497 // enforce minimum, -1 disables it and uses regular junction deviation
500 THEKERNEL
->planner
->z_junction_deviation
= jd
;
502 if (gcode
->has_letter('S')) {
503 float mps
= gcode
->get_value('S');
507 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
509 if (gcode
->has_letter('Y')) {
510 alpha_stepper_motor
->default_minimum_actuator_rate
= gcode
->get_value('Y');
514 case 220: // M220 - speed override percentage
515 gcode
->mark_as_taken();
516 if (gcode
->has_letter('S')) {
517 float factor
= gcode
->get_value('S');
518 // enforce minimum 10% speed
521 // enforce maximum 10x speed
522 if (factor
> 1000.0F
)
525 seconds_per_minute
= 6000.0F
/ factor
;
527 gcode
->stream
->printf("Speed factor at %f %%\n", 6000.0F
/ seconds_per_minute
);
531 case 400: // wait until all moves are done up to this point
532 gcode
->mark_as_taken();
533 THEKERNEL
->conveyor
->wait_for_empty_queue();
536 case 500: // M500 saves some volatile settings to config override file
537 case 503: { // M503 just prints the settings
538 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
);
539 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
540 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
);
541 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",
542 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
543 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
545 // get or save any arm solution specific optional values
546 BaseSolution::arm_options_t options
;
547 if(arm_solution
->get_optional(options
) && !options
.empty()) {
548 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
549 for(auto &i
: options
) {
550 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
552 gcode
->stream
->printf("\n");
554 gcode
->mark_as_taken();
558 case 665: { // M665 set optional arm solution variables based on arm solution.
559 gcode
->mark_as_taken();
560 // the parameter args could be any letter each arm solution only accepts certain ones
561 BaseSolution::arm_options_t options
= gcode
->get_args();
562 options
.erase('S'); // don't include the S
563 options
.erase('U'); // don't include the U
564 if(options
.size() > 0) {
565 // set the specified options
566 arm_solution
->set_optional(options
);
569 if(arm_solution
->get_optional(options
)) {
570 // foreach optional value
571 for(auto &i
: options
) {
572 // print all current values of supported options
573 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
574 gcode
->add_nl
= true;
578 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
579 this->delta_segments_per_second
= gcode
->get_value('S');
580 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
582 }else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
583 this->mm_per_line_segment
= gcode
->get_value('U');
584 this->delta_segments_per_second
= 0;
585 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
593 if( this->motion_mode
< 0)
597 float target
[3], offset
[3];
598 clear_vector(offset
);
600 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
602 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
603 if( gcode
->has_letter(letter
) ) {
604 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
607 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
608 if( gcode
->has_letter(letter
) ) {
609 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
613 if( gcode
->has_letter('F') ) {
614 if( this->motion_mode
== MOTION_MODE_SEEK
)
615 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
617 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
620 //Perform any physical actions
621 switch(this->motion_mode
) {
622 case MOTION_MODE_CANCEL
: break;
623 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
624 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
625 case MOTION_MODE_CW_ARC
:
626 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
629 // last_milestone was set to target in append_milestone, no need to do it again
633 // We received a new gcode, and one of the functions
634 // determined the distance for that given gcode. So now we can attach this gcode to the right block
636 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
638 //If the queue is empty, execute immediatly, otherwise attach to the last added block
639 THEKERNEL
->conveyor
->append_gcode(gcode
);
642 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
643 void Robot::reset_axis_position(float x
, float y
, float z
)
645 this->last_milestone
[X_AXIS
] = x
;
646 this->last_milestone
[Y_AXIS
] = y
;
647 this->last_milestone
[Z_AXIS
] = z
;
648 this->transformed_last_milestone
[X_AXIS
] = x
;
649 this->transformed_last_milestone
[Y_AXIS
] = y
;
650 this->transformed_last_milestone
[Z_AXIS
] = z
;
652 float actuator_pos
[3];
653 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
654 for (int i
= 0; i
< 3; i
++)
655 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
658 // Reset the position for an axis (used in homing and G92)
659 void Robot::reset_axis_position(float position
, int axis
)
661 this->last_milestone
[axis
] = position
;
662 this->transformed_last_milestone
[axis
] = position
;
664 float actuator_pos
[3];
665 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
667 for (int i
= 0; i
< 3; i
++)
668 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
671 // Use FK to find out where actuator is and reset lastmilestone to match
672 void Robot::reset_position_from_current_actuator_position()
674 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
675 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
676 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
678 // now reset actuator correctly, NOTE this may lose a little precision
679 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
680 for (int i
= 0; i
< 3; i
++)
681 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
684 // Convert target from millimeters to steps, and append this to the planner
685 void Robot::append_milestone( float target
[], float rate_mm_s
)
689 float actuator_pos
[3];
690 float transformed_target
[3]; // adjust target for bed compensation
691 float millimeters_of_travel
;
693 // unity transform by default
694 memcpy(transformed_target
, target
, sizeof(transformed_target
));
696 // check function pointer and call if set to transform the target to compensate for bed
697 if(compensationTransform
) {
698 // some compensation strategies can transform XYZ, some just change Z
699 compensationTransform(transformed_target
);
702 // find distance moved by each axis, use transformed target from last_transformed_target
703 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
704 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
706 // store last transformed
707 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
709 // Compute how long this move moves, so we can attach it to the block for later use
710 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
712 // find distance unit vector
713 for (int i
= 0; i
< 3; i
++)
714 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
716 // Do not move faster than the configured cartesian limits
717 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
718 if ( max_speeds
[axis
] > 0 ) {
719 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
721 if (axis_speed
> max_speeds
[axis
])
722 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
726 // find actuator position given cartesian position, use actual adjusted target
727 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
729 // check per-actuator speed limits
730 for (int actuator
= 0; actuator
<= 2; actuator
++) {
731 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
733 if (actuator_rate
> actuators
[actuator
]->get_max_rate())
734 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
737 // Append the block to the planner
738 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
740 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
741 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
745 // Append a move to the queue ( cutting it into segments if needed )
746 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
748 // Find out the distance for this gcode
749 // 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
750 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 ));
752 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
753 if( gcode
->millimeters_of_travel
< 0.00001F
) {
757 // Mark the gcode as having a known distance
758 this->distance_in_gcode_is_known( gcode
);
760 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
761 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
762 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
763 // 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
766 if(this->delta_segments_per_second
> 1.0F
) {
767 // enabled if set to something > 1, it is set to 0.0 by default
768 // segment based on current speed and requested segments per second
769 // the faster the travel speed the fewer segments needed
770 // NOTE rate is mm/sec and we take into account any speed override
771 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
772 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
773 // TODO if we are only moving in Z on a delta we don't really need to segment at all
776 if(this->mm_per_line_segment
== 0.0F
) {
777 segments
= 1; // don't split it up
779 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
784 // A vector to keep track of the endpoint of each segment
785 float segment_delta
[3];
786 float segment_end
[3];
788 // How far do we move each segment?
789 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
790 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
792 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
793 // We always add another point after this loop so we stop at segments-1, ie i < segments
794 for (int i
= 1; i
< segments
; i
++) {
795 if(halted
) return; // don't queue any more segments
796 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
797 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
799 // Append the end of this segment to the queue
800 this->append_milestone(segment_end
, rate_mm_s
);
804 // Append the end of this full move to the queue
805 this->append_milestone(target
, rate_mm_s
);
807 // if adding these blocks didn't start executing, do that now
808 THEKERNEL
->conveyor
->ensure_running();
812 // Append an arc to the queue ( cutting it into segments as needed )
813 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
817 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
818 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
819 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
820 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
821 float r_axis1
= -offset
[this->plane_axis_1
];
822 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
823 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
825 // Patch from GRBL Firmware - Christoph Baumann 04072015
826 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
827 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
828 if (is_clockwise
) { // Correct atan2 output per direction
829 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2*M_PI
; }
831 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2*M_PI
; }
834 // Find the distance for this gcode
835 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
837 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
838 if( gcode
->millimeters_of_travel
< 0.00001F
) {
842 // Mark the gcode as having a known distance
843 this->distance_in_gcode_is_known( gcode
);
845 // Figure out how many segments for this gcode
846 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
848 float theta_per_segment
= angular_travel
/ segments
;
849 float linear_per_segment
= linear_travel
/ segments
;
851 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
852 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
853 r_T = [cos(phi) -sin(phi);
854 sin(phi) cos(phi] * r ;
855 For arc generation, the center of the circle is the axis of rotation and the radius vector is
856 defined from the circle center to the initial position. Each line segment is formed by successive
857 vector rotations. This requires only two cos() and sin() computations to form the rotation
858 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
859 all float numbers are single precision on the Arduino. (True float precision will not have
860 round off issues for CNC applications.) Single precision error can accumulate to be greater than
861 tool precision in some cases. Therefore, arc path correction is implemented.
863 Small angle approximation may be used to reduce computation overhead further. This approximation
864 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
865 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
866 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
867 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
868 issue for CNC machines with the single precision Arduino calculations.
869 This approximation also allows mc_arc to immediately insert a line segment into the planner
870 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
871 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
872 This is important when there are successive arc motions.
874 // Vector rotation matrix values
875 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
876 float sin_T
= theta_per_segment
;
885 // Initialize the linear axis
886 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
888 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
889 if(halted
) return; // don't queue any more segments
891 if (count
< this->arc_correction
) {
892 // Apply vector rotation matrix
893 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
894 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
898 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
899 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
900 cos_Ti
= cosf(i
* theta_per_segment
);
901 sin_Ti
= sinf(i
* theta_per_segment
);
902 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
903 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
907 // Update arc_target location
908 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
909 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
910 arc_target
[this->plane_axis_2
] += linear_per_segment
;
912 // Append this segment to the queue
913 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
917 // Ensure last segment arrives at target location.
918 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
921 // Do the math for an arc and add it to the queue
922 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
926 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
928 // Set clockwise/counter-clockwise sign for mc_arc computations
929 bool is_clockwise
= false;
930 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
935 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
940 float Robot::theta(float x
, float y
)
942 float t
= atanf(x
/ fabs(y
));
954 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
956 this->plane_axis_0
= axis_0
;
957 this->plane_axis_1
= axis_1
;
958 this->plane_axis_2
= axis_2
;
961 void Robot::clearToolOffset()
963 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
966 void Robot::setToolOffset(const float offset
[3])
968 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));