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 "StepTicker.h"
30 #include "checksumm.h"
32 #include "ConfigValue.h"
33 #include "libs/StreamOutput.h"
34 #include "StreamOutputPool.h"
36 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
37 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
38 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
39 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
40 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
41 #define arc_correction_checksum CHECKSUM("arc_correction")
42 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
43 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
44 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
47 #define arm_solution_checksum CHECKSUM("arm_solution")
48 #define cartesian_checksum CHECKSUM("cartesian")
49 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
50 #define rostock_checksum CHECKSUM("rostock")
51 #define linear_delta_checksum CHECKSUM("linear_delta")
52 #define delta_checksum CHECKSUM("delta")
53 #define hbot_checksum CHECKSUM("hbot")
54 #define corexy_checksum CHECKSUM("corexy")
55 #define kossel_checksum CHECKSUM("kossel")
57 // stepper motor stuff
58 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
59 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
60 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
61 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
62 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
63 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
64 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
65 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
66 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
68 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
69 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
70 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
72 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
73 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
74 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
77 // new-style actuator stuff
78 #define actuator_checksum CHEKCSUM("actuator")
80 #define step_pin_checksum CHECKSUM("step_pin")
81 #define dir_pin_checksum CHEKCSUM("dir_pin")
82 #define en_pin_checksum CHECKSUM("en_pin")
84 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
85 #define max_rate_checksum CHECKSUM("max_rate")
87 #define alpha_checksum CHECKSUM("alpha")
88 #define beta_checksum CHECKSUM("beta")
89 #define gamma_checksum CHECKSUM("gamma")
92 #define NEXT_ACTION_DEFAULT 0
93 #define NEXT_ACTION_DWELL 1
94 #define NEXT_ACTION_GO_HOME 2
96 #define MOTION_MODE_SEEK 0 // G0
97 #define MOTION_MODE_LINEAR 1 // G1
98 #define MOTION_MODE_CW_ARC 2 // G2
99 #define MOTION_MODE_CCW_ARC 3 // G3
100 #define MOTION_MODE_CANCEL 4 // G80
102 #define PATH_CONTROL_MODE_EXACT_PATH 0
103 #define PATH_CONTROL_MODE_EXACT_STOP 1
104 #define PATH_CONTROL_MODE_CONTINOUS 2
106 #define PROGRAM_FLOW_RUNNING 0
107 #define PROGRAM_FLOW_PAUSED 1
108 #define PROGRAM_FLOW_COMPLETED 2
110 #define SPINDLE_DIRECTION_CW 0
111 #define SPINDLE_DIRECTION_CCW 1
113 // 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
114 // It takes care of cutting arcs into segments, same thing for line that are too long
115 #define max(a,b) (((a) > (b)) ? (a) : (b))
119 this->inch_mode
= false;
120 this->absolute_mode
= true;
121 this->motion_mode
= MOTION_MODE_SEEK
;
122 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
123 clear_vector(this->last_milestone
);
124 this->arm_solution
= NULL
;
125 seconds_per_minute
= 60.0F
;
126 this->clearToolOffset();
129 //Called when the module has just been loaded
130 void Robot::on_module_loaded()
132 this->register_for_event(ON_GCODE_RECEIVED
);
133 this->register_for_event(ON_GET_PUBLIC_DATA
);
134 this->register_for_event(ON_SET_PUBLIC_DATA
);
137 this->on_config_reload(this);
140 void Robot::on_config_reload(void *argument
)
143 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
144 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
145 // To make adding those solution easier, they have their own, separate object.
146 // Here we read the config to find out which arm solution to use
147 if (this->arm_solution
) delete this->arm_solution
;
148 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
149 // Note checksums are not const expressions when in debug mode, so don't use switch
150 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
151 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
153 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
154 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
157 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== cartesian_checksum
) {
160 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
163 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
167 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
168 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
169 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
170 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
171 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
172 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
174 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
175 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
176 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
188 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
189 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
190 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
191 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
192 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
193 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
194 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
195 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
196 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
198 float steps_per_mm
[3] = {
199 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
200 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
201 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
204 // TODO: delete or detect old steppermotors
205 // Make our 3 StepperMotors
206 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
207 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
208 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
210 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
211 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
212 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
214 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
215 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
216 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
217 check_max_actuator_speeds(); // check the configs are sane
220 actuators
.push_back(alpha_stepper_motor
);
221 actuators
.push_back(beta_stepper_motor
);
222 actuators
.push_back(gamma_stepper_motor
);
225 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
226 // so the first move can be correct if homing is not performed
227 float actuator_pos
[3];
228 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
229 for (int i
= 0; i
< 3; i
++)
230 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
232 //this->clearToolOffset();
235 // this does a sanity check that actuator speeds do not exceed steps rate capability
236 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
237 void Robot::check_max_actuator_speeds()
239 float step_freq
= alpha_stepper_motor
->max_rate
* alpha_stepper_motor
->get_steps_per_mm();
240 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
241 alpha_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm());
242 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
);
245 step_freq
= beta_stepper_motor
->max_rate
* beta_stepper_motor
->get_steps_per_mm();
246 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
247 beta_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm());
248 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
);
251 step_freq
= gamma_stepper_motor
->max_rate
* gamma_stepper_motor
->get_steps_per_mm();
252 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
253 gamma_stepper_motor
->max_rate
= floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm());
254 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
);
258 void Robot::on_get_public_data(void *argument
)
260 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
262 if(!pdr
->starts_with(robot_checksum
)) return;
264 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
265 static float return_data
;
266 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
267 pdr
->set_data_ptr(&return_data
);
270 } else if(pdr
->second_element_is(current_position_checksum
)) {
271 static float return_data
[3];
272 return_data
[0] = from_millimeters(this->last_milestone
[0]);
273 return_data
[1] = from_millimeters(this->last_milestone
[1]);
274 return_data
[2] = from_millimeters(this->last_milestone
[2]);
276 pdr
->set_data_ptr(&return_data
);
281 void Robot::on_set_public_data(void *argument
)
283 PublicDataRequest
*pdr
= static_cast<PublicDataRequest
*>(argument
);
285 if(!pdr
->starts_with(robot_checksum
)) return;
287 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
288 // NOTE do not use this while printing!
289 float t
= *static_cast<float *>(pdr
->get_data_ptr());
290 // enforce minimum 10% speed
291 if (t
< 10.0F
) t
= 10.0F
;
293 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
295 } else if(pdr
->second_element_is(current_position_checksum
)) {
296 float *t
= static_cast<float *>(pdr
->get_data_ptr());
297 for (int i
= 0; i
< 3; i
++) {
298 this->last_milestone
[i
] = this->to_millimeters(t
[i
]);
301 float actuator_pos
[3];
302 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
303 for (int i
= 0; i
< 3; i
++)
304 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
310 //A GCode has been received
311 //See if the current Gcode line has some orders for us
312 void Robot::on_gcode_received(void *argument
)
314 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
316 this->motion_mode
= -1;
318 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
321 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
322 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
323 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
324 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
325 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
326 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
327 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
328 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
329 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
330 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
331 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
333 if(gcode
->get_num_args() == 0) {
334 clear_vector(this->last_milestone
);
336 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
337 if ( gcode
->has_letter(letter
) )
338 this->last_milestone
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
));
342 // TODO: handle any number of actuators
343 float actuator_pos
[3];
344 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
346 for (int i
= 0; i
< 3; i
++)
347 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
349 gcode
->mark_as_taken();
353 } else if( gcode
->has_m
) {
355 case 92: // M92 - set steps per mm
356 if (gcode
->has_letter('X'))
357 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
358 if (gcode
->has_letter('Y'))
359 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
360 if (gcode
->has_letter('Z'))
361 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
362 if (gcode
->has_letter('F'))
363 seconds_per_minute
= gcode
->get_value('F');
365 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
);
366 gcode
->add_nl
= true;
367 gcode
->mark_as_taken();
368 check_max_actuator_speeds();
372 int n
= snprintf(buf
, sizeof(buf
), "C: X:%1.3f Y:%1.3f Z:%1.3f",
373 from_millimeters(this->last_milestone
[0]),
374 from_millimeters(this->last_milestone
[1]),
375 from_millimeters(this->last_milestone
[2]));
376 gcode
->txt_after_ok
.append(buf
, n
);
377 gcode
->mark_as_taken();
381 case 203: // M203 Set maximum feedrates in mm/sec
382 if (gcode
->has_letter('X'))
383 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
384 if (gcode
->has_letter('Y'))
385 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
386 if (gcode
->has_letter('Z'))
387 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
388 if (gcode
->has_letter('A'))
389 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
390 if (gcode
->has_letter('B'))
391 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
392 if (gcode
->has_letter('C'))
393 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
395 check_max_actuator_speeds();
397 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
398 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
399 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
400 gcode
->add_nl
= true;
401 gcode
->mark_as_taken();
404 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
405 gcode
->mark_as_taken();
407 if (gcode
->has_letter('S')) {
408 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
409 THEKERNEL
->conveyor
->wait_for_empty_queue();
410 float acc
= gcode
->get_value('S'); // mm/s^2
414 THEKERNEL
->planner
->acceleration
= acc
;
418 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
419 gcode
->mark_as_taken();
420 if (gcode
->has_letter('X')) {
421 float jd
= gcode
->get_value('X');
425 THEKERNEL
->planner
->junction_deviation
= jd
;
427 if (gcode
->has_letter('S')) {
428 float mps
= gcode
->get_value('S');
432 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
436 case 220: // M220 - speed override percentage
437 gcode
->mark_as_taken();
438 if (gcode
->has_letter('S')) {
439 float factor
= gcode
->get_value('S');
440 // enforce minimum 10% speed
443 // enforce maximum 10x speed
444 if (factor
> 1000.0F
)
447 seconds_per_minute
= 6000.0F
/ factor
;
451 case 400: // wait until all moves are done up to this point
452 gcode
->mark_as_taken();
453 THEKERNEL
->conveyor
->wait_for_empty_queue();
456 case 500: // M500 saves some volatile settings to config override file
457 case 503: { // M503 just prints the settings
458 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
);
459 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
460 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
);
461 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",
462 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
463 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
465 // get or save any arm solution specific optional values
466 BaseSolution::arm_options_t options
;
467 if(arm_solution
->get_optional(options
) && !options
.empty()) {
468 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
469 for(auto &i
: options
) {
470 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
472 gcode
->stream
->printf("\n");
474 gcode
->mark_as_taken();
478 case 665: { // M665 set optional arm solution variables based on arm solution.
479 gcode
->mark_as_taken();
480 // the parameter args could be any letter except S so ask solution what options it supports
481 BaseSolution::arm_options_t options
;
482 if(arm_solution
->get_optional(options
)) {
483 for(auto &i
: options
) {
484 // foreach optional value
486 if(gcode
->has_letter(c
)) { // set new value
487 i
.second
= gcode
->get_value(c
);
489 // print all current values of supported options
490 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
491 gcode
->add_nl
= true;
493 // set the new options
494 arm_solution
->set_optional(options
);
497 // set delta segments per second, not saved by M500
498 if(gcode
->has_letter('S')) {
499 this->delta_segments_per_second
= gcode
->get_value('S');
506 if( this->motion_mode
< 0)
510 float target
[3], offset
[3];
511 clear_vector(offset
);
513 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
515 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
516 if( gcode
->has_letter(letter
) ) {
517 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
520 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
521 if( gcode
->has_letter(letter
) ) {
522 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
526 if( gcode
->has_letter('F') ) {
527 if( this->motion_mode
== MOTION_MODE_SEEK
)
528 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
530 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
533 //Perform any physical actions
534 switch(this->motion_mode
) {
535 case MOTION_MODE_CANCEL
: break;
536 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
537 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
538 case MOTION_MODE_CW_ARC
:
539 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
542 // last_milestone was set to target in append_milestone, no need to do it again
546 // We received a new gcode, and one of the functions
547 // determined the distance for that given gcode. So now we can attach this gcode to the right block
549 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
552 //If the queue is empty, execute immediatly, otherwise attach to the last added block
553 THEKERNEL
->conveyor
->append_gcode(gcode
);
556 // Reset the position for all axes ( used in homing and G92 stuff )
557 void Robot::reset_axis_position(float position
, int axis
)
559 this->last_milestone
[axis
] = position
;
561 float actuator_pos
[3];
562 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
564 for (int i
= 0; i
< 3; i
++)
565 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
569 // Convert target from millimeters to steps, and append this to the planner
570 void Robot::append_milestone( float target
[], float rate_mm_s
)
574 float actuator_pos
[3];
575 float millimeters_of_travel
;
577 // find distance moved by each axis
578 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
579 deltas
[axis
] = target
[axis
] - last_milestone
[axis
];
581 // Compute how long this move moves, so we can attach it to the block for later use
582 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
584 // find distance unit vector
585 for (int i
= 0; i
< 3; i
++)
586 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
588 // Do not move faster than the configured cartesian limits
589 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
590 if ( max_speeds
[axis
] > 0 ) {
591 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
593 if (axis_speed
> max_speeds
[axis
])
594 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
598 // find actuator position given cartesian position
599 arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
601 // check per-actuator speed limits
602 for (int actuator
= 0; actuator
<= 2; actuator
++) {
603 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
605 if (actuator_rate
> actuators
[actuator
]->max_rate
)
606 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
609 // Append the block to the planner
610 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
612 // Update the last_milestone to the current target for the next time we use last_milestone
613 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
617 // Append a move to the queue ( cutting it into segments if needed )
618 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
621 // Find out the distance for this gcode
622 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 );
624 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
625 if( gcode
->millimeters_of_travel
< 1e-8F
) {
629 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
631 // Mark the gcode as having a known distance
632 this->distance_in_gcode_is_known( gcode
);
634 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
635 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
636 // 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
639 if(this->delta_segments_per_second
> 1.0F
) {
640 // enabled if set to something > 1, it is set to 0.0 by default
641 // segment based on current speed and requested segments per second
642 // the faster the travel speed the fewer segments needed
643 // NOTE rate is mm/sec and we take into account any speed override
644 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
645 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
646 // TODO if we are only moving in Z on a delta we don't really need to segment at all
649 if(this->mm_per_line_segment
== 0.0F
) {
650 segments
= 1; // don't split it up
652 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
657 // A vector to keep track of the endpoint of each segment
658 float segment_delta
[3];
659 float segment_end
[3];
661 // How far do we move each segment?
662 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
663 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
665 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
666 // We always add another point after this loop so we stop at segments-1, ie i < segments
667 for (int i
= 1; i
< segments
; i
++) {
668 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
669 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
671 // Append the end of this segment to the queue
672 this->append_milestone(segment_end
, rate_mm_s
);
676 // Append the end of this full move to the queue
677 this->append_milestone(target
, rate_mm_s
);
679 // if adding these blocks didn't start executing, do that now
680 THEKERNEL
->conveyor
->ensure_running();
684 // Append an arc to the queue ( cutting it into segments as needed )
685 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
689 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
690 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
691 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
692 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
693 float r_axis1
= -offset
[this->plane_axis_1
];
694 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
695 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
697 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
698 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
699 if (angular_travel
< 0) {
700 angular_travel
+= 2 * M_PI
;
703 angular_travel
-= 2 * M_PI
;
706 // Find the distance for this gcode
707 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
709 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
710 if( gcode
->millimeters_of_travel
< 0.0001F
) {
714 // Mark the gcode as having a known distance
715 this->distance_in_gcode_is_known( gcode
);
717 // Figure out how many segments for this gcode
718 uint16_t segments
= floor(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
720 float theta_per_segment
= angular_travel
/ segments
;
721 float linear_per_segment
= linear_travel
/ segments
;
723 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
724 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
725 r_T = [cos(phi) -sin(phi);
726 sin(phi) cos(phi] * r ;
727 For arc generation, the center of the circle is the axis of rotation and the radius vector is
728 defined from the circle center to the initial position. Each line segment is formed by successive
729 vector rotations. This requires only two cos() and sin() computations to form the rotation
730 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
731 all float numbers are single precision on the Arduino. (True float precision will not have
732 round off issues for CNC applications.) Single precision error can accumulate to be greater than
733 tool precision in some cases. Therefore, arc path correction is implemented.
735 Small angle approximation may be used to reduce computation overhead further. This approximation
736 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
737 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
738 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
739 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
740 issue for CNC machines with the single precision Arduino calculations.
741 This approximation also allows mc_arc to immediately insert a line segment into the planner
742 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
743 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
744 This is important when there are successive arc motions.
746 // Vector rotation matrix values
747 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
748 float sin_T
= theta_per_segment
;
757 // Initialize the linear axis
758 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
760 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
762 if (count
< this->arc_correction
) {
763 // Apply vector rotation matrix
764 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
765 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
769 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
770 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
771 cos_Ti
= cosf(i
* theta_per_segment
);
772 sin_Ti
= sinf(i
* theta_per_segment
);
773 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
774 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
778 // Update arc_target location
779 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
780 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
781 arc_target
[this->plane_axis_2
] += linear_per_segment
;
783 // Append this segment to the queue
784 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
788 // Ensure last segment arrives at target location.
789 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
792 // Do the math for an arc and add it to the queue
793 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
797 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
799 // Set clockwise/counter-clockwise sign for mc_arc computations
800 bool is_clockwise
= false;
801 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
806 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
811 float Robot::theta(float x
, float y
)
813 float t
= atanf(x
/ fabs(y
));
825 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
827 this->plane_axis_0
= axis_0
;
828 this->plane_axis_1
= axis_1
;
829 this->plane_axis_2
= axis_2
;
832 void Robot::clearToolOffset()
834 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
837 void Robot::setToolOffset(const float offset
[3])
839 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));