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"
18 #include "nuts_bolts.h"
20 #include "StepperMotor.h"
22 #include "PublicDataRequest.h"
23 #include "RobotPublicAccess.h"
24 #include "arm_solutions/BaseSolution.h"
25 #include "arm_solutions/CartesianSolution.h"
26 #include "arm_solutions/RotatableCartesianSolution.h"
27 #include "arm_solutions/LinearDeltaSolution.h"
28 #include "arm_solutions/HBotSolution.h"
29 #include "arm_solutions/MorganSCARASolution.h"
30 #include "StepTicker.h"
31 #include "checksumm.h"
33 #include "ConfigValue.h"
34 #include "libs/StreamOutput.h"
35 #include "StreamOutputPool.h"
37 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
38 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
39 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
40 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
41 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
42 #define arc_correction_checksum CHECKSUM("arc_correction")
43 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
44 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
45 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
48 #define arm_solution_checksum CHECKSUM("arm_solution")
49 #define cartesian_checksum CHECKSUM("cartesian")
50 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
51 #define rostock_checksum CHECKSUM("rostock")
52 #define linear_delta_checksum CHECKSUM("linear_delta")
53 #define delta_checksum CHECKSUM("delta")
54 #define hbot_checksum CHECKSUM("hbot")
55 #define corexy_checksum CHECKSUM("corexy")
56 #define kossel_checksum CHECKSUM("kossel")
57 #define morgan_checksum CHECKSUM("morgan")
59 // stepper motor stuff
60 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
61 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
62 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
63 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
64 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
65 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
66 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
67 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
68 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
70 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
71 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
72 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
74 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
75 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
76 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
79 // new-style actuator stuff
80 #define actuator_checksum CHEKCSUM("actuator")
82 #define step_pin_checksum CHECKSUM("step_pin")
83 #define dir_pin_checksum CHEKCSUM("dir_pin")
84 #define en_pin_checksum CHECKSUM("en_pin")
86 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
87 #define max_rate_checksum CHECKSUM("max_rate")
89 #define alpha_checksum CHECKSUM("alpha")
90 #define beta_checksum CHECKSUM("beta")
91 #define gamma_checksum CHECKSUM("gamma")
94 #define NEXT_ACTION_DEFAULT 0
95 #define NEXT_ACTION_DWELL 1
96 #define NEXT_ACTION_GO_HOME 2
98 #define MOTION_MODE_SEEK 0 // G0
99 #define MOTION_MODE_LINEAR 1 // G1
100 #define MOTION_MODE_CW_ARC 2 // G2
101 #define MOTION_MODE_CCW_ARC 3 // G3
102 #define MOTION_MODE_CANCEL 4 // G80
104 #define PATH_CONTROL_MODE_EXACT_PATH 0
105 #define PATH_CONTROL_MODE_EXACT_STOP 1
106 #define PATH_CONTROL_MODE_CONTINOUS 2
108 #define PROGRAM_FLOW_RUNNING 0
109 #define PROGRAM_FLOW_PAUSED 1
110 #define PROGRAM_FLOW_COMPLETED 2
112 #define SPINDLE_DIRECTION_CW 0
113 #define SPINDLE_DIRECTION_CCW 1
115 // 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
116 // It takes care of cutting arcs into segments, same thing for line that are too long
120 this->inch_mode
= false;
121 this->absolute_mode
= true;
122 this->motion_mode
= MOTION_MODE_SEEK
;
123 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
124 clear_vector(this->last_milestone
);
125 clear_vector(this->transformed_last_milestone
);
126 this->arm_solution
= NULL
;
127 seconds_per_minute
= 60.0F
;
128 this->clearToolOffset();
129 this->compensationTransform
= nullptr;
133 //Called when the module has just been loaded
134 void Robot::on_module_loaded()
136 this->register_for_event(ON_GCODE_RECEIVED
);
137 this->register_for_event(ON_GET_PUBLIC_DATA
);
138 this->register_for_event(ON_SET_PUBLIC_DATA
);
139 this->register_for_event(ON_HALT
);
142 this->on_config_reload(this);
145 void Robot::on_config_reload(void *argument
)
148 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
149 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
150 // To make adding those solution easier, they have their own, separate object.
151 // Here we read the config to find out which arm solution to use
152 if (this->arm_solution
) delete this->arm_solution
;
153 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
154 // Note checksums are not const expressions when in debug mode, so don't use switch
155 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
156 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
158 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
159 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
161 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
162 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
164 } else if(solution_checksum
== morgan_checksum
) {
165 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
167 } else if(solution_checksum
== cartesian_checksum
) {
168 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
171 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
175 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
176 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
177 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
178 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
179 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
180 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
182 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
183 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
184 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
196 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
197 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
198 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
199 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
200 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
201 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
202 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
203 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
204 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
206 float steps_per_mm
[3] = {
207 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
208 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
209 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
212 // TODO: delete or detect old steppermotors
213 // Make our 3 StepperMotors
214 this->alpha_stepper_motor
= new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
);
215 this->beta_stepper_motor
= new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
);
216 this->gamma_stepper_motor
= new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
);
218 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
219 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
220 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
222 alpha_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
223 beta_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
224 gamma_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
225 check_max_actuator_speeds(); // check the configs are sane
228 actuators
.push_back(alpha_stepper_motor
);
229 actuators
.push_back(beta_stepper_motor
);
230 actuators
.push_back(gamma_stepper_motor
);
233 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
234 // so the first move can be correct if homing is not performed
235 float actuator_pos
[3];
236 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
237 for (int i
= 0; i
< 3; i
++)
238 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
240 //this->clearToolOffset();
243 // this does a sanity check that actuator speeds do not exceed steps rate capability
244 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
245 void Robot::check_max_actuator_speeds()
247 float step_freq
= alpha_stepper_motor
->get_max_rate() * alpha_stepper_motor
->get_steps_per_mm();
248 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
249 alpha_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm()));
250 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
);
253 step_freq
= beta_stepper_motor
->get_max_rate() * beta_stepper_motor
->get_steps_per_mm();
254 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
255 beta_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm()));
256 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
);
259 step_freq
= gamma_stepper_motor
->get_max_rate() * gamma_stepper_motor
->get_steps_per_mm();
260 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
261 gamma_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm()));
262 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
);
266 void Robot::on_halt(void *arg
)
268 halted
= (arg
== nullptr);
271 void Robot::on_get_public_data(void *argument
)
273 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
275 if(!pdr
->starts_with(robot_checksum
)) return;
277 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
278 static float return_data
;
279 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
280 pdr
->set_data_ptr(&return_data
);
283 } else if(pdr
->second_element_is(current_position_checksum
)) {
284 static float return_data
[3];
285 return_data
[0] = from_millimeters(this->last_milestone
[0]);
286 return_data
[1] = from_millimeters(this->last_milestone
[1]);
287 return_data
[2] = from_millimeters(this->last_milestone
[2]);
289 pdr
->set_data_ptr(&return_data
);
294 void Robot::on_set_public_data(void *argument
)
296 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
298 if(!pdr
->starts_with(robot_checksum
)) return;
300 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
301 // NOTE do not use this while printing!
302 float t
= *static_cast<float *>(pdr
->get_data_ptr());
303 // enforce minimum 10% speed
304 if (t
< 10.0F
) t
= 10.0F
;
306 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
308 } else if(pdr
->second_element_is(current_position_checksum
)) {
309 float *t
= static_cast<float *>(pdr
->get_data_ptr());
310 for (int i
= 0; i
< 3; i
++) {
311 this->last_milestone
[i
] = this->to_millimeters(t
[i
]);
314 float actuator_pos
[3];
315 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
316 for (int i
= 0; i
< 3; i
++)
317 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
323 //A GCode has been received
324 //See if the current Gcode line has some orders for us
325 void Robot::on_gcode_received(void *argument
)
327 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
329 this->motion_mode
= -1;
331 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
334 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
335 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
336 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
337 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
338 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
339 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
340 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
341 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
342 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
343 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
344 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
346 if(gcode
->get_num_args() == 0) {
347 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
348 reset_axis_position(0, i
);
352 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
353 if ( gcode
->has_letter(letter
) ) {
354 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
359 gcode
->mark_as_taken();
363 } else if( gcode
->has_m
) {
365 case 92: // M92 - set steps per mm
366 if (gcode
->has_letter('X'))
367 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
368 if (gcode
->has_letter('Y'))
369 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
370 if (gcode
->has_letter('Z'))
371 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
372 if (gcode
->has_letter('F'))
373 seconds_per_minute
= gcode
->get_value('F');
375 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
);
376 gcode
->add_nl
= true;
377 gcode
->mark_as_taken();
378 check_max_actuator_speeds();
382 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 ",
383 from_millimeters(this->last_milestone
[0]),
384 from_millimeters(this->last_milestone
[1]),
385 from_millimeters(this->last_milestone
[2]),
386 actuators
[X_AXIS
]->get_current_position(),
387 actuators
[Y_AXIS
]->get_current_position(),
388 actuators
[Z_AXIS
]->get_current_position() );
389 gcode
->txt_after_ok
.append(buf
, n
);
390 gcode
->mark_as_taken();
394 case 203: // M203 Set maximum feedrates in mm/sec
395 if (gcode
->has_letter('X'))
396 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
397 if (gcode
->has_letter('Y'))
398 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
399 if (gcode
->has_letter('Z'))
400 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
401 if (gcode
->has_letter('A'))
402 alpha_stepper_motor
->set_max_rate(gcode
->get_value('A'));
403 if (gcode
->has_letter('B'))
404 beta_stepper_motor
->set_max_rate(gcode
->get_value('B'));
405 if (gcode
->has_letter('C'))
406 gamma_stepper_motor
->set_max_rate(gcode
->get_value('C'));
408 check_max_actuator_speeds();
410 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
411 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
412 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
413 gcode
->add_nl
= true;
414 gcode
->mark_as_taken();
417 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
418 gcode
->mark_as_taken();
420 if (gcode
->has_letter('S')) {
421 float acc
= gcode
->get_value('S'); // mm/s^2
425 THEKERNEL
->planner
->acceleration
= acc
;
427 if (gcode
->has_letter('Z')) {
428 float acc
= gcode
->get_value('Z'); // mm/s^2
432 THEKERNEL
->planner
->z_acceleration
= acc
;
436 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
437 gcode
->mark_as_taken();
438 if (gcode
->has_letter('X')) {
439 float jd
= gcode
->get_value('X');
443 THEKERNEL
->planner
->junction_deviation
= jd
;
445 if (gcode
->has_letter('Z')) {
446 float jd
= gcode
->get_value('Z');
447 // enforce minimum, -1 disables it and uses regular junction deviation
450 THEKERNEL
->planner
->z_junction_deviation
= jd
;
452 if (gcode
->has_letter('S')) {
453 float mps
= gcode
->get_value('S');
457 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
459 if (gcode
->has_letter('Y')) {
460 alpha_stepper_motor
->default_minimum_actuator_rate
= gcode
->get_value('Y');
464 case 220: // M220 - speed override percentage
465 gcode
->mark_as_taken();
466 if (gcode
->has_letter('S')) {
467 float factor
= gcode
->get_value('S');
468 // enforce minimum 10% speed
471 // enforce maximum 10x speed
472 if (factor
> 1000.0F
)
475 seconds_per_minute
= 6000.0F
/ factor
;
479 case 400: // wait until all moves are done up to this point
480 gcode
->mark_as_taken();
481 THEKERNEL
->conveyor
->wait_for_empty_queue();
484 case 500: // M500 saves some volatile settings to config override file
485 case 503: { // M503 just prints the settings
486 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
);
487 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
488 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
);
489 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",
490 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
491 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
493 // get or save any arm solution specific optional values
494 BaseSolution::arm_options_t options
;
495 if(arm_solution
->get_optional(options
) && !options
.empty()) {
496 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
497 for(auto &i
: options
) {
498 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
500 gcode
->stream
->printf("\n");
502 gcode
->mark_as_taken();
506 case 665: { // M665 set optional arm solution variables based on arm solution.
507 gcode
->mark_as_taken();
508 // the parameter args could be any letter except S so ask solution what options it supports
509 BaseSolution::arm_options_t options
;
510 if(arm_solution
->get_optional(options
)) {
511 for(auto &i
: options
) {
512 // foreach optional value
514 if(gcode
->has_letter(c
)) { // set new value
515 i
.second
= gcode
->get_value(c
);
517 // print all current values of supported options
518 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
519 gcode
->add_nl
= true;
521 // set the new options
522 arm_solution
->set_optional(options
);
525 // set delta segments per second, not saved by M500
526 if(gcode
->has_letter('S')) {
527 this->delta_segments_per_second
= gcode
->get_value('S');
534 if( this->motion_mode
< 0)
538 float target
[3], offset
[3];
539 clear_vector(offset
);
541 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
543 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
544 if( gcode
->has_letter(letter
) ) {
545 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
548 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
549 if( gcode
->has_letter(letter
) ) {
550 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
554 if( gcode
->has_letter('F') ) {
555 if( this->motion_mode
== MOTION_MODE_SEEK
)
556 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
558 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
561 //Perform any physical actions
562 switch(this->motion_mode
) {
563 case MOTION_MODE_CANCEL
: break;
564 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
565 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
566 case MOTION_MODE_CW_ARC
:
567 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
570 // last_milestone was set to target in append_milestone, no need to do it again
574 // We received a new gcode, and one of the functions
575 // determined the distance for that given gcode. So now we can attach this gcode to the right block
577 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
579 //If the queue is empty, execute immediatly, otherwise attach to the last added block
580 THEKERNEL
->conveyor
->append_gcode(gcode
);
583 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
584 void Robot::reset_axis_position(float x
, float y
, float z
)
586 this->last_milestone
[X_AXIS
] = x
;
587 this->last_milestone
[Y_AXIS
] = y
;
588 this->last_milestone
[Z_AXIS
] = z
;
589 this->transformed_last_milestone
[X_AXIS
] = x
;
590 this->transformed_last_milestone
[Y_AXIS
] = y
;
591 this->transformed_last_milestone
[Z_AXIS
] = z
;
593 float actuator_pos
[3];
594 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 // Reset the position for an axis (used in homing and G92)
600 void Robot::reset_axis_position(float position
, int axis
)
602 this->last_milestone
[axis
] = position
;
603 this->transformed_last_milestone
[axis
] = position
;
605 float actuator_pos
[3];
606 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 // Use FK to find out where actuator is and reset lastmilestone to match
613 void Robot::reset_position_from_current_actuator_position()
615 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
616 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
617 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
620 // Convert target from millimeters to steps, and append this to the planner
621 void Robot::append_milestone( float target
[], float rate_mm_s
)
625 float actuator_pos
[3];
626 float transformed_target
[3]; // adjust target for bed compensation
627 float millimeters_of_travel
;
629 // unity transform by default
630 memcpy(transformed_target
, target
, sizeof(transformed_target
));
632 // check function pointer and call if set to transform the target to compensate for bed
633 if(compensationTransform
) {
634 // some compensation strategies can transform XYZ, some just change Z
635 compensationTransform(transformed_target
);
638 // find distance moved by each axis, use transformed target from last_transformed_target
639 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
640 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
642 // store last transformed
643 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
645 // Compute how long this move moves, so we can attach it to the block for later use
646 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
648 // find distance unit vector
649 for (int i
= 0; i
< 3; i
++)
650 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
652 // Do not move faster than the configured cartesian limits
653 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
654 if ( max_speeds
[axis
] > 0 ) {
655 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
657 if (axis_speed
> max_speeds
[axis
])
658 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
662 // find actuator position given cartesian position, use actual adjusted target
663 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
665 // check per-actuator speed limits
666 for (int actuator
= 0; actuator
<= 2; actuator
++) {
667 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
669 if (actuator_rate
> actuators
[actuator
]->get_max_rate())
670 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
673 // Append the block to the planner
674 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
676 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
677 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
681 // Append a move to the queue ( cutting it into segments if needed )
682 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
685 // Find out the distance for this gcode
686 gcode
->millimeters_of_travel
= 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 );
688 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
689 if( gcode
->millimeters_of_travel
< 1e-8F
) {
693 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
695 // Mark the gcode as having a known distance
696 this->distance_in_gcode_is_known( gcode
);
698 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
699 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
700 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second 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
703 if(this->delta_segments_per_second
> 1.0F
) {
704 // enabled if set to something > 1, it is set to 0.0 by default
705 // segment based on current speed and requested segments per second
706 // the faster the travel speed the fewer segments needed
707 // NOTE rate is mm/sec and we take into account any speed override
708 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
709 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
710 // TODO if we are only moving in Z on a delta we don't really need to segment at all
713 if(this->mm_per_line_segment
== 0.0F
) {
714 segments
= 1; // don't split it up
716 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
721 // A vector to keep track of the endpoint of each segment
722 float segment_delta
[3];
723 float segment_end
[3];
725 // How far do we move each segment?
726 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
727 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
729 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
730 // We always add another point after this loop so we stop at segments-1, ie i < segments
731 for (int i
= 1; i
< segments
; i
++) {
732 if(halted
) return; // don;t queue any more segments
733 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
734 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
736 // Append the end of this segment to the queue
737 this->append_milestone(segment_end
, rate_mm_s
);
741 // Append the end of this full move to the queue
742 this->append_milestone(target
, rate_mm_s
);
744 // if adding these blocks didn't start executing, do that now
745 THEKERNEL
->conveyor
->ensure_running();
749 // Append an arc to the queue ( cutting it into segments as needed )
750 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
754 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
755 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
756 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
757 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
758 float r_axis1
= -offset
[this->plane_axis_1
];
759 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
760 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
762 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
763 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
764 if (angular_travel
< 0) {
765 angular_travel
+= 2 * M_PI
;
768 angular_travel
-= 2 * M_PI
;
771 // Find the distance for this gcode
772 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
774 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
775 if( gcode
->millimeters_of_travel
< 0.0001F
) {
779 // Mark the gcode as having a known distance
780 this->distance_in_gcode_is_known( gcode
);
782 // Figure out how many segments for this gcode
783 uint16_t segments
= floor(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
785 float theta_per_segment
= angular_travel
/ segments
;
786 float linear_per_segment
= linear_travel
/ segments
;
788 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
789 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
790 r_T = [cos(phi) -sin(phi);
791 sin(phi) cos(phi] * r ;
792 For arc generation, the center of the circle is the axis of rotation and the radius vector is
793 defined from the circle center to the initial position. Each line segment is formed by successive
794 vector rotations. This requires only two cos() and sin() computations to form the rotation
795 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
796 all float numbers are single precision on the Arduino. (True float precision will not have
797 round off issues for CNC applications.) Single precision error can accumulate to be greater than
798 tool precision in some cases. Therefore, arc path correction is implemented.
800 Small angle approximation may be used to reduce computation overhead further. This approximation
801 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
802 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
803 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
804 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
805 issue for CNC machines with the single precision Arduino calculations.
806 This approximation also allows mc_arc to immediately insert a line segment into the planner
807 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
808 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
809 This is important when there are successive arc motions.
811 // Vector rotation matrix values
812 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
813 float sin_T
= theta_per_segment
;
822 // Initialize the linear axis
823 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
825 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
826 if(halted
) return; // don't queue any more segments
828 if (count
< this->arc_correction
) {
829 // Apply vector rotation matrix
830 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
831 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
835 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
836 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
837 cos_Ti
= cosf(i
* theta_per_segment
);
838 sin_Ti
= sinf(i
* theta_per_segment
);
839 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
840 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
844 // Update arc_target location
845 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
846 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
847 arc_target
[this->plane_axis_2
] += linear_per_segment
;
849 // Append this segment to the queue
850 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
854 // Ensure last segment arrives at target location.
855 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
858 // Do the math for an arc and add it to the queue
859 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
863 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
865 // Set clockwise/counter-clockwise sign for mc_arc computations
866 bool is_clockwise
= false;
867 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
872 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
877 float Robot::theta(float x
, float y
)
879 float t
= atanf(x
/ fabs(y
));
891 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
893 this->plane_axis_0
= axis_0
;
894 this->plane_axis_1
= axis_1
;
895 this->plane_axis_2
= axis_2
;
898 void Robot::clearToolOffset()
900 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
903 void Robot::setToolOffset(const float offset
[3])
905 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));