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
117 #define max(a,b) (((a) > (b)) ? (a) : (b))
121 this->inch_mode
= false;
122 this->absolute_mode
= true;
123 this->motion_mode
= MOTION_MODE_SEEK
;
124 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
125 clear_vector(this->last_milestone
);
126 clear_vector(this->transformed_last_milestone
);
127 this->arm_solution
= NULL
;
128 seconds_per_minute
= 60.0F
;
129 this->clearToolOffset();
130 this->compensationTransform
= nullptr;
134 //Called when the module has just been loaded
135 void Robot::on_module_loaded()
137 this->register_for_event(ON_GCODE_RECEIVED
);
138 this->register_for_event(ON_GET_PUBLIC_DATA
);
139 this->register_for_event(ON_SET_PUBLIC_DATA
);
140 this->register_for_event(ON_HALT
);
143 this->on_config_reload(this);
146 void Robot::on_config_reload(void *argument
)
149 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
150 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
151 // To make adding those solution easier, they have their own, separate object.
152 // Here we read the config to find out which arm solution to use
153 if (this->arm_solution
) delete this->arm_solution
;
154 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
155 // Note checksums are not const expressions when in debug mode, so don't use switch
156 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
157 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
160 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
163 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== morgan_checksum
) {
166 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== cartesian_checksum
) {
169 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
172 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
176 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
177 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
178 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
179 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
180 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
181 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
183 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
184 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
185 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
197 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
198 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
199 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
200 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
201 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
202 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
203 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
204 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
205 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
207 float steps_per_mm
[3] = {
208 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
209 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
210 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
213 // TODO: delete or detect old steppermotors
214 // Make our 3 StepperMotors
215 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
216 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
217 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
219 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
220 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
221 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
223 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
224 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
225 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
226 check_max_actuator_speeds(); // check the configs are sane
229 actuators
.push_back(alpha_stepper_motor
);
230 actuators
.push_back(beta_stepper_motor
);
231 actuators
.push_back(gamma_stepper_motor
);
234 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
235 // so the first move can be correct if homing is not performed
236 float actuator_pos
[3];
237 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
238 for (int i
= 0; i
< 3; i
++)
239 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
241 //this->clearToolOffset();
244 // this does a sanity check that actuator speeds do not exceed steps rate capability
245 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
246 void Robot::check_max_actuator_speeds()
248 float step_freq
= alpha_stepper_motor
->max_rate
* alpha_stepper_motor
->get_steps_per_mm();
249 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
250 alpha_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm());
251 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
);
254 step_freq
= beta_stepper_motor
->max_rate
* beta_stepper_motor
->get_steps_per_mm();
255 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
256 beta_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm());
257 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
);
260 step_freq
= gamma_stepper_motor
->max_rate
* gamma_stepper_motor
->get_steps_per_mm();
261 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
262 gamma_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm());
263 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
);
267 void Robot::on_halt(void *arg
)
269 halted
= (arg
== nullptr);
272 void Robot::on_get_public_data(void *argument
)
274 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
276 if(!pdr
->starts_with(robot_checksum
)) return;
278 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
279 static float return_data
;
280 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
281 pdr
->set_data_ptr(&return_data
);
284 } else if(pdr
->second_element_is(current_position_checksum
)) {
285 static float return_data
[3];
286 return_data
[0] = from_millimeters(this->last_milestone
[0]);
287 return_data
[1] = from_millimeters(this->last_milestone
[1]);
288 return_data
[2] = from_millimeters(this->last_milestone
[2]);
290 pdr
->set_data_ptr(&return_data
);
295 void Robot::on_set_public_data(void *argument
)
297 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
299 if(!pdr
->starts_with(robot_checksum
)) return;
301 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
302 // NOTE do not use this while printing!
303 float t
= *static_cast<float *>(pdr
->get_data_ptr());
304 // enforce minimum 10% speed
305 if (t
< 10.0F
) t
= 10.0F
;
307 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
309 } else if(pdr
->second_element_is(current_position_checksum
)) {
310 float *t
= static_cast<float *>(pdr
->get_data_ptr());
311 for (int i
= 0; i
< 3; i
++) {
312 this->last_milestone
[i
] = this->to_millimeters(t
[i
]);
315 float actuator_pos
[3];
316 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
317 for (int i
= 0; i
< 3; i
++)
318 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
324 //A GCode has been received
325 //See if the current Gcode line has some orders for us
326 void Robot::on_gcode_received(void *argument
)
328 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
330 this->motion_mode
= -1;
332 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
335 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
336 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
337 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
338 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
339 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
340 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
341 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
342 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
343 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
344 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
345 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
347 if(gcode
->get_num_args() == 0) {
348 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
349 reset_axis_position(0, i
);
353 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
354 if ( gcode
->has_letter(letter
) ) {
355 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
360 gcode
->mark_as_taken();
364 } else if( gcode
->has_m
) {
366 case 92: // M92 - set steps per mm
367 if (gcode
->has_letter('X'))
368 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
369 if (gcode
->has_letter('Y'))
370 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
371 if (gcode
->has_letter('Z'))
372 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
373 if (gcode
->has_letter('F'))
374 seconds_per_minute
= gcode
->get_value('F');
376 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
);
377 gcode
->add_nl
= true;
378 gcode
->mark_as_taken();
379 check_max_actuator_speeds();
383 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 ",
384 from_millimeters(this->last_milestone
[0]),
385 from_millimeters(this->last_milestone
[1]),
386 from_millimeters(this->last_milestone
[2]),
387 actuators
[X_AXIS
]->get_current_position(),
388 actuators
[Y_AXIS
]->get_current_position(),
389 actuators
[Z_AXIS
]->get_current_position() );
390 gcode
->txt_after_ok
.append(buf
, n
);
391 gcode
->mark_as_taken();
395 case 203: // M203 Set maximum feedrates in mm/sec
396 if (gcode
->has_letter('X'))
397 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
398 if (gcode
->has_letter('Y'))
399 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
400 if (gcode
->has_letter('Z'))
401 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
402 if (gcode
->has_letter('A'))
403 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
404 if (gcode
->has_letter('B'))
405 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
406 if (gcode
->has_letter('C'))
407 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
409 check_max_actuator_speeds();
411 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
412 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
413 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
414 gcode
->add_nl
= true;
415 gcode
->mark_as_taken();
418 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
419 gcode
->mark_as_taken();
421 if (gcode
->has_letter('S')) {
422 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
423 THEKERNEL
->conveyor
->wait_for_empty_queue();
424 float acc
= gcode
->get_value('S'); // mm/s^2
428 THEKERNEL
->planner
->acceleration
= acc
;
430 if (gcode
->has_letter('Z')) {
431 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
432 THEKERNEL
->conveyor
->wait_for_empty_queue();
433 float acc
= gcode
->get_value('Z'); // mm/s^2
437 THEKERNEL
->planner
->z_acceleration
= acc
;
441 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
442 gcode
->mark_as_taken();
443 if (gcode
->has_letter('X')) {
444 float jd
= gcode
->get_value('X');
448 THEKERNEL
->planner
->junction_deviation
= jd
;
450 if (gcode
->has_letter('Z')) {
451 float jd
= gcode
->get_value('Z');
452 // enforce minimum, -1 disables it and uses regular junction deviation
455 THEKERNEL
->planner
->z_junction_deviation
= jd
;
457 if (gcode
->has_letter('S')) {
458 float mps
= gcode
->get_value('S');
462 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
466 case 220: // M220 - speed override percentage
467 gcode
->mark_as_taken();
468 if (gcode
->has_letter('S')) {
469 float factor
= gcode
->get_value('S');
470 // enforce minimum 10% speed
473 // enforce maximum 10x speed
474 if (factor
> 1000.0F
)
477 seconds_per_minute
= 6000.0F
/ factor
;
481 case 400: // wait until all moves are done up to this point
482 gcode
->mark_as_taken();
483 THEKERNEL
->conveyor
->wait_for_empty_queue();
486 case 500: // M500 saves some volatile settings to config override file
487 case 503: { // M503 just prints the settings
488 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
);
489 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
490 gcode
->stream
->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed:\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
);
491 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",
492 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
493 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
495 // get or save any arm solution specific optional values
496 BaseSolution::arm_options_t options
;
497 if(arm_solution
->get_optional(options
) && !options
.empty()) {
498 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
499 for(auto &i
: options
) {
500 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
502 gcode
->stream
->printf("\n");
504 gcode
->mark_as_taken();
508 case 665: { // M665 set optional arm solution variables based on arm solution.
509 gcode
->mark_as_taken();
510 // the parameter args could be any letter except S so ask solution what options it supports
511 BaseSolution::arm_options_t options
;
512 if(arm_solution
->get_optional(options
)) {
513 for(auto &i
: options
) {
514 // foreach optional value
516 if(gcode
->has_letter(c
)) { // set new value
517 i
.second
= gcode
->get_value(c
);
519 // print all current values of supported options
520 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
521 gcode
->add_nl
= true;
523 // set the new options
524 arm_solution
->set_optional(options
);
527 // set delta segments per second, not saved by M500
528 if(gcode
->has_letter('S')) {
529 this->delta_segments_per_second
= gcode
->get_value('S');
536 if( this->motion_mode
< 0)
540 float target
[3], offset
[3];
541 clear_vector(offset
);
543 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
545 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
546 if( gcode
->has_letter(letter
) ) {
547 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
550 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
551 if( gcode
->has_letter(letter
) ) {
552 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
556 if( gcode
->has_letter('F') ) {
557 if( this->motion_mode
== MOTION_MODE_SEEK
)
558 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
560 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
563 //Perform any physical actions
564 switch(this->motion_mode
) {
565 case MOTION_MODE_CANCEL
: break;
566 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
567 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
568 case MOTION_MODE_CW_ARC
:
569 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
572 // last_milestone was set to target in append_milestone, no need to do it again
576 // We received a new gcode, and one of the functions
577 // determined the distance for that given gcode. So now we can attach this gcode to the right block
579 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
581 //If the queue is empty, execute immediatly, otherwise attach to the last added block
582 THEKERNEL
->conveyor
->append_gcode(gcode
);
585 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
586 void Robot::reset_axis_position(float x
, float y
, float z
)
588 this->last_milestone
[X_AXIS
] = x
;
589 this->last_milestone
[Y_AXIS
] = y
;
590 this->last_milestone
[Z_AXIS
] = z
;
591 this->transformed_last_milestone
[X_AXIS
] = x
;
592 this->transformed_last_milestone
[Y_AXIS
] = y
;
593 this->transformed_last_milestone
[Z_AXIS
] = z
;
595 float actuator_pos
[3];
596 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
597 for (int i
= 0; i
< 3; i
++)
598 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
601 // Reset the position for an axis (used in homing and G92)
602 void Robot::reset_axis_position(float position
, int axis
)
604 this->last_milestone
[axis
] = position
;
605 this->transformed_last_milestone
[axis
] = position
;
607 float actuator_pos
[3];
608 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
610 for (int i
= 0; i
< 3; i
++)
611 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
614 // Use FK to find out where actuator is and reset lastmilestone to match
615 void Robot::reset_position_from_current_actuator_position()
617 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
618 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
619 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
622 // Convert target from millimeters to steps, and append this to the planner
623 void Robot::append_milestone( float target
[], float rate_mm_s
)
627 float actuator_pos
[3];
628 float transformed_target
[3]; // adjust target for bed compensation
629 float millimeters_of_travel
;
631 // unity transform by default
632 memcpy(transformed_target
, target
, sizeof(transformed_target
));
634 // check function pointer and call if set to transform the target to compensate for bed
635 if(compensationTransform
) {
636 // some compensation strategies can transform XYZ, some just change Z
637 compensationTransform(transformed_target
);
640 // find distance moved by each axis, use transformed target from last_transformed_target
641 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
642 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
644 // store last transformed
645 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
647 // Compute how long this move moves, so we can attach it to the block for later use
648 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
650 // find distance unit vector
651 for (int i
= 0; i
< 3; i
++)
652 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
654 // Do not move faster than the configured cartesian limits
655 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
656 if ( max_speeds
[axis
] > 0 ) {
657 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
659 if (axis_speed
> max_speeds
[axis
])
660 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
664 // find actuator position given cartesian position, use actual adjusted target
665 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
667 // check per-actuator speed limits
668 for (int actuator
= 0; actuator
<= 2; actuator
++) {
669 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
671 if (actuator_rate
> actuators
[actuator
]->max_rate
)
672 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
675 // Append the block to the planner
676 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
678 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
679 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
683 // Append a move to the queue ( cutting it into segments if needed )
684 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
687 // Find out the distance for this gcode
688 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 );
690 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
691 if( gcode
->millimeters_of_travel
< 1e-8F
) {
695 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
697 // Mark the gcode as having a known distance
698 this->distance_in_gcode_is_known( gcode
);
700 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
701 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
702 // 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
705 if(this->delta_segments_per_second
> 1.0F
) {
706 // enabled if set to something > 1, it is set to 0.0 by default
707 // segment based on current speed and requested segments per second
708 // the faster the travel speed the fewer segments needed
709 // NOTE rate is mm/sec and we take into account any speed override
710 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
711 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
712 // TODO if we are only moving in Z on a delta we don't really need to segment at all
715 if(this->mm_per_line_segment
== 0.0F
) {
716 segments
= 1; // don't split it up
718 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
723 // A vector to keep track of the endpoint of each segment
724 float segment_delta
[3];
725 float segment_end
[3];
727 // How far do we move each segment?
728 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
729 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
731 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
732 // We always add another point after this loop so we stop at segments-1, ie i < segments
733 for (int i
= 1; i
< segments
; i
++) {
734 if(halted
) return; // don;t queue any more segments
735 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
736 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
738 // Append the end of this segment to the queue
739 this->append_milestone(segment_end
, rate_mm_s
);
743 // Append the end of this full move to the queue
744 this->append_milestone(target
, rate_mm_s
);
746 // if adding these blocks didn't start executing, do that now
747 THEKERNEL
->conveyor
->ensure_running();
751 // Append an arc to the queue ( cutting it into segments as needed )
752 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
756 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
757 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
758 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
759 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
760 float r_axis1
= -offset
[this->plane_axis_1
];
761 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
762 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
764 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
765 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
766 if (angular_travel
< 0) {
767 angular_travel
+= 2 * M_PI
;
770 angular_travel
-= 2 * M_PI
;
773 // Find the distance for this gcode
774 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
776 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
777 if( gcode
->millimeters_of_travel
< 0.0001F
) {
781 // Mark the gcode as having a known distance
782 this->distance_in_gcode_is_known( gcode
);
784 // Figure out how many segments for this gcode
785 uint16_t segments
= floor(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
787 float theta_per_segment
= angular_travel
/ segments
;
788 float linear_per_segment
= linear_travel
/ segments
;
790 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
791 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
792 r_T = [cos(phi) -sin(phi);
793 sin(phi) cos(phi] * r ;
794 For arc generation, the center of the circle is the axis of rotation and the radius vector is
795 defined from the circle center to the initial position. Each line segment is formed by successive
796 vector rotations. This requires only two cos() and sin() computations to form the rotation
797 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
798 all float numbers are single precision on the Arduino. (True float precision will not have
799 round off issues for CNC applications.) Single precision error can accumulate to be greater than
800 tool precision in some cases. Therefore, arc path correction is implemented.
802 Small angle approximation may be used to reduce computation overhead further. This approximation
803 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
804 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
805 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
806 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
807 issue for CNC machines with the single precision Arduino calculations.
808 This approximation also allows mc_arc to immediately insert a line segment into the planner
809 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
810 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
811 This is important when there are successive arc motions.
813 // Vector rotation matrix values
814 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
815 float sin_T
= theta_per_segment
;
824 // Initialize the linear axis
825 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
827 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
828 if(halted
) return; // don't queue any more segments
830 if (count
< this->arc_correction
) {
831 // Apply vector rotation matrix
832 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
833 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
837 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
838 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
839 cos_Ti
= cosf(i
* theta_per_segment
);
840 sin_Ti
= sinf(i
* theta_per_segment
);
841 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
842 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
846 // Update arc_target location
847 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
848 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
849 arc_target
[this->plane_axis_2
] += linear_per_segment
;
851 // Append this segment to the queue
852 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
856 // Ensure last segment arrives at target location.
857 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
860 // Do the math for an arc and add it to the queue
861 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
865 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
867 // Set clockwise/counter-clockwise sign for mc_arc computations
868 bool is_clockwise
= false;
869 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
874 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
879 float Robot::theta(float x
, float y
)
881 float t
= atanf(x
/ fabs(y
));
893 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
895 this->plane_axis_0
= axis_0
;
896 this->plane_axis_1
= axis_1
;
897 this->plane_axis_2
= axis_2
;
900 void Robot::clearToolOffset()
902 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
905 void Robot::setToolOffset(const float offset
[3])
907 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));