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 this->arm_solution
= NULL
;
127 seconds_per_minute
= 60.0F
;
128 this->clearToolOffset();
129 this->adjustZfnc
= nullptr;
132 //Called when the module has just been loaded
133 void Robot::on_module_loaded()
135 this->register_for_event(ON_GCODE_RECEIVED
);
136 this->register_for_event(ON_GET_PUBLIC_DATA
);
137 this->register_for_event(ON_SET_PUBLIC_DATA
);
140 this->on_config_reload(this);
143 void Robot::on_config_reload(void *argument
)
146 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
147 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
148 // To make adding those solution easier, they have their own, separate object.
149 // Here we read the config to find out which arm solution to use
150 if (this->arm_solution
) delete this->arm_solution
;
151 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
152 // Note checksums are not const expressions when in debug mode, so don't use switch
153 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
154 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
157 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
160 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== morgan_checksum
) {
163 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== cartesian_checksum
) {
166 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
169 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
173 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
174 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
175 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
176 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
177 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
178 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
180 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
181 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
182 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
194 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
195 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
196 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
197 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
198 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
199 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
200 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
201 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
202 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
204 float steps_per_mm
[3] = {
205 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
206 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
207 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
210 // TODO: delete or detect old steppermotors
211 // Make our 3 StepperMotors
212 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
213 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
214 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
216 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
217 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
218 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
220 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
221 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
222 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
223 check_max_actuator_speeds(); // check the configs are sane
226 actuators
.push_back(alpha_stepper_motor
);
227 actuators
.push_back(beta_stepper_motor
);
228 actuators
.push_back(gamma_stepper_motor
);
231 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
232 // so the first move can be correct if homing is not performed
233 float actuator_pos
[3];
234 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
235 for (int i
= 0; i
< 3; i
++)
236 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
238 //this->clearToolOffset();
241 // this does a sanity check that actuator speeds do not exceed steps rate capability
242 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
243 void Robot::check_max_actuator_speeds()
245 float step_freq
= alpha_stepper_motor
->max_rate
* alpha_stepper_motor
->get_steps_per_mm();
246 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
247 alpha_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm());
248 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
);
251 step_freq
= beta_stepper_motor
->max_rate
* beta_stepper_motor
->get_steps_per_mm();
252 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
253 beta_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm());
254 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
);
257 step_freq
= gamma_stepper_motor
->max_rate
* gamma_stepper_motor
->get_steps_per_mm();
258 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
259 gamma_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm());
260 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
);
264 void Robot::on_get_public_data(void *argument
)
266 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
268 if(!pdr
->starts_with(robot_checksum
)) return;
270 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
271 static float return_data
;
272 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
273 pdr
->set_data_ptr(&return_data
);
276 } else if(pdr
->second_element_is(current_position_checksum
)) {
277 static float return_data
[3];
278 return_data
[0] = from_millimeters(this->last_milestone
[0]);
279 return_data
[1] = from_millimeters(this->last_milestone
[1]);
280 return_data
[2] = from_millimeters(this->last_milestone
[2]);
282 pdr
->set_data_ptr(&return_data
);
287 void Robot::on_set_public_data(void *argument
)
289 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
291 if(!pdr
->starts_with(robot_checksum
)) return;
293 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
294 // NOTE do not use this while printing!
295 float t
= *static_cast<float *>(pdr
->get_data_ptr());
296 // enforce minimum 10% speed
297 if (t
< 10.0F
) t
= 10.0F
;
299 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
301 } else if(pdr
->second_element_is(current_position_checksum
)) {
302 float *t
= static_cast<float *>(pdr
->get_data_ptr());
303 for (int i
= 0; i
< 3; i
++) {
304 this->last_milestone
[i
] = this->to_millimeters(t
[i
]);
307 float actuator_pos
[3];
308 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
309 for (int i
= 0; i
< 3; i
++)
310 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
316 //A GCode has been received
317 //See if the current Gcode line has some orders for us
318 void Robot::on_gcode_received(void *argument
)
320 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
322 this->motion_mode
= -1;
324 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
327 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
328 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
329 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
330 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
331 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
332 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
333 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
334 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
335 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
336 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
337 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
339 if(gcode
->get_num_args() == 0) {
340 clear_vector(this->last_milestone
);
342 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
343 if ( gcode
->has_letter(letter
) )
344 this->last_milestone
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
));
348 // TODO: handle any number of actuators
349 float actuator_pos
[3];
350 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
352 for (int i
= 0; i
< 3; i
++)
353 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
355 gcode
->mark_as_taken();
359 } else if( gcode
->has_m
) {
361 case 92: // M92 - set steps per mm
362 if (gcode
->has_letter('X'))
363 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
364 if (gcode
->has_letter('Y'))
365 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
366 if (gcode
->has_letter('Z'))
367 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
368 if (gcode
->has_letter('F'))
369 seconds_per_minute
= gcode
->get_value('F');
371 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
);
372 gcode
->add_nl
= true;
373 gcode
->mark_as_taken();
374 check_max_actuator_speeds();
378 int n
= snprintf(buf
, sizeof(buf
), "C: X:%1.3f Y:%1.3f Z:%1.3f",
379 from_millimeters(this->last_milestone
[0]),
380 from_millimeters(this->last_milestone
[1]),
381 from_millimeters(this->last_milestone
[2]));
382 gcode
->txt_after_ok
.append(buf
, n
);
383 gcode
->mark_as_taken();
387 case 203: // M203 Set maximum feedrates in mm/sec
388 if (gcode
->has_letter('X'))
389 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
390 if (gcode
->has_letter('Y'))
391 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
392 if (gcode
->has_letter('Z'))
393 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
394 if (gcode
->has_letter('A'))
395 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
396 if (gcode
->has_letter('B'))
397 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
398 if (gcode
->has_letter('C'))
399 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
401 check_max_actuator_speeds();
403 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
404 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
405 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
406 gcode
->add_nl
= true;
407 gcode
->mark_as_taken();
410 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
411 gcode
->mark_as_taken();
413 if (gcode
->has_letter('S')) {
414 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
415 THEKERNEL
->conveyor
->wait_for_empty_queue();
416 float acc
= gcode
->get_value('S'); // mm/s^2
420 THEKERNEL
->planner
->acceleration
= acc
;
424 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
425 gcode
->mark_as_taken();
426 if (gcode
->has_letter('X')) {
427 float jd
= gcode
->get_value('X');
431 THEKERNEL
->planner
->junction_deviation
= jd
;
433 if (gcode
->has_letter('S')) {
434 float mps
= gcode
->get_value('S');
438 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
442 case 220: // M220 - speed override percentage
443 gcode
->mark_as_taken();
444 if (gcode
->has_letter('S')) {
445 float factor
= gcode
->get_value('S');
446 // enforce minimum 10% speed
449 // enforce maximum 10x speed
450 if (factor
> 1000.0F
)
453 seconds_per_minute
= 6000.0F
/ factor
;
457 case 400: // wait until all moves are done up to this point
458 gcode
->mark_as_taken();
459 THEKERNEL
->conveyor
->wait_for_empty_queue();
462 case 500: // M500 saves some volatile settings to config override file
463 case 503: { // M503 just prints the settings
464 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
);
465 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
466 gcode
->stream
->printf(";X- Junction Deviation, S - Minimum Planner speed:\nM205 X%1.5f S%1.5f\n", THEKERNEL
->planner
->junction_deviation
, THEKERNEL
->planner
->minimum_planner_speed
);
467 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",
468 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
469 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
471 // get or save any arm solution specific optional values
472 BaseSolution::arm_options_t options
;
473 if(arm_solution
->get_optional(options
) && !options
.empty()) {
474 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
475 for(auto &i
: options
) {
476 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
478 gcode
->stream
->printf("\n");
480 gcode
->mark_as_taken();
484 case 665: { // M665 set optional arm solution variables based on arm solution.
485 gcode
->mark_as_taken();
486 // the parameter args could be any letter except S so ask solution what options it supports
487 BaseSolution::arm_options_t options
;
488 if(arm_solution
->get_optional(options
)) {
489 for(auto &i
: options
) {
490 // foreach optional value
492 if(gcode
->has_letter(c
)) { // set new value
493 i
.second
= gcode
->get_value(c
);
495 // print all current values of supported options
496 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
497 gcode
->add_nl
= true;
499 // set the new options
500 arm_solution
->set_optional(options
);
503 // set delta segments per second, not saved by M500
504 if(gcode
->has_letter('S')) {
505 this->delta_segments_per_second
= gcode
->get_value('S');
512 if( this->motion_mode
< 0)
516 float target
[3], offset
[3];
517 clear_vector(offset
);
519 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
521 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
522 if( gcode
->has_letter(letter
) ) {
523 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
526 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
527 if( gcode
->has_letter(letter
) ) {
528 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
532 if( gcode
->has_letter('F') ) {
533 if( this->motion_mode
== MOTION_MODE_SEEK
)
534 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
536 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
539 //Perform any physical actions
540 switch(this->motion_mode
) {
541 case MOTION_MODE_CANCEL
: break;
542 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
543 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
544 case MOTION_MODE_CW_ARC
:
545 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
548 // last_milestone was set to target in append_milestone, no need to do it again
552 // We received a new gcode, and one of the functions
553 // determined the distance for that given gcode. So now we can attach this gcode to the right block
555 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
558 //If the queue is empty, execute immediatly, otherwise attach to the last added block
559 THEKERNEL
->conveyor
->append_gcode(gcode
);
562 // Reset the position for all axes ( used in homing and G92 stuff )
563 void Robot::reset_axis_position(float position
, int axis
)
565 this->last_milestone
[axis
] = position
;
567 float actuator_pos
[3];
568 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
570 for (int i
= 0; i
< 3; i
++)
571 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
575 // Convert target from millimeters to steps, and append this to the planner
576 void Robot::append_milestone( float target
[], float rate_mm_s
)
580 float actuator_pos
[3];
581 float adj_target
[3]; // adjust target for bed leveling
582 float millimeters_of_travel
;
584 memcpy(adj_target
, target
, sizeof(adj_target
));
586 // check function pointer and call if set to adjust Z for bed leveling
588 adj_target
[Z_AXIS
] += adjustZfnc(target
[X_AXIS
], target
[Y_AXIS
]);
591 // find distance moved by each axis, use actual adjusted target
592 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
593 deltas
[axis
] = adj_target
[axis
] - last_milestone
[axis
];
595 // Compute how long this move moves, so we can attach it to the block for later use
596 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
598 // find distance unit vector
599 for (int i
= 0; i
< 3; i
++)
600 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
602 // Do not move faster than the configured cartesian limits
603 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
604 if ( max_speeds
[axis
] > 0 ) {
605 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
607 if (axis_speed
> max_speeds
[axis
])
608 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
612 // find actuator position given cartesian position, use actual adjusted target
613 arm_solution
->cartesian_to_actuator( adj_target
, actuator_pos
);
615 // check per-actuator speed limits
616 for (int actuator
= 0; actuator
<= 2; actuator
++) {
617 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
619 if (actuator_rate
> actuators
[actuator
]->max_rate
)
620 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
623 // Append the block to the planner
624 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
626 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
627 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
631 // Append a move to the queue ( cutting it into segments if needed )
632 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
635 // Find out the distance for this gcode
636 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 );
638 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
639 if( gcode
->millimeters_of_travel
< 1e-8F
) {
643 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
645 // Mark the gcode as having a known distance
646 this->distance_in_gcode_is_known( gcode
);
648 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
649 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
650 // 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
653 if(this->delta_segments_per_second
> 1.0F
) {
654 // enabled if set to something > 1, it is set to 0.0 by default
655 // segment based on current speed and requested segments per second
656 // the faster the travel speed the fewer segments needed
657 // NOTE rate is mm/sec and we take into account any speed override
658 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
659 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
660 // TODO if we are only moving in Z on a delta we don't really need to segment at all
663 if(this->mm_per_line_segment
== 0.0F
) {
664 segments
= 1; // don't split it up
666 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
671 // A vector to keep track of the endpoint of each segment
672 float segment_delta
[3];
673 float segment_end
[3];
675 // How far do we move each segment?
676 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
677 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
679 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
680 // We always add another point after this loop so we stop at segments-1, ie i < segments
681 for (int i
= 1; i
< segments
; i
++) {
682 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
683 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
685 // Append the end of this segment to the queue
686 this->append_milestone(segment_end
, rate_mm_s
);
690 // Append the end of this full move to the queue
691 this->append_milestone(target
, rate_mm_s
);
693 // if adding these blocks didn't start executing, do that now
694 THEKERNEL
->conveyor
->ensure_running();
698 // Append an arc to the queue ( cutting it into segments as needed )
699 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
703 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
704 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
705 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
706 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
707 float r_axis1
= -offset
[this->plane_axis_1
];
708 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
709 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
711 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
712 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
713 if (angular_travel
< 0) {
714 angular_travel
+= 2 * M_PI
;
717 angular_travel
-= 2 * M_PI
;
720 // Find the distance for this gcode
721 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
723 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
724 if( gcode
->millimeters_of_travel
< 0.0001F
) {
728 // Mark the gcode as having a known distance
729 this->distance_in_gcode_is_known( gcode
);
731 // Figure out how many segments for this gcode
732 uint16_t segments
= floor(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
734 float theta_per_segment
= angular_travel
/ segments
;
735 float linear_per_segment
= linear_travel
/ segments
;
737 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
738 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
739 r_T = [cos(phi) -sin(phi);
740 sin(phi) cos(phi] * r ;
741 For arc generation, the center of the circle is the axis of rotation and the radius vector is
742 defined from the circle center to the initial position. Each line segment is formed by successive
743 vector rotations. This requires only two cos() and sin() computations to form the rotation
744 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
745 all float numbers are single precision on the Arduino. (True float precision will not have
746 round off issues for CNC applications.) Single precision error can accumulate to be greater than
747 tool precision in some cases. Therefore, arc path correction is implemented.
749 Small angle approximation may be used to reduce computation overhead further. This approximation
750 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
751 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
752 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
753 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
754 issue for CNC machines with the single precision Arduino calculations.
755 This approximation also allows mc_arc to immediately insert a line segment into the planner
756 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
757 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
758 This is important when there are successive arc motions.
760 // Vector rotation matrix values
761 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
762 float sin_T
= theta_per_segment
;
771 // Initialize the linear axis
772 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
774 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
776 if (count
< this->arc_correction
) {
777 // Apply vector rotation matrix
778 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
779 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
783 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
784 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
785 cos_Ti
= cosf(i
* theta_per_segment
);
786 sin_Ti
= sinf(i
* theta_per_segment
);
787 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
788 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
792 // Update arc_target location
793 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
794 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
795 arc_target
[this->plane_axis_2
] += linear_per_segment
;
797 // Append this segment to the queue
798 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
802 // Ensure last segment arrives at target location.
803 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
806 // Do the math for an arc and add it to the queue
807 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
811 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
813 // Set clockwise/counter-clockwise sign for mc_arc computations
814 bool is_clockwise
= false;
815 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
820 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
825 float Robot::theta(float x
, float y
)
827 float t
= atanf(x
/ fabs(y
));
839 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
841 this->plane_axis_0
= axis_0
;
842 this->plane_axis_1
= axis_1
;
843 this->plane_axis_2
= axis_2
;
846 void Robot::clearToolOffset()
848 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
851 void Robot::setToolOffset(const float offset
[3])
853 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));