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/RostockSolution.h"
28 #include "arm_solutions/JohannKosselSolution.h"
29 #include "arm_solutions/HBotSolution.h"
30 #include "StepTicker.h"
31 #include "checksumm.h"
33 #include "ConfigValue.h"
34 #include "libs/StreamOutput.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 delta_checksum CHECKSUM("delta")
52 #define hbot_checksum CHECKSUM("hbot")
53 #define corexy_checksum CHECKSUM("corexy")
54 #define kossel_checksum CHECKSUM("kossel")
56 // stepper motor stuff
57 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
58 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
59 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
60 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
61 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
62 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
63 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
64 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
65 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
67 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
68 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
69 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
71 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
72 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
73 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
76 // new-style actuator stuff
77 #define actuator_checksum CHEKCSUM("actuator")
79 #define step_pin_checksum CHECKSUM("step_pin")
80 #define dir_pin_checksum CHEKCSUM("dir_pin")
81 #define en_pin_checksum CHECKSUM("en_pin")
83 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
84 #define max_rate_checksum CHECKSUM("max_rate")
86 #define alpha_checksum CHECKSUM("alpha")
87 #define beta_checksum CHECKSUM("beta")
88 #define gamma_checksum CHECKSUM("gamma")
91 // 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
92 // It takes care of cutting arcs into segments, same thing for line that are too long
93 #define max(a,b) (((a) > (b)) ? (a) : (b))
96 this->inch_mode
= false;
97 this->absolute_mode
= true;
98 this->motion_mode
= MOTION_MODE_SEEK
;
99 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
100 clear_vector(this->last_milestone
);
101 this->arm_solution
= NULL
;
102 seconds_per_minute
= 60.0F
;
105 //Called when the module has just been loaded
106 void Robot::on_module_loaded() {
107 register_for_event(ON_CONFIG_RELOAD
);
108 this->register_for_event(ON_GCODE_RECEIVED
);
109 this->register_for_event(ON_GET_PUBLIC_DATA
);
110 this->register_for_event(ON_SET_PUBLIC_DATA
);
113 this->on_config_reload(this);
116 void Robot::on_config_reload(void* argument
){
118 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
119 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
120 // To make adding those solution easier, they have their own, separate object.
121 // Here we read the config to find out which arm solution to use
122 if (this->arm_solution
) delete this->arm_solution
;
123 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
124 // Note checksums are not const expressions when in debug mode, so don't use switch
125 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
126 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
128 }else if(solution_checksum
== rostock_checksum
) {
129 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
131 }else if(solution_checksum
== kossel_checksum
) {
132 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
134 }else if(solution_checksum
== delta_checksum
) {
135 // place holder for now
136 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
138 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
139 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
141 }else if(solution_checksum
== cartesian_checksum
) {
142 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
145 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
149 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
150 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
151 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
152 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
153 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
154 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
156 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
157 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
158 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
170 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
171 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
172 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
173 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
174 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
175 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
176 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
177 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
178 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
180 float steps_per_mm
[3] = {
181 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
182 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
183 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
186 // TODO: delete or detect old steppermotors
187 // Make our 3 StepperMotors
188 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
189 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
190 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
192 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
193 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
194 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
196 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
197 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
198 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
201 actuators
.push_back(alpha_stepper_motor
);
202 actuators
.push_back(beta_stepper_motor
);
203 actuators
.push_back(gamma_stepper_motor
);
205 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
206 // so the first move can be correct if homing is not performed
207 float actuator_pos
[3];
208 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
209 for (int i
= 0; i
< 3; i
++)
210 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
213 void Robot::on_get_public_data(void* argument
){
214 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
216 if(!pdr
->starts_with(robot_checksum
)) return;
218 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
219 static float return_data
;
220 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
221 pdr
->set_data_ptr(&return_data
);
224 }else if(pdr
->second_element_is(current_position_checksum
)) {
225 static float return_data
[3];
226 return_data
[0]= from_millimeters(this->last_milestone
[0]);
227 return_data
[1]= from_millimeters(this->last_milestone
[1]);
228 return_data
[2]= from_millimeters(this->last_milestone
[2]);
230 pdr
->set_data_ptr(&return_data
);
235 void Robot::on_set_public_data(void* argument
){
236 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
238 if(!pdr
->starts_with(robot_checksum
)) return;
240 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
241 // NOTE do not use this while printing!
242 float t
= *static_cast<float*>(pdr
->get_data_ptr());
243 // enforce minimum 10% speed
244 if (t
< 10.0F
) t
= 10.0F
;
246 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
251 //A GCode has been received
252 //See if the current Gcode line has some orders for us
253 void Robot::on_gcode_received(void * argument
){
254 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
256 //Temp variables, constant properties are stored in the object
257 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
258 this->motion_mode
= -1;
260 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
263 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
264 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
265 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
266 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
267 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
268 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
269 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
270 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
271 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
272 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
273 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
275 if(gcode
->get_num_args() == 0){
276 clear_vector(this->last_milestone
);
278 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
279 if ( gcode
->has_letter(letter
) )
280 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
284 // TODO: handle any number of actuators
285 float actuator_pos
[3];
286 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
288 for (int i
= 0; i
< 3; i
++)
289 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
291 gcode
->mark_as_taken();
295 }else if( gcode
->has_m
){
297 case 92: // M92 - set steps per mm
298 if (gcode
->has_letter('X'))
299 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
300 if (gcode
->has_letter('Y'))
301 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
302 if (gcode
->has_letter('Z'))
303 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
304 if (gcode
->has_letter('F'))
305 seconds_per_minute
= gcode
->get_value('F');
307 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
);
308 gcode
->add_nl
= true;
309 gcode
->mark_as_taken();
314 int n
= snprintf(buf
, sizeof(buf
), "C: X:%1.3f Y:%1.3f Z:%1.3f",
315 from_millimeters(this->last_milestone
[0]),
316 from_millimeters(this->last_milestone
[1]),
317 from_millimeters(this->last_milestone
[2]));
318 gcode
->txt_after_ok
.append(buf
, n
);
319 gcode
->mark_as_taken();
323 case 203: // M203 Set maximum feedrates in mm/sec
324 if (gcode
->has_letter('X'))
325 this->max_speeds
[X_AXIS
]= gcode
->get_value('X');
326 if (gcode
->has_letter('Y'))
327 this->max_speeds
[Y_AXIS
]= gcode
->get_value('Y');
328 if (gcode
->has_letter('Z'))
329 this->max_speeds
[Z_AXIS
]= gcode
->get_value('Z');
330 if (gcode
->has_letter('A'))
331 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
332 if (gcode
->has_letter('B'))
333 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
334 if (gcode
->has_letter('C'))
335 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
337 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
338 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
339 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
340 gcode
->add_nl
= true;
341 gcode
->mark_as_taken();
344 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
345 gcode
->mark_as_taken();
347 if (gcode
->has_letter('S'))
349 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
350 THEKERNEL
->conveyor
->wait_for_empty_queue();
351 float acc
= gcode
->get_value('S'); // mm/s^2
355 THEKERNEL
->planner
->acceleration
= acc
;
359 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
360 gcode
->mark_as_taken();
361 if (gcode
->has_letter('X'))
363 float jd
= gcode
->get_value('X');
367 THEKERNEL
->planner
->junction_deviation
= jd
;
369 if (gcode
->has_letter('S'))
371 float mps
= gcode
->get_value('S');
375 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
379 case 220: // M220 - speed override percentage
380 gcode
->mark_as_taken();
381 if (gcode
->has_letter('S'))
383 float factor
= gcode
->get_value('S');
384 // enforce minimum 10% speed
387 // enforce maximum 10x speed
388 if (factor
> 1000.0F
)
391 seconds_per_minute
= 6000.0F
/ factor
;
395 case 400: // wait until all moves are done up to this point
396 gcode
->mark_as_taken();
397 THEKERNEL
->conveyor
->wait_for_empty_queue();
400 case 500: // M500 saves some volatile settings to config override file
401 case 503: { // M503 just prints the settings
402 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
);
403 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
404 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
);
405 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",
406 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
407 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
409 // get or save any arm solution specific optional values
410 BaseSolution::arm_options_t options
;
411 if(arm_solution
->get_optional(options
) && !options
.empty()) {
412 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
413 for(auto& i
: options
) {
414 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
416 gcode
->stream
->printf("\n");
418 gcode
->mark_as_taken();
422 case 665: { // M665 set optional arm solution variables based on arm solution.
423 gcode
->mark_as_taken();
424 // the parameter args could be any letter except S so ask solution what options it supports
425 BaseSolution::arm_options_t options
;
426 if(arm_solution
->get_optional(options
)) {
427 for(auto& i
: options
) {
428 // foreach optional value
430 if(gcode
->has_letter(c
)) { // set new value
431 i
.second
= gcode
->get_value(c
);
433 // print all current values of supported options
434 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
435 gcode
->add_nl
= true;
437 // set the new options
438 arm_solution
->set_optional(options
);
441 // set delta segments per second, not saved by M500
442 if(gcode
->has_letter('S')) {
443 this->delta_segments_per_second
= gcode
->get_value('S');
450 if( this->motion_mode
< 0)
454 float target
[3], offset
[3];
455 clear_vector(offset
);
457 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
459 for(char letter
= 'I'; letter
<= 'K'; letter
++){
460 if( gcode
->has_letter(letter
) ){
461 offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
));
464 for(char letter
= 'X'; letter
<= 'Z'; letter
++){
465 if( gcode
->has_letter(letter
) ){
466 target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']);
470 if( gcode
->has_letter('F') )
472 if( this->motion_mode
== MOTION_MODE_SEEK
)
473 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
475 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
478 //Perform any physical actions
479 switch( next_action
){
480 case NEXT_ACTION_DEFAULT
:
481 switch(this->motion_mode
){
482 case MOTION_MODE_CANCEL
: break;
483 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
484 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
485 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
490 // As far as the parser is concerned, the position is now == target. In reality the
491 // motion control system might still be processing the action and the real tool position
492 // in any intermediate location.
493 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->position[] = target[];
497 // We received a new gcode, and one of the functions
498 // determined the distance for that given gcode. So now we can attach this gcode to the right block
500 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
502 //If the queue is empty, execute immediatly, otherwise attach to the last added block
503 THEKERNEL
->conveyor
->append_gcode(gcode
);
506 // Reset the position for all axes ( used in homing and G92 stuff )
507 void Robot::reset_axis_position(float position
, int axis
) {
508 this->last_milestone
[axis
] = position
;
510 float actuator_pos
[3];
511 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
513 for (int i
= 0; i
< 3; i
++)
514 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
518 // Convert target from millimeters to steps, and append this to the planner
519 void Robot::append_milestone( float target
[], float rate_mm_s
)
523 float actuator_pos
[3];
524 float millimeters_of_travel
;
526 // find distance moved by each axis
527 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
528 deltas
[axis
] = target
[axis
] - last_milestone
[axis
];
530 // Compute how long this move moves, so we can attach it to the block for later use
531 millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
533 // find distance unit vector
534 for (int i
= 0; i
< 3; i
++)
535 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
537 // Do not move faster than the configured cartesian limits
538 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
540 if ( max_speeds
[axis
] > 0 )
542 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
544 if (axis_speed
> max_speeds
[axis
])
545 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
549 // find actuator position given cartesian position
550 arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
552 // check per-actuator speed limits
553 for (int actuator
= 0; actuator
<= 2; actuator
++)
555 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
557 if (actuator_rate
> actuators
[actuator
]->max_rate
)
558 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
561 // Append the block to the planner
562 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
564 // Update the last_milestone to the current target for the next time we use last_milestone
565 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
569 // Append a move to the queue ( cutting it into segments if needed )
570 void Robot::append_line(Gcode
* gcode
, float target
[], float rate_mm_s
){
572 // Find out the distance for this gcode
573 gcode
->millimeters_of_travel
= pow( target
[X_AXIS
]-this->last_milestone
[X_AXIS
], 2 ) + pow( target
[Y_AXIS
]-this->last_milestone
[Y_AXIS
], 2 ) + pow( target
[Z_AXIS
]-this->last_milestone
[Z_AXIS
], 2 );
575 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
576 if( gcode
->millimeters_of_travel
< 1e-8F
){
580 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
582 // Mark the gcode as having a known distance
583 this->distance_in_gcode_is_known( gcode
);
585 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
586 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
587 // 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
590 if(this->delta_segments_per_second
> 1.0F
) {
591 // enabled if set to something > 1, it is set to 0.0 by default
592 // segment based on current speed and requested segments per second
593 // the faster the travel speed the fewer segments needed
594 // NOTE rate is mm/sec and we take into account any speed override
595 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
596 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
597 // TODO if we are only moving in Z on a delta we don't really need to segment at all
600 if(this->mm_per_line_segment
== 0.0F
){
601 segments
= 1; // don't split it up
603 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
609 // A vector to keep track of the endpoint of each segment
610 float segment_delta
[3];
611 float segment_end
[3];
613 // How far do we move each segment?
614 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
615 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
617 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
618 // We always add another point after this loop so we stop at segments-1, ie i < segments
619 for (int i
= 1; i
< segments
; i
++)
621 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ )
622 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
624 // Append the end of this segment to the queue
625 this->append_milestone(segment_end
, rate_mm_s
);
629 // Append the end of this full move to the queue
630 this->append_milestone(target
, rate_mm_s
);
632 // if adding these blocks didn't start executing, do that now
633 THEKERNEL
->conveyor
->ensure_running();
637 // Append an arc to the queue ( cutting it into segments as needed )
638 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
641 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
642 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
643 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
644 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
645 float r_axis1
= -offset
[this->plane_axis_1
];
646 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
647 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
649 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
650 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
651 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
652 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
654 // Find the distance for this gcode
655 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
657 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
658 if( gcode
->millimeters_of_travel
< 0.0001F
){ return; }
660 // Mark the gcode as having a known distance
661 this->distance_in_gcode_is_known( gcode
);
663 // Figure out how many segments for this gcode
664 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
666 float theta_per_segment
= angular_travel
/segments
;
667 float linear_per_segment
= linear_travel
/segments
;
669 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
670 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
671 r_T = [cos(phi) -sin(phi);
672 sin(phi) cos(phi] * r ;
673 For arc generation, the center of the circle is the axis of rotation and the radius vector is
674 defined from the circle center to the initial position. Each line segment is formed by successive
675 vector rotations. This requires only two cos() and sin() computations to form the rotation
676 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
677 all float numbers are single precision on the Arduino. (True float precision will not have
678 round off issues for CNC applications.) Single precision error can accumulate to be greater than
679 tool precision in some cases. Therefore, arc path correction is implemented.
681 Small angle approximation may be used to reduce computation overhead further. This approximation
682 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
683 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
684 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
685 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
686 issue for CNC machines with the single precision Arduino calculations.
687 This approximation also allows mc_arc to immediately insert a line segment into the planner
688 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
689 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
690 This is important when there are successive arc motions.
692 // Vector rotation matrix values
693 float cos_T
= 1-0.5F
*theta_per_segment
*theta_per_segment
; // Small angle approximation
694 float sin_T
= theta_per_segment
;
703 // Initialize the linear axis
704 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
706 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
708 if (count
< this->arc_correction
) {
709 // Apply vector rotation matrix
710 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
711 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
715 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
716 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
717 cos_Ti
= cosf(i
*theta_per_segment
);
718 sin_Ti
= sinf(i
*theta_per_segment
);
719 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
720 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
724 // Update arc_target location
725 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
726 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
727 arc_target
[this->plane_axis_2
] += linear_per_segment
;
729 // Append this segment to the queue
730 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
734 // Ensure last segment arrives at target location.
735 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
738 // Do the math for an arc and add it to the queue
739 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
742 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
744 // Set clockwise/counter-clockwise sign for mc_arc computations
745 bool is_clockwise
= false;
746 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
749 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
754 float Robot::theta(float x
, float y
){
755 float t
= atanf(x
/fabs(y
));
756 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
759 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
760 this->plane_axis_0
= axis_0
;
761 this->plane_axis_1
= axis_1
;
762 this->plane_axis_2
= axis_2
;