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 "arm_solutions/BaseSolution.h"
24 #include "arm_solutions/CartesianSolution.h"
25 #include "arm_solutions/RotatableCartesianSolution.h"
26 #include "arm_solutions/RostockSolution.h"
27 #include "arm_solutions/JohannKosselSolution.h"
28 #include "arm_solutions/HBotSolution.h"
29 #include "StepTicker.h"
31 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
32 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
33 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
34 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
35 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
36 #define arc_correction_checksum CHECKSUM("arc_correction")
37 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
38 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
39 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
42 #define arm_solution_checksum CHECKSUM("arm_solution")
43 #define cartesian_checksum CHECKSUM("cartesian")
44 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
45 #define rostock_checksum CHECKSUM("rostock")
46 #define delta_checksum CHECKSUM("delta")
47 #define hbot_checksum CHECKSUM("hbot")
48 #define corexy_checksum CHECKSUM("corexy")
49 #define kossel_checksum CHECKSUM("kossel")
51 // stepper motor stuff
52 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
53 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
54 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
55 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
56 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
57 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
58 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
59 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
60 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
62 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
63 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
64 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
66 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
67 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
68 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
71 // new-style actuator stuff
72 #define actuator_checksum CHEKCSUM("actuator")
74 #define step_pin_checksum CHECKSUM("step_pin")
75 #define dir_pin_checksum CHEKCSUM("dir_pin")
76 #define en_pin_checksum CHECKSUM("en_pin")
78 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
79 #define max_rate_checksum CHECKSUM("max_rate")
81 #define alpha_checksum CHECKSUM("alpha")
82 #define beta_checksum CHECKSUM("beta")
83 #define gamma_checksum CHECKSUM("gamma")
86 // 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
87 // It takes care of cutting arcs into segments, same thing for line that are too long
88 #define max(a,b) (((a) > (b)) ? (a) : (b))
91 this->inch_mode
= false;
92 this->absolute_mode
= true;
93 this->motion_mode
= MOTION_MODE_SEEK
;
94 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
95 clear_vector(this->last_milestone
);
96 this->arm_solution
= NULL
;
97 seconds_per_minute
= 60.0F
;
100 //Called when the module has just been loaded
101 void Robot::on_module_loaded() {
102 register_for_event(ON_CONFIG_RELOAD
);
103 this->register_for_event(ON_GCODE_RECEIVED
);
104 this->register_for_event(ON_GET_PUBLIC_DATA
);
105 this->register_for_event(ON_SET_PUBLIC_DATA
);
108 this->on_config_reload(this);
111 void Robot::on_config_reload(void* argument
){
113 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
114 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
115 // To make adding those solution easier, they have their own, separate object.
116 // Here we read the config to find out which arm solution to use
117 if (this->arm_solution
) delete this->arm_solution
;
118 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
119 // Note checksums are not const expressions when in debug mode, so don't use switch
120 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
121 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
123 }else if(solution_checksum
== rostock_checksum
) {
124 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
126 }else if(solution_checksum
== kossel_checksum
) {
127 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
129 }else if(solution_checksum
== delta_checksum
) {
130 // place holder for now
131 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
133 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
134 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
136 }else if(solution_checksum
== cartesian_checksum
) {
137 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
140 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
144 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
145 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
146 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
147 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
148 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
149 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
151 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
152 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
153 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
165 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
166 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
167 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
168 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
169 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
170 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
171 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
172 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
173 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
175 float steps_per_mm
[3] = {
176 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
177 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
178 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
181 // TODO: delete or detect old steppermotors
182 // Make our 3 StepperMotors
183 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
184 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
185 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
187 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
188 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
189 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
191 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
192 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
193 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
196 actuators
.push_back(alpha_stepper_motor
);
197 actuators
.push_back(beta_stepper_motor
);
198 actuators
.push_back(gamma_stepper_motor
);
200 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
201 // so the first move can be correct if homing is not performed
202 float actuator_pos
[3];
203 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
204 for (int i
= 0; i
< 3; i
++)
205 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
208 void Robot::on_get_public_data(void* argument
){
209 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
211 if(!pdr
->starts_with(robot_checksum
)) return;
213 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
214 static float return_data
;
215 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
216 pdr
->set_data_ptr(&return_data
);
219 }else if(pdr
->second_element_is(current_position_checksum
)) {
220 static float return_data
[3];
221 return_data
[0]= from_millimeters(this->last_milestone
[0]);
222 return_data
[1]= from_millimeters(this->last_milestone
[1]);
223 return_data
[2]= from_millimeters(this->last_milestone
[2]);
225 pdr
->set_data_ptr(&return_data
);
230 void Robot::on_set_public_data(void* argument
){
231 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
233 if(!pdr
->starts_with(robot_checksum
)) return;
235 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
236 // NOTE do not use this while printing!
237 float t
= *static_cast<float*>(pdr
->get_data_ptr());
238 // enforce minimum 10% speed
239 if (t
< 10.0F
) t
= 10.0F
;
241 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
246 //A GCode has been received
247 //See if the current Gcode line has some orders for us
248 void Robot::on_gcode_received(void * argument
){
249 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
251 //Temp variables, constant properties are stored in the object
252 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
253 this->motion_mode
= -1;
255 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
258 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
259 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
260 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
261 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
262 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
263 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
264 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
265 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
266 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
267 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
268 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
270 if(gcode
->get_num_args() == 0){
271 clear_vector(this->last_milestone
);
273 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
274 if ( gcode
->has_letter(letter
) )
275 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
279 // TODO: handle any number of actuators
280 float actuator_pos
[3];
281 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
283 for (int i
= 0; i
< 3; i
++)
284 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
286 gcode
->mark_as_taken();
290 }else if( gcode
->has_m
){
292 case 92: // M92 - set steps per mm
293 if (gcode
->has_letter('X'))
294 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
295 if (gcode
->has_letter('Y'))
296 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
297 if (gcode
->has_letter('Z'))
298 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
299 if (gcode
->has_letter('F'))
300 seconds_per_minute
= gcode
->get_value('F');
302 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
);
303 gcode
->add_nl
= true;
304 gcode
->mark_as_taken();
306 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
307 from_millimeters(this->last_milestone
[0]),
308 from_millimeters(this->last_milestone
[1]),
309 from_millimeters(this->last_milestone
[2]));
310 gcode
->add_nl
= true;
311 gcode
->mark_as_taken();
314 case 203: // M203 Set maximum feedrates in mm/sec
315 if (gcode
->has_letter('X'))
316 this->max_speeds
[X_AXIS
]= gcode
->get_value('X');
317 if (gcode
->has_letter('Y'))
318 this->max_speeds
[Y_AXIS
]= gcode
->get_value('Y');
319 if (gcode
->has_letter('Z'))
320 this->max_speeds
[Z_AXIS
]= gcode
->get_value('Z');
321 if (gcode
->has_letter('A'))
322 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
323 if (gcode
->has_letter('B'))
324 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
325 if (gcode
->has_letter('C'))
326 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
328 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
329 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
330 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
331 gcode
->add_nl
= true;
332 gcode
->mark_as_taken();
335 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
336 gcode
->mark_as_taken();
338 if (gcode
->has_letter('S'))
340 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
341 THEKERNEL
->conveyor
->wait_for_empty_queue();
342 float acc
= gcode
->get_value('S'); // mm/s^2
346 THEKERNEL
->planner
->acceleration
= acc
;
350 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
351 gcode
->mark_as_taken();
352 if (gcode
->has_letter('X'))
354 float jd
= gcode
->get_value('X');
358 THEKERNEL
->planner
->junction_deviation
= jd
;
360 if (gcode
->has_letter('S'))
362 float mps
= gcode
->get_value('S');
366 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
370 case 220: // M220 - speed override percentage
371 gcode
->mark_as_taken();
372 if (gcode
->has_letter('S'))
374 float factor
= gcode
->get_value('S');
375 // enforce minimum 10% speed
378 // enforce maximum 10x speed
379 if (factor
> 1000.0F
)
382 seconds_per_minute
= 6000.0F
/ factor
;
386 case 400: // wait until all moves are done up to this point
387 gcode
->mark_as_taken();
388 THEKERNEL
->conveyor
->wait_for_empty_queue();
391 case 500: // M500 saves some volatile settings to config override file
392 case 503: // M503 just prints the settings
393 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
);
394 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
395 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
);
396 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",
397 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
398 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
399 gcode
->mark_as_taken();
402 case 665: // M665 set optional arm solution variables based on arm solution. NOTE these are not saved with M500
403 gcode
->mark_as_taken();
404 // the parameter args could be any letter except S so try each one
405 for(char c
='A';c
<='Z';c
++) {
406 if(c
== 'S') continue; // used for segments per second
408 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
410 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
411 v
= gcode
->get_value(c
);
412 arm_solution
->set_optional(c
, v
);
414 if(supported
) { // print all current values of supported options
415 gcode
->stream
->printf("%c %8.3f ", c
, v
);
416 gcode
->add_nl
= true;
419 // set delta segments per second
420 if(gcode
->has_letter('S')) {
421 this->delta_segments_per_second
= gcode
->get_value('S');
427 if( this->motion_mode
< 0)
431 float target
[3], offset
[3];
432 clear_vector(offset
);
434 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
436 for(char letter
= 'I'; letter
<= 'K'; letter
++){
437 if( gcode
->has_letter(letter
) ){
438 offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
));
441 for(char letter
= 'X'; letter
<= 'Z'; letter
++){
442 if( gcode
->has_letter(letter
) ){
443 target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']);
447 if( gcode
->has_letter('F') )
449 if( this->motion_mode
== MOTION_MODE_SEEK
)
450 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
452 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
455 //Perform any physical actions
456 switch( next_action
){
457 case NEXT_ACTION_DEFAULT
:
458 switch(this->motion_mode
){
459 case MOTION_MODE_CANCEL
: break;
460 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
461 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
462 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
467 // As far as the parser is concerned, the position is now == target. In reality the
468 // motion control system might still be processing the action and the real tool position
469 // in any intermediate location.
470 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->position[] = target[];
474 // We received a new gcode, and one of the functions
475 // determined the distance for that given gcode. So now we can attach this gcode to the right block
477 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
479 //If the queue is empty, execute immediatly, otherwise attach to the last added block
480 THEKERNEL
->conveyor
->append_gcode(gcode
);
483 // Reset the position for all axes ( used in homing and G92 stuff )
484 void Robot::reset_axis_position(float position
, int axis
) {
485 this->last_milestone
[axis
] = position
;
487 float actuator_pos
[3];
488 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
490 for (int i
= 0; i
< 3; i
++)
491 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
495 // Convert target from millimeters to steps, and append this to the planner
496 void Robot::append_milestone( float target
[], float rate_mm_s
)
500 float actuator_pos
[3];
501 float millimeters_of_travel
;
503 // find distance moved by each axis
504 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
505 deltas
[axis
] = target
[axis
] - last_milestone
[axis
];
507 // Compute how long this move moves, so we can attach it to the block for later use
508 millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
510 // find distance unit vector
511 for (int i
= 0; i
< 3; i
++)
512 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
514 // Do not move faster than the configured cartesian limits
515 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
517 if ( max_speeds
[axis
] > 0 )
519 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
521 if (axis_speed
> max_speeds
[axis
])
522 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
526 // find actuator position given cartesian position
527 arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
529 // check per-actuator speed limits
530 for (int actuator
= 0; actuator
<= 2; actuator
++)
532 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
534 if (actuator_rate
> actuators
[actuator
]->max_rate
)
535 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
538 // Append the block to the planner
539 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
541 // Update the last_milestone to the current target for the next time we use last_milestone
542 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
546 // Append a move to the queue ( cutting it into segments if needed )
547 void Robot::append_line(Gcode
* gcode
, float target
[], float rate_mm_s
){
549 // Find out the distance for this gcode
550 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 );
552 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
553 if( gcode
->millimeters_of_travel
< 1e-8F
){
557 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
559 // Mark the gcode as having a known distance
560 this->distance_in_gcode_is_known( gcode
);
562 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
563 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
564 // 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
567 if(this->delta_segments_per_second
> 1.0F
) {
568 // enabled if set to something > 1, it is set to 0.0 by default
569 // segment based on current speed and requested segments per second
570 // the faster the travel speed the fewer segments needed
571 // NOTE rate is mm/sec and we take into account any speed override
572 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
573 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
574 // TODO if we are only moving in Z on a delta we don't really need to segment at all
577 if(this->mm_per_line_segment
== 0.0F
){
578 segments
= 1; // don't split it up
580 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
586 // A vector to keep track of the endpoint of each segment
587 float segment_delta
[3];
588 float segment_end
[3];
590 // How far do we move each segment?
591 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
592 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
594 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
595 // We always add another point after this loop so we stop at segments-1, ie i < segments
596 for (int i
= 1; i
< segments
; i
++)
598 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ )
599 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
601 // Append the end of this segment to the queue
602 this->append_milestone(segment_end
, rate_mm_s
);
606 // Append the end of this full move to the queue
607 this->append_milestone(target
, rate_mm_s
);
609 // if adding these blocks didn't start executing, do that now
610 THEKERNEL
->conveyor
->ensure_running();
614 // Append an arc to the queue ( cutting it into segments as needed )
615 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
618 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
619 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
620 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
621 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
622 float r_axis1
= -offset
[this->plane_axis_1
];
623 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
624 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
626 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
627 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
628 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
629 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
631 // Find the distance for this gcode
632 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
634 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
635 if( gcode
->millimeters_of_travel
< 0.0001F
){ return; }
637 // Mark the gcode as having a known distance
638 this->distance_in_gcode_is_known( gcode
);
640 // Figure out how many segments for this gcode
641 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
643 float theta_per_segment
= angular_travel
/segments
;
644 float linear_per_segment
= linear_travel
/segments
;
646 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
647 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
648 r_T = [cos(phi) -sin(phi);
649 sin(phi) cos(phi] * r ;
650 For arc generation, the center of the circle is the axis of rotation and the radius vector is
651 defined from the circle center to the initial position. Each line segment is formed by successive
652 vector rotations. This requires only two cos() and sin() computations to form the rotation
653 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
654 all float numbers are single precision on the Arduino. (True float precision will not have
655 round off issues for CNC applications.) Single precision error can accumulate to be greater than
656 tool precision in some cases. Therefore, arc path correction is implemented.
658 Small angle approximation may be used to reduce computation overhead further. This approximation
659 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
660 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
661 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
662 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
663 issue for CNC machines with the single precision Arduino calculations.
664 This approximation also allows mc_arc to immediately insert a line segment into the planner
665 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
666 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
667 This is important when there are successive arc motions.
669 // Vector rotation matrix values
670 float cos_T
= 1-0.5F
*theta_per_segment
*theta_per_segment
; // Small angle approximation
671 float sin_T
= theta_per_segment
;
680 // Initialize the linear axis
681 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
683 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
685 if (count
< this->arc_correction
) {
686 // Apply vector rotation matrix
687 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
688 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
692 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
693 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
694 cos_Ti
= cosf(i
*theta_per_segment
);
695 sin_Ti
= sinf(i
*theta_per_segment
);
696 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
697 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
701 // Update arc_target location
702 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
703 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
704 arc_target
[this->plane_axis_2
] += linear_per_segment
;
706 // Append this segment to the queue
707 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
711 // Ensure last segment arrives at target location.
712 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
715 // Do the math for an arc and add it to the queue
716 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
719 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
721 // Set clockwise/counter-clockwise sign for mc_arc computations
722 bool is_clockwise
= false;
723 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
726 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
731 float Robot::theta(float x
, float y
){
732 float t
= atanf(x
/fabs(y
));
733 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
736 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
737 this->plane_axis_0
= axis_0
;
738 this->plane_axis_1
= axis_1
;
739 this->plane_axis_2
= axis_2
;