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 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('S')) {
451 float mps
= gcode
->get_value('S');
455 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
459 case 220: // M220 - speed override percentage
460 gcode
->mark_as_taken();
461 if (gcode
->has_letter('S')) {
462 float factor
= gcode
->get_value('S');
463 // enforce minimum 10% speed
466 // enforce maximum 10x speed
467 if (factor
> 1000.0F
)
470 seconds_per_minute
= 6000.0F
/ factor
;
474 case 400: // wait until all moves are done up to this point
475 gcode
->mark_as_taken();
476 THEKERNEL
->conveyor
->wait_for_empty_queue();
479 case 500: // M500 saves some volatile settings to config override file
480 case 503: { // M503 just prints the settings
481 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
);
482 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
483 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
);
484 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",
485 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
486 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
488 // get or save any arm solution specific optional values
489 BaseSolution::arm_options_t options
;
490 if(arm_solution
->get_optional(options
) && !options
.empty()) {
491 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
492 for(auto &i
: options
) {
493 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
495 gcode
->stream
->printf("\n");
497 gcode
->mark_as_taken();
501 case 665: { // M665 set optional arm solution variables based on arm solution.
502 gcode
->mark_as_taken();
503 // the parameter args could be any letter except S so ask solution what options it supports
504 BaseSolution::arm_options_t options
;
505 if(arm_solution
->get_optional(options
)) {
506 for(auto &i
: options
) {
507 // foreach optional value
509 if(gcode
->has_letter(c
)) { // set new value
510 i
.second
= gcode
->get_value(c
);
512 // print all current values of supported options
513 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
514 gcode
->add_nl
= true;
516 // set the new options
517 arm_solution
->set_optional(options
);
520 // set delta segments per second, not saved by M500
521 if(gcode
->has_letter('S')) {
522 this->delta_segments_per_second
= gcode
->get_value('S');
529 if( this->motion_mode
< 0)
533 float target
[3], offset
[3];
534 clear_vector(offset
);
536 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
538 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
539 if( gcode
->has_letter(letter
) ) {
540 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
543 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
544 if( gcode
->has_letter(letter
) ) {
545 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
549 if( gcode
->has_letter('F') ) {
550 if( this->motion_mode
== MOTION_MODE_SEEK
)
551 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
553 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
556 //Perform any physical actions
557 switch(this->motion_mode
) {
558 case MOTION_MODE_CANCEL
: break;
559 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
560 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
561 case MOTION_MODE_CW_ARC
:
562 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
565 // last_milestone was set to target in append_milestone, no need to do it again
569 // We received a new gcode, and one of the functions
570 // determined the distance for that given gcode. So now we can attach this gcode to the right block
572 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
574 //If the queue is empty, execute immediatly, otherwise attach to the last added block
575 THEKERNEL
->conveyor
->append_gcode(gcode
);
578 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
579 void Robot::reset_axis_position(float x
, float y
, float z
)
581 this->last_milestone
[X_AXIS
] = x
;
582 this->last_milestone
[Y_AXIS
] = y
;
583 this->last_milestone
[Z_AXIS
] = z
;
584 this->transformed_last_milestone
[X_AXIS
] = x
;
585 this->transformed_last_milestone
[Y_AXIS
] = y
;
586 this->transformed_last_milestone
[Z_AXIS
] = z
;
588 float actuator_pos
[3];
589 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
590 for (int i
= 0; i
< 3; i
++)
591 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
594 // Reset the position for an axis (used in homing and G92)
595 void Robot::reset_axis_position(float position
, int axis
)
597 this->last_milestone
[axis
] = position
;
598 this->transformed_last_milestone
[axis
] = position
;
600 float actuator_pos
[3];
601 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
603 for (int i
= 0; i
< 3; i
++)
604 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
607 // Use FK to find out where actuator is and reset lastmilestone to match
608 void Robot::reset_position_from_current_actuator_position()
610 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
611 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
614 // Convert target from millimeters to steps, and append this to the planner
615 void Robot::append_milestone( float target
[], float rate_mm_s
)
619 float actuator_pos
[3];
620 float transformed_target
[3]; // adjust target for bed compensation
621 float millimeters_of_travel
;
623 // unity transform by default
624 memcpy(transformed_target
, target
, sizeof(transformed_target
));
626 // check function pointer and call if set to transform the target to compensate for bed
627 if(compensationTransform
) {
628 // some compensation strategies can transform XYZ, some just change Z
629 compensationTransform(transformed_target
);
632 // find distance moved by each axis, use transformed target from last_transformed_target
633 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
634 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
636 // store last transformed
637 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
639 // Compute how long this move moves, so we can attach it to the block for later use
640 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
642 // find distance unit vector
643 for (int i
= 0; i
< 3; i
++)
644 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
646 // Do not move faster than the configured cartesian limits
647 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
648 if ( max_speeds
[axis
] > 0 ) {
649 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
651 if (axis_speed
> max_speeds
[axis
])
652 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
656 // find actuator position given cartesian position, use actual adjusted target
657 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
659 // check per-actuator speed limits
660 for (int actuator
= 0; actuator
<= 2; actuator
++) {
661 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
663 if (actuator_rate
> actuators
[actuator
]->max_rate
)
664 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
667 // Append the block to the planner
668 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
670 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
671 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
675 // Append a move to the queue ( cutting it into segments if needed )
676 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
679 // Find out the distance for this gcode
680 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 );
682 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
683 if( gcode
->millimeters_of_travel
< 1e-8F
) {
687 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
689 // Mark the gcode as having a known distance
690 this->distance_in_gcode_is_known( gcode
);
692 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
693 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
694 // 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
697 if(this->delta_segments_per_second
> 1.0F
) {
698 // enabled if set to something > 1, it is set to 0.0 by default
699 // segment based on current speed and requested segments per second
700 // the faster the travel speed the fewer segments needed
701 // NOTE rate is mm/sec and we take into account any speed override
702 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
703 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
704 // TODO if we are only moving in Z on a delta we don't really need to segment at all
707 if(this->mm_per_line_segment
== 0.0F
) {
708 segments
= 1; // don't split it up
710 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
715 // A vector to keep track of the endpoint of each segment
716 float segment_delta
[3];
717 float segment_end
[3];
719 // How far do we move each segment?
720 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
721 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
723 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
724 // We always add another point after this loop so we stop at segments-1, ie i < segments
725 for (int i
= 1; i
< segments
; i
++) {
726 if(halted
) return; // don;t queue any more segments
727 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
728 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
730 // Append the end of this segment to the queue
731 this->append_milestone(segment_end
, rate_mm_s
);
735 // Append the end of this full move to the queue
736 this->append_milestone(target
, rate_mm_s
);
738 // if adding these blocks didn't start executing, do that now
739 THEKERNEL
->conveyor
->ensure_running();
743 // Append an arc to the queue ( cutting it into segments as needed )
744 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
748 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
749 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
750 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
751 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
752 float r_axis1
= -offset
[this->plane_axis_1
];
753 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
754 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
756 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
757 float angular_travel
= atan2(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
758 if (angular_travel
< 0) {
759 angular_travel
+= 2 * M_PI
;
762 angular_travel
-= 2 * M_PI
;
765 // Find the distance for this gcode
766 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
768 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
769 if( gcode
->millimeters_of_travel
< 0.0001F
) {
773 // Mark the gcode as having a known distance
774 this->distance_in_gcode_is_known( gcode
);
776 // Figure out how many segments for this gcode
777 uint16_t segments
= floor(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
779 float theta_per_segment
= angular_travel
/ segments
;
780 float linear_per_segment
= linear_travel
/ segments
;
782 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
783 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
784 r_T = [cos(phi) -sin(phi);
785 sin(phi) cos(phi] * r ;
786 For arc generation, the center of the circle is the axis of rotation and the radius vector is
787 defined from the circle center to the initial position. Each line segment is formed by successive
788 vector rotations. This requires only two cos() and sin() computations to form the rotation
789 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
790 all float numbers are single precision on the Arduino. (True float precision will not have
791 round off issues for CNC applications.) Single precision error can accumulate to be greater than
792 tool precision in some cases. Therefore, arc path correction is implemented.
794 Small angle approximation may be used to reduce computation overhead further. This approximation
795 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
796 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
797 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
798 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
799 issue for CNC machines with the single precision Arduino calculations.
800 This approximation also allows mc_arc to immediately insert a line segment into the planner
801 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
802 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
803 This is important when there are successive arc motions.
805 // Vector rotation matrix values
806 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
807 float sin_T
= theta_per_segment
;
816 // Initialize the linear axis
817 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
819 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
820 if(halted
) return; // don't queue any more segments
822 if (count
< this->arc_correction
) {
823 // Apply vector rotation matrix
824 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
825 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
829 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
830 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
831 cos_Ti
= cosf(i
* theta_per_segment
);
832 sin_Ti
= sinf(i
* theta_per_segment
);
833 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
834 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
838 // Update arc_target location
839 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
840 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
841 arc_target
[this->plane_axis_2
] += linear_per_segment
;
843 // Append this segment to the queue
844 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
848 // Ensure last segment arrives at target location.
849 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
852 // Do the math for an arc and add it to the queue
853 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
857 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
859 // Set clockwise/counter-clockwise sign for mc_arc computations
860 bool is_clockwise
= false;
861 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
866 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
871 float Robot::theta(float x
, float y
)
873 float t
= atanf(x
/ fabs(y
));
885 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
887 this->plane_axis_0
= axis_0
;
888 this->plane_axis_1
= axis_1
;
889 this->plane_axis_2
= axis_2
;
892 void Robot::clearToolOffset()
894 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
897 void Robot::setToolOffset(const float offset
[3])
899 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));