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"
11 #include "mbed.h" // for us_ticker_read()
20 #include "nuts_bolts.h"
22 #include "StepperMotor.h"
24 #include "PublicDataRequest.h"
25 #include "PublicData.h"
26 #include "arm_solutions/BaseSolution.h"
27 #include "arm_solutions/CartesianSolution.h"
28 #include "arm_solutions/RotatableCartesianSolution.h"
29 #include "arm_solutions/LinearDeltaSolution.h"
30 #include "arm_solutions/RotatableDeltaSolution.h"
31 #include "arm_solutions/HBotSolution.h"
32 #include "arm_solutions/CoreXZSolution.h"
33 #include "arm_solutions/MorganSCARASolution.h"
34 #include "StepTicker.h"
35 #include "checksumm.h"
37 #include "ConfigValue.h"
38 #include "libs/StreamOutput.h"
39 #include "StreamOutputPool.h"
40 #include "ExtruderPublicAccess.h"
42 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
43 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
44 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
45 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
46 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
47 #define arc_correction_checksum CHECKSUM("arc_correction")
48 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
49 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
50 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
53 #define arm_solution_checksum CHECKSUM("arm_solution")
54 #define cartesian_checksum CHECKSUM("cartesian")
55 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
56 #define rostock_checksum CHECKSUM("rostock")
57 #define linear_delta_checksum CHECKSUM("linear_delta")
58 #define rotatable_delta_checksum CHECKSUM("rotatable_delta")
59 #define delta_checksum CHECKSUM("delta")
60 #define hbot_checksum CHECKSUM("hbot")
61 #define corexy_checksum CHECKSUM("corexy")
62 #define corexz_checksum CHECKSUM("corexz")
63 #define kossel_checksum CHECKSUM("kossel")
64 #define morgan_checksum CHECKSUM("morgan")
66 // stepper motor stuff
67 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
68 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
69 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
70 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
71 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
72 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
73 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
74 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
75 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
77 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
78 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
79 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
81 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
82 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
83 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
86 // new-style actuator stuff
87 #define actuator_checksum CHEKCSUM("actuator")
89 #define step_pin_checksum CHECKSUM("step_pin")
90 #define dir_pin_checksum CHEKCSUM("dir_pin")
91 #define en_pin_checksum CHECKSUM("en_pin")
93 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
94 #define max_rate_checksum CHECKSUM("max_rate")
96 #define alpha_checksum CHECKSUM("alpha")
97 #define beta_checksum CHECKSUM("beta")
98 #define gamma_checksum CHECKSUM("gamma")
100 #define NEXT_ACTION_DEFAULT 0
101 #define NEXT_ACTION_DWELL 1
102 #define NEXT_ACTION_GO_HOME 2
104 #define MOTION_MODE_SEEK 0 // G0
105 #define MOTION_MODE_LINEAR 1 // G1
106 #define MOTION_MODE_CW_ARC 2 // G2
107 #define MOTION_MODE_CCW_ARC 3 // G3
108 #define MOTION_MODE_CANCEL 4 // G80
110 #define PATH_CONTROL_MODE_EXACT_PATH 0
111 #define PATH_CONTROL_MODE_EXACT_STOP 1
112 #define PATH_CONTROL_MODE_CONTINOUS 2
114 #define PROGRAM_FLOW_RUNNING 0
115 #define PROGRAM_FLOW_PAUSED 1
116 #define PROGRAM_FLOW_COMPLETED 2
118 #define SPINDLE_DIRECTION_CW 0
119 #define SPINDLE_DIRECTION_CCW 1
121 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
123 // 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
124 // It takes care of cutting arcs into segments, same thing for line that are too long
128 this->inch_mode
= false;
129 this->absolute_mode
= true;
130 this->motion_mode
= MOTION_MODE_SEEK
;
131 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
132 clear_vector(this->last_milestone
);
133 clear_vector(this->transformed_last_milestone
);
134 this->arm_solution
= NULL
;
135 seconds_per_minute
= 60.0F
;
136 this->clearToolOffset();
137 this->compensationTransform
= nullptr;
140 //Called when the module has just been loaded
141 void Robot::on_module_loaded()
143 this->register_for_event(ON_GCODE_RECEIVED
);
146 this->on_config_reload(this);
149 void Robot::on_config_reload(void *argument
)
152 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
153 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
154 // To make adding those solution easier, they have their own, separate object.
155 // Here we read the config to find out which arm solution to use
156 if (this->arm_solution
) delete this->arm_solution
;
157 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
158 // Note checksums are not const expressions when in debug mode, so don't use switch
159 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
160 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== corexz_checksum
) {
163 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
166 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
169 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
171 } else if(solution_checksum
== rotatable_delta_checksum
) {
172 this->arm_solution
= new RotatableDeltaSolution(THEKERNEL
->config
);
175 } else if(solution_checksum
== morgan_checksum
) {
176 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
178 } else if(solution_checksum
== cartesian_checksum
) {
179 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
182 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
186 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
187 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
188 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
189 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
190 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
191 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
193 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
194 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
195 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
207 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
208 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
209 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
210 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
211 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
212 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
213 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
214 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
215 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
217 float steps_per_mm
[3] = {
218 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
219 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
220 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
223 // TODO: delete or detect old steppermotors
224 // Make our 3 StepperMotors
225 this->alpha_stepper_motor
= new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
);
226 this->beta_stepper_motor
= new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
);
227 this->gamma_stepper_motor
= new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
);
229 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
230 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
231 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
233 alpha_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
234 beta_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
235 gamma_stepper_motor
->set_max_rate(THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
);
236 check_max_actuator_speeds(); // check the configs are sane
239 actuators
.push_back(alpha_stepper_motor
);
240 actuators
.push_back(beta_stepper_motor
);
241 actuators
.push_back(gamma_stepper_motor
);
244 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
245 // so the first move can be correct if homing is not performed
246 float actuator_pos
[3];
247 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
248 for (int i
= 0; i
< 3; i
++)
249 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
251 //this->clearToolOffset();
254 void Robot::push_state()
256 bool am
= this->absolute_mode
;
257 bool im
= this->inch_mode
;
258 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, im
);
262 void Robot::pop_state()
264 if(!state_stack
.empty()) {
265 auto s
= state_stack
.top();
267 this->feed_rate
= std::get
<0>(s
);
268 this->seek_rate
= std::get
<1>(s
);
269 this->absolute_mode
= std::get
<2>(s
);
270 this->inch_mode
= std::get
<3>(s
);
274 // this does a sanity check that actuator speeds do not exceed steps rate capability
275 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
276 void Robot::check_max_actuator_speeds()
278 float step_freq
= alpha_stepper_motor
->get_max_rate() * alpha_stepper_motor
->get_steps_per_mm();
279 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
280 alpha_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ alpha_stepper_motor
->get_steps_per_mm()));
281 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
);
284 step_freq
= beta_stepper_motor
->get_max_rate() * beta_stepper_motor
->get_steps_per_mm();
285 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
286 beta_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ beta_stepper_motor
->get_steps_per_mm()));
287 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
);
290 step_freq
= gamma_stepper_motor
->get_max_rate() * gamma_stepper_motor
->get_steps_per_mm();
291 if(step_freq
> THEKERNEL
->base_stepping_frequency
) {
292 gamma_stepper_motor
->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ gamma_stepper_motor
->get_steps_per_mm()));
293 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
);
297 //A GCode has been received
298 //See if the current Gcode line has some orders for us
299 void Robot::on_gcode_received(void *argument
)
301 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
303 this->motion_mode
= -1;
305 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
308 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
309 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
310 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
311 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
313 uint32_t delay_ms
= 0;
314 if (gcode
->has_letter('P')) {
315 delay_ms
= gcode
->get_int('P');
317 if (gcode
->has_letter('S')) {
318 delay_ms
+= gcode
->get_int('S') * 1000;
322 THEKERNEL
->conveyor
->wait_for_empty_queue();
323 // wait for specified time
324 uint32_t start
= us_ticker_read(); // mbed call
325 while ((us_ticker_read() - start
) < delay_ms
*1000) {
326 THEKERNEL
->call_event(ON_IDLE
, this);
331 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
332 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
333 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
334 case 20: this->inch_mode
= true; break;
335 case 21: this->inch_mode
= false; break;
336 case 90: this->absolute_mode
= true; break;
337 case 91: this->absolute_mode
= false; break;
339 if(gcode
->get_num_args() == 0) {
340 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
341 reset_axis_position(0, i
);
345 for (char letter
= 'X'; letter
<= 'Z'; letter
++) {
346 if ( gcode
->has_letter(letter
) ) {
347 reset_axis_position(this->to_millimeters(gcode
->get_value(letter
)), letter
- 'X');
354 } else if( gcode
->has_m
) {
356 case 92: // M92 - set steps per mm
357 if (gcode
->has_letter('X'))
358 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
359 if (gcode
->has_letter('Y'))
360 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
361 if (gcode
->has_letter('Z'))
362 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
363 if (gcode
->has_letter('F'))
364 seconds_per_minute
= gcode
->get_value('F');
366 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
);
367 gcode
->add_nl
= true;
368 check_max_actuator_speeds();
373 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 ",
374 from_millimeters(this->last_milestone
[0]),
375 from_millimeters(this->last_milestone
[1]),
376 from_millimeters(this->last_milestone
[2]),
377 actuators
[X_AXIS
]->get_current_position(),
378 actuators
[Y_AXIS
]->get_current_position(),
379 actuators
[Z_AXIS
]->get_current_position() );
380 gcode
->txt_after_ok
.append(buf
, n
);
384 case 120: // push state
388 case 121: // pop state
392 case 203: // M203 Set maximum feedrates in mm/sec
393 if (gcode
->has_letter('X'))
394 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
395 if (gcode
->has_letter('Y'))
396 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
397 if (gcode
->has_letter('Z'))
398 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
399 if (gcode
->has_letter('A'))
400 alpha_stepper_motor
->set_max_rate(gcode
->get_value('A'));
401 if (gcode
->has_letter('B'))
402 beta_stepper_motor
->set_max_rate(gcode
->get_value('B'));
403 if (gcode
->has_letter('C'))
404 gamma_stepper_motor
->set_max_rate(gcode
->get_value('C'));
406 check_max_actuator_speeds();
408 if(gcode
->get_num_args() == 0) {
409 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
410 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
411 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
412 gcode
->add_nl
= true;
417 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
418 if (gcode
->has_letter('S')) {
419 float acc
= gcode
->get_value('S'); // mm/s^2
423 THEKERNEL
->planner
->acceleration
= acc
;
425 if (gcode
->has_letter('Z')) {
426 float acc
= gcode
->get_value('Z'); // mm/s^2
430 THEKERNEL
->planner
->z_acceleration
= acc
;
434 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
435 if (gcode
->has_letter('X')) {
436 float jd
= gcode
->get_value('X');
440 THEKERNEL
->planner
->junction_deviation
= jd
;
442 if (gcode
->has_letter('Z')) {
443 float jd
= gcode
->get_value('Z');
444 // enforce minimum, -1 disables it and uses regular junction deviation
447 THEKERNEL
->planner
->z_junction_deviation
= jd
;
449 if (gcode
->has_letter('S')) {
450 float mps
= gcode
->get_value('S');
454 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
456 if (gcode
->has_letter('Y')) {
457 alpha_stepper_motor
->default_minimum_actuator_rate
= gcode
->get_value('Y');
461 case 220: // M220 - speed override percentage
462 if (gcode
->has_letter('S')) {
463 float factor
= gcode
->get_value('S');
464 // enforce minimum 10% speed
467 // enforce maximum 10x speed
468 if (factor
> 1000.0F
)
471 seconds_per_minute
= 6000.0F
/ factor
;
473 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
477 case 400: // wait until all moves are done up to this point
478 THEKERNEL
->conveyor
->wait_for_empty_queue();
481 case 500: // M500 saves some volatile settings to config override file
482 case 503: { // M503 just prints the settings
483 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
);
484 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL
->planner
->acceleration
, THEKERNEL
->planner
->z_acceleration
);
485 gcode
->stream
->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL
->planner
->junction_deviation
, THEKERNEL
->planner
->z_junction_deviation
, THEKERNEL
->planner
->minimum_planner_speed
);
486 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",
487 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
488 alpha_stepper_motor
->get_max_rate(), beta_stepper_motor
->get_max_rate(), gamma_stepper_motor
->get_max_rate());
490 // get or save any arm solution specific optional values
491 BaseSolution::arm_options_t options
;
492 if(arm_solution
->get_optional(options
) && !options
.empty()) {
493 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
494 for(auto &i
: options
) {
495 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
497 gcode
->stream
->printf("\n");
503 case 665: { // M665 set optional arm solution variables based on arm solution.
504 // the parameter args could be any letter each arm solution only accepts certain ones
505 BaseSolution::arm_options_t options
= gcode
->get_args();
506 options
.erase('S'); // don't include the S
507 options
.erase('U'); // don't include the U
508 if(options
.size() > 0) {
509 // set the specified options
510 arm_solution
->set_optional(options
);
513 if(arm_solution
->get_optional(options
)) {
514 // foreach optional value
515 for(auto &i
: options
) {
516 // print all current values of supported options
517 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
518 gcode
->add_nl
= true;
522 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
523 this->delta_segments_per_second
= gcode
->get_value('S');
524 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
526 }else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
527 this->mm_per_line_segment
= gcode
->get_value('U');
528 this->delta_segments_per_second
= 0;
529 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
537 if( this->motion_mode
< 0)
541 float target
[3], offset
[3];
542 clear_vector(offset
);
544 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
546 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
547 if( gcode
->has_letter(letter
) ) {
548 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
551 for(char letter
= 'X'; letter
<= 'Z'; letter
++) {
552 if( gcode
->has_letter(letter
) ) {
553 target
[letter
- 'X'] = this->to_millimeters(gcode
->get_value(letter
)) + (this->absolute_mode
? this->toolOffset
[letter
- 'X'] : target
[letter
- 'X']);
557 if( gcode
->has_letter('F') ) {
558 if( this->motion_mode
== MOTION_MODE_SEEK
)
559 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
561 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
564 //Perform any physical actions
565 switch(this->motion_mode
) {
566 case MOTION_MODE_CANCEL
: break;
567 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
568 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
569 case MOTION_MODE_CW_ARC
:
570 case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
573 // last_milestone was set to target in append_milestone, no need to do it again
577 // We received a new gcode, and one of the functions
578 // determined the distance for that given gcode. So now we can attach this gcode to the right block
580 void Robot::distance_in_gcode_is_known(Gcode
*gcode
)
582 //If the queue is empty, execute immediatly, otherwise attach to the last added block
583 THEKERNEL
->conveyor
->append_gcode(gcode
);
586 // reset the position for all axis (used in homing for delta as last_milestone may be bogus)
587 void Robot::reset_axis_position(float x
, float y
, float z
)
589 this->last_milestone
[X_AXIS
] = x
;
590 this->last_milestone
[Y_AXIS
] = y
;
591 this->last_milestone
[Z_AXIS
] = z
;
592 this->transformed_last_milestone
[X_AXIS
] = x
;
593 this->transformed_last_milestone
[Y_AXIS
] = y
;
594 this->transformed_last_milestone
[Z_AXIS
] = z
;
596 float actuator_pos
[3];
597 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
598 for (int i
= 0; i
< 3; i
++)
599 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
602 // Reset the position for an axis (used in homing and G92)
603 void Robot::reset_axis_position(float position
, int axis
)
605 this->last_milestone
[axis
] = position
;
606 this->transformed_last_milestone
[axis
] = position
;
608 float actuator_pos
[3];
609 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
611 for (int i
= 0; i
< 3; i
++)
612 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
615 // Use FK to find out where actuator is and reset lastmilestone to match
616 void Robot::reset_position_from_current_actuator_position()
618 float actuator_pos
[]= {actuators
[X_AXIS
]->get_current_position(), actuators
[Y_AXIS
]->get_current_position(), actuators
[Z_AXIS
]->get_current_position()};
619 arm_solution
->actuator_to_cartesian(actuator_pos
, this->last_milestone
);
620 memcpy(this->transformed_last_milestone
, this->last_milestone
, sizeof(this->transformed_last_milestone
));
622 // now reset actuator correctly, NOTE this may lose a little precision
623 arm_solution
->cartesian_to_actuator(this->last_milestone
, actuator_pos
);
624 for (int i
= 0; i
< 3; i
++)
625 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
628 // Convert target from millimeters to steps, and append this to the planner
629 void Robot::append_milestone(Gcode
*gcode
, float target
[], float rate_mm_s
)
633 float actuator_pos
[3];
634 float transformed_target
[3]; // adjust target for bed compensation
635 float millimeters_of_travel
;
637 // unity transform by default
638 memcpy(transformed_target
, target
, sizeof(transformed_target
));
640 // check function pointer and call if set to transform the target to compensate for bed
641 if(compensationTransform
) {
642 // some compensation strategies can transform XYZ, some just change Z
643 compensationTransform(transformed_target
);
646 // find distance moved by each axis, use transformed target from last_transformed_target
647 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++){
648 deltas
[axis
] = transformed_target
[axis
] - transformed_last_milestone
[axis
];
650 // store last transformed
651 memcpy(this->transformed_last_milestone
, transformed_target
, sizeof(this->transformed_last_milestone
));
653 // Compute how long this move moves, so we can attach it to the block for later use
654 millimeters_of_travel
= sqrtf( powf( deltas
[X_AXIS
], 2 ) + powf( deltas
[Y_AXIS
], 2 ) + powf( deltas
[Z_AXIS
], 2 ) );
656 // find distance unit vector
657 for (int i
= 0; i
< 3; i
++)
658 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
660 // Do not move faster than the configured cartesian limits
661 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
662 if ( max_speeds
[axis
] > 0 ) {
663 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
665 if (axis_speed
> max_speeds
[axis
])
666 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
670 // find actuator position given cartesian position, use actual adjusted target
671 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
673 float isecs
= rate_mm_s
/ millimeters_of_travel
;
674 // check per-actuator speed limits
675 for (int actuator
= 0; actuator
<= 2; actuator
++) {
676 float actuator_rate
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * isecs
;
677 if (actuator_rate
> actuators
[actuator
]->get_max_rate()){
678 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
679 isecs
= rate_mm_s
/ millimeters_of_travel
;
683 // Append the block to the planner
684 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
686 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
687 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
691 // Append a move to the queue ( cutting it into segments if needed )
692 void Robot::append_line(Gcode
*gcode
, float target
[], float rate_mm_s
)
694 // Find out the distance for this gcode
695 // NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
696 gcode
->millimeters_of_travel
= sqrtf(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 ));
698 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
699 if( gcode
->millimeters_of_travel
< 0.00001F
) {
703 // Mark the gcode as having a known distance
704 this->distance_in_gcode_is_known( gcode
);
706 // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
707 // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
708 // We also check we are not exceeding the E max_speed for the current extruder
709 // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
710 // we need to ask it now passing in the relevant data.
711 // NOTE we need to do this before we segment the line (for deltas)
712 if(gcode
->has_letter('E')) {
714 data
[0]= gcode
->get_value('E'); // E target (maybe absolute or relative)
715 data
[1]= rate_mm_s
/ gcode
->millimeters_of_travel
; // inverted seconds for the move
716 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
717 rate_mm_s
*= data
[1];
718 //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
722 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
723 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
724 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
725 // 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
728 if(this->delta_segments_per_second
> 1.0F
) {
729 // enabled if set to something > 1, it is set to 0.0 by default
730 // segment based on current speed and requested segments per second
731 // the faster the travel speed the fewer segments needed
732 // NOTE rate is mm/sec and we take into account any speed override
733 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
734 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
735 // TODO if we are only moving in Z on a delta we don't really need to segment at all
738 if(this->mm_per_line_segment
== 0.0F
) {
739 segments
= 1; // don't split it up
741 segments
= ceilf( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
746 // A vector to keep track of the endpoint of each segment
747 float segment_delta
[3];
748 float segment_end
[3];
750 // How far do we move each segment?
751 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
752 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
754 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
755 // We always add another point after this loop so we stop at segments-1, ie i < segments
756 for (int i
= 1; i
< segments
; i
++) {
757 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
758 for(int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++ )
759 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
761 // Append the end of this segment to the queue
762 this->append_milestone(gcode
, segment_end
, rate_mm_s
);
766 // Append the end of this full move to the queue
767 this->append_milestone(gcode
, target
, rate_mm_s
);
769 // if adding these blocks didn't start executing, do that now
770 THEKERNEL
->conveyor
->ensure_running();
774 // Append an arc to the queue ( cutting it into segments as needed )
775 void Robot::append_arc(Gcode
*gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
)
779 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
780 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
781 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
782 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
783 float r_axis1
= -offset
[this->plane_axis_1
];
784 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
785 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
787 // Patch from GRBL Firmware - Christoph Baumann 04072015
788 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
789 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
790 if (is_clockwise
) { // Correct atan2 output per direction
791 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= 2*M_PI
; }
793 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= 2*M_PI
; }
796 // Find the distance for this gcode
797 gcode
->millimeters_of_travel
= hypotf(angular_travel
* radius
, fabs(linear_travel
));
799 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
800 if( gcode
->millimeters_of_travel
< 0.00001F
) {
804 // Mark the gcode as having a known distance
805 this->distance_in_gcode_is_known( gcode
);
807 // Figure out how many segments for this gcode
808 uint16_t segments
= floorf(gcode
->millimeters_of_travel
/ this->mm_per_arc_segment
);
810 float theta_per_segment
= angular_travel
/ segments
;
811 float linear_per_segment
= linear_travel
/ segments
;
813 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
814 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
815 r_T = [cos(phi) -sin(phi);
816 sin(phi) cos(phi] * r ;
817 For arc generation, the center of the circle is the axis of rotation and the radius vector is
818 defined from the circle center to the initial position. Each line segment is formed by successive
819 vector rotations. This requires only two cos() and sin() computations to form the rotation
820 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
821 all float numbers are single precision on the Arduino. (True float precision will not have
822 round off issues for CNC applications.) Single precision error can accumulate to be greater than
823 tool precision in some cases. Therefore, arc path correction is implemented.
825 Small angle approximation may be used to reduce computation overhead further. This approximation
826 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
827 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
828 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
829 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
830 issue for CNC machines with the single precision Arduino calculations.
831 This approximation also allows mc_arc to immediately insert a line segment into the planner
832 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
833 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
834 This is important when there are successive arc motions.
836 // Vector rotation matrix values
837 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
838 float sin_T
= theta_per_segment
;
847 // Initialize the linear axis
848 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
850 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
851 if(THEKERNEL
->is_halted()) return; // don't queue any more segments
853 if (count
< this->arc_correction
) {
854 // Apply vector rotation matrix
855 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
856 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
860 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
861 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
862 cos_Ti
= cosf(i
* theta_per_segment
);
863 sin_Ti
= sinf(i
* theta_per_segment
);
864 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
865 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
869 // Update arc_target location
870 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
871 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
872 arc_target
[this->plane_axis_2
] += linear_per_segment
;
874 // Append this segment to the queue
875 this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
879 // Ensure last segment arrives at target location.
880 this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
);
883 // Do the math for an arc and add it to the queue
884 void Robot::compute_arc(Gcode
*gcode
, float offset
[], float target
[])
888 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
890 // Set clockwise/counter-clockwise sign for mc_arc computations
891 bool is_clockwise
= false;
892 if( this->motion_mode
== MOTION_MODE_CW_ARC
) {
897 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
902 float Robot::theta(float x
, float y
)
904 float t
= atanf(x
/ fabs(y
));
916 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
918 this->plane_axis_0
= axis_0
;
919 this->plane_axis_1
= axis_1
;
920 this->plane_axis_2
= axis_2
;
923 void Robot::clearToolOffset()
925 memset(this->toolOffset
, 0, sizeof(this->toolOffset
));
928 void Robot::setToolOffset(const float offset
[3])
930 memcpy(this->toolOffset
, offset
, sizeof(this->toolOffset
));