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"
16 #include "libs/nuts_bolts.h"
18 #include "libs/StepperMotor.h"
19 #include "../communication/utils/Gcode.h"
20 #include "PublicDataRequest.h"
21 #include "arm_solutions/BaseSolution.h"
22 #include "arm_solutions/CartesianSolution.h"
23 #include "arm_solutions/RotatableCartesianSolution.h"
24 #include "arm_solutions/RostockSolution.h"
25 #include "arm_solutions/JohannKosselSolution.h"
26 #include "arm_solutions/HBotSolution.h"
28 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
29 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
30 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
31 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
32 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
33 #define arc_correction_checksum CHECKSUM("arc_correction")
34 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
35 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
36 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
39 #define arm_solution_checksum CHECKSUM("arm_solution")
40 #define cartesian_checksum CHECKSUM("cartesian")
41 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
42 #define rostock_checksum CHECKSUM("rostock")
43 #define delta_checksum CHECKSUM("delta")
44 #define hbot_checksum CHECKSUM("hbot")
45 #define corexy_checksum CHECKSUM("corexy")
46 #define kossel_checksum CHECKSUM("kossel")
48 // stepper motor stuff
49 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
50 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
51 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
52 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
53 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
54 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
55 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
56 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
57 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
59 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
60 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
61 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
63 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
64 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
65 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
68 // new-style actuator stuff
69 #define actuator_checksum CHEKCSUM("actuator")
71 #define step_pin_checksum CHECKSUM("step_pin")
72 #define dir_pin_checksum CHEKCSUM("dir_pin")
73 #define en_pin_checksum CHECKSUM("en_pin")
75 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
76 #define max_rate_checksum CHECKSUM("max_rate")
78 #define alpha_checksum CHECKSUM("alpha")
79 #define beta_checksum CHECKSUM("beta")
80 #define gamma_checksum CHECKSUM("gamma")
83 // 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
84 // It takes care of cutting arcs into segments, same thing for line that are too long
85 #define max(a,b) (((a) > (b)) ? (a) : (b))
88 this->inch_mode
= false;
89 this->absolute_mode
= true;
90 this->motion_mode
= MOTION_MODE_SEEK
;
91 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
92 clear_vector(this->current_position
);
93 clear_vector(this->last_milestone
);
94 this->arm_solution
= NULL
;
95 seconds_per_minute
= 60.0F
;
98 //Called when the module has just been loaded
99 void Robot::on_module_loaded() {
100 register_for_event(ON_CONFIG_RELOAD
);
101 this->register_for_event(ON_GCODE_RECEIVED
);
102 this->register_for_event(ON_GET_PUBLIC_DATA
);
103 this->register_for_event(ON_SET_PUBLIC_DATA
);
106 this->on_config_reload(this);
109 void Robot::on_config_reload(void* argument
){
111 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
112 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
113 // To make adding those solution easier, they have their own, separate object.
114 // Here we read the config to find out which arm solution to use
115 if (this->arm_solution
) delete this->arm_solution
;
116 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
117 // Note checksums are not const expressions when in debug mode, so don't use switch
118 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
119 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
121 }else if(solution_checksum
== rostock_checksum
) {
122 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
124 }else if(solution_checksum
== kossel_checksum
) {
125 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
127 }else if(solution_checksum
== delta_checksum
) {
128 // place holder for now
129 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
131 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
132 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
134 }else if(solution_checksum
== cartesian_checksum
) {
135 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
138 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
142 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
143 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
144 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
145 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
146 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
147 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
149 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
150 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
151 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
163 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
164 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
165 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
166 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
167 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
168 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
169 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
170 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
171 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
173 float steps_per_mm
[3] = {
174 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
175 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
176 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
179 // TODO: delete or detect old steppermotors
180 // Make our 3 StepperMotors
181 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
182 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
183 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
185 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
186 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
187 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
189 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
190 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
191 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
194 actuators
.push_back(alpha_stepper_motor
);
195 actuators
.push_back(beta_stepper_motor
);
196 actuators
.push_back(gamma_stepper_motor
);
199 void Robot::on_get_public_data(void* argument
){
200 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
202 if(!pdr
->starts_with(robot_checksum
)) return;
204 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
205 static float return_data
;
206 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
207 pdr
->set_data_ptr(&return_data
);
210 }else if(pdr
->second_element_is(current_position_checksum
)) {
211 static float return_data
[3];
212 return_data
[0]= from_millimeters(this->current_position
[0]);
213 return_data
[1]= from_millimeters(this->current_position
[1]);
214 return_data
[2]= from_millimeters(this->current_position
[2]);
216 pdr
->set_data_ptr(&return_data
);
221 void Robot::on_set_public_data(void* argument
){
222 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
224 if(!pdr
->starts_with(robot_checksum
)) return;
226 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
227 // NOTE do not use this while printing!
228 float t
= *static_cast<float*>(pdr
->get_data_ptr());
229 // enforce minimum 10% speed
230 if (t
< 10.0F
) t
= 10.0F
;
232 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
237 //A GCode has been received
238 //See if the current Gcode line has some orders for us
239 void Robot::on_gcode_received(void * argument
){
240 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
242 //Temp variables, constant properties are stored in the object
243 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
244 this->motion_mode
= -1;
246 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
249 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
250 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
251 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
252 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
253 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
254 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
255 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
256 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
257 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
258 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
259 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
261 if(gcode
->get_num_args() == 0){
262 clear_vector(this->last_milestone
);
264 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
265 if ( gcode
->has_letter(letter
) )
266 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
269 memcpy(this->current_position
, this->last_milestone
, sizeof(float)*3); // current_position[] = last_milestone[];
271 // TODO: handle any number of actuators
272 float actuator_pos
[3];
273 arm_solution
->cartesian_to_actuator(current_position
, actuator_pos
);
275 for (int i
= 0; i
< 3; i
++)
276 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
278 gcode
->mark_as_taken();
282 }else if( gcode
->has_m
){
284 case 92: // M92 - set steps per mm
285 if (gcode
->has_letter('X'))
286 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
287 if (gcode
->has_letter('Y'))
288 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
289 if (gcode
->has_letter('Z'))
290 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
291 if (gcode
->has_letter('F'))
292 seconds_per_minute
= gcode
->get_value('F');
294 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
);
295 gcode
->add_nl
= true;
296 gcode
->mark_as_taken();
298 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
299 from_millimeters(this->current_position
[0]),
300 from_millimeters(this->current_position
[1]),
301 from_millimeters(this->current_position
[2]));
302 gcode
->add_nl
= true;
303 gcode
->mark_as_taken();
306 // TODO I'm not sure if the following is safe to do here, or should it go on the block queue?
307 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
308 gcode
->mark_as_taken();
309 if (gcode
->has_letter('S'))
311 float acc
= gcode
->get_value('S'); // mm/s^2
315 THEKERNEL
->planner
->acceleration
= acc
;
319 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
320 gcode
->mark_as_taken();
321 if (gcode
->has_letter('X'))
323 float jd
= gcode
->get_value('X');
327 THEKERNEL
->planner
->junction_deviation
= jd
;
329 if (gcode
->has_letter('S'))
331 float mps
= gcode
->get_value('S');
335 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
339 case 220: // M220 - speed override percentage
340 gcode
->mark_as_taken();
341 if (gcode
->has_letter('S'))
343 float factor
= gcode
->get_value('S');
344 // enforce minimum 10% speed
347 // enforce maximum 10x speed
348 if (factor
> 1000.0F
)
351 seconds_per_minute
= 6000.0F
/ factor
;
355 case 400: // wait until all moves are done up to this point
356 gcode
->mark_as_taken();
357 THEKERNEL
->conveyor
->wait_for_empty_queue();
360 case 500: // M500 saves some volatile settings to config override file
361 case 503: // M503 just prints the settings
362 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
);
363 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
364 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
);
365 gcode
->mark_as_taken();
368 case 665: // M665 set optional arm solution variables based on arm solution
369 gcode
->mark_as_taken();
370 // the parameter args could be any letter so try each one
371 for(char c
='A';c
<='Z';c
++) {
373 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
375 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
376 v
= gcode
->get_value(c
);
377 arm_solution
->set_optional(c
, v
);
379 if(supported
) { // print all current values of supported options
380 gcode
->stream
->printf("%c %8.3f ", c
, v
);
381 gcode
->add_nl
= true;
389 if( this->motion_mode
< 0)
393 float target
[3], offset
[3];
394 clear_vector(offset
);
396 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
398 for(char letter
= 'I'; letter
<= 'K'; letter
++){
399 if( gcode
->has_letter(letter
) ){
400 offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
));
403 for(char letter
= 'X'; letter
<= 'Z'; letter
++){
404 if( gcode
->has_letter(letter
) ){
405 target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']);
409 if( gcode
->has_letter('F') )
411 if( this->motion_mode
== MOTION_MODE_SEEK
)
412 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
414 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
417 //Perform any physical actions
418 switch( next_action
){
419 case NEXT_ACTION_DEFAULT
:
420 switch(this->motion_mode
){
421 case MOTION_MODE_CANCEL
: break;
422 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
423 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
424 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
429 // As far as the parser is concerned, the position is now == target. In reality the
430 // motion control system might still be processing the action and the real tool position
431 // in any intermediate location.
432 memcpy(this->current_position
, target
, sizeof(this->current_position
)); // this->position[] = target[];
436 // We received a new gcode, and one of the functions
437 // determined the distance for that given gcode. So now we can attach this gcode to the right block
439 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
441 //If the queue is empty, execute immediatly, otherwise attach to the last added block
442 THEKERNEL
->conveyor
->append_gcode(gcode
);
445 // Reset the position for all axes ( used in homing and G92 stuff )
446 void Robot::reset_axis_position(float position
, int axis
) {
447 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
449 float actuator_pos
[3];
450 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
452 for (int i
= 0; i
< 3; i
++)
453 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
457 // Convert target from millimeters to steps, and append this to the planner
458 void Robot::append_milestone( float target
[], float rate_mm_s
)
462 float actuator_pos
[3];
463 float millimeters_of_travel
;
465 // find distance moved by each axis
466 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
467 deltas
[axis
] = target
[axis
] - last_milestone
[axis
];
469 // Compute how long this move moves, so we can attach it to the block for later use
470 millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
472 // find distance unit vector
473 for (int i
= 0; i
< 3; i
++)
474 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
476 // Do not move faster than the configured cartesian limits
477 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
479 if ( max_speeds
[axis
] > 0 )
481 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
483 if (axis_speed
> max_speeds
[axis
])
484 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
488 // find actuator position given cartesian position
489 arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
491 // check per-actuator speed limits
492 for (int actuator
= 0; actuator
<= 2; actuator
++)
494 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
496 if (actuator_rate
> actuators
[actuator
]->max_rate
)
497 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
500 // Append the block to the planner
501 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
503 // Update the last_milestone to the current target for the next time we use last_milestone
504 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
508 // Append a move to the queue ( cutting it into segments if needed )
509 void Robot::append_line(Gcode
* gcode
, float target
[], float rate_mm_s
){
511 // Find out the distance for this gcode
512 gcode
->millimeters_of_travel
= sqrtf( pow( target
[X_AXIS
]-this->current_position
[X_AXIS
], 2 ) + pow( target
[Y_AXIS
]-this->current_position
[Y_AXIS
], 2 ) + pow( target
[Z_AXIS
]-this->current_position
[Z_AXIS
], 2 ) );
514 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
515 if( gcode
->millimeters_of_travel
< 0.0001F
){
519 // Mark the gcode as having a known distance
520 this->distance_in_gcode_is_known( gcode
);
522 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
523 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
524 // 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
527 if(this->delta_segments_per_second
> 1.0F
) {
528 // enabled if set to something > 1, it is set to 0.0 by default
529 // segment based on current speed and requested segments per second
530 // the faster the travel speed the fewer segments needed
531 // NOTE rate is mm/sec and we take into account any speed override
532 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
533 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
534 // TODO if we are only moving in Z on a delta we don't really need to segment at all
537 if(this->mm_per_line_segment
== 0.0F
){
538 segments
= 1; // don't split it up
540 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
544 // A vector to keep track of the endpoint of each segment
545 float temp_target
[3];
547 memcpy( temp_target
, this->current_position
, sizeof(temp_target
)); // temp_target[] = this->current_position[];
550 for( int i
=0; i
<segments
-1; i
++ ){
551 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
552 // Append the end of this segment to the queue
553 this->append_milestone(temp_target
, rate_mm_s
);
556 // Append the end of this full move to the queue
557 this->append_milestone(target
, rate_mm_s
);
559 // if adding these blocks didn't start executing, do that now
560 THEKERNEL
->conveyor
->ensure_running();
564 // Append an arc to the queue ( cutting it into segments as needed )
565 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
568 float center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
569 float center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
570 float linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
571 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
572 float r_axis1
= -offset
[this->plane_axis_1
];
573 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
574 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
576 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
577 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
578 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
579 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
581 // Find the distance for this gcode
582 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
584 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
585 if( gcode
->millimeters_of_travel
< 0.0001F
){ return; }
587 // Mark the gcode as having a known distance
588 this->distance_in_gcode_is_known( gcode
);
590 // Figure out how many segments for this gcode
591 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
593 float theta_per_segment
= angular_travel
/segments
;
594 float linear_per_segment
= linear_travel
/segments
;
596 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
597 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
598 r_T = [cos(phi) -sin(phi);
599 sin(phi) cos(phi] * r ;
600 For arc generation, the center of the circle is the axis of rotation and the radius vector is
601 defined from the circle center to the initial position. Each line segment is formed by successive
602 vector rotations. This requires only two cos() and sin() computations to form the rotation
603 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
604 all float numbers are single precision on the Arduino. (True float precision will not have
605 round off issues for CNC applications.) Single precision error can accumulate to be greater than
606 tool precision in some cases. Therefore, arc path correction is implemented.
608 Small angle approximation may be used to reduce computation overhead further. This approximation
609 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
610 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
611 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
612 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
613 issue for CNC machines with the single precision Arduino calculations.
614 This approximation also allows mc_arc to immediately insert a line segment into the planner
615 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
616 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
617 This is important when there are successive arc motions.
619 // Vector rotation matrix values
620 float cos_T
= 1-0.5F
*theta_per_segment
*theta_per_segment
; // Small angle approximation
621 float sin_T
= theta_per_segment
;
630 // Initialize the linear axis
631 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
633 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
635 if (count
< this->arc_correction
) {
636 // Apply vector rotation matrix
637 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
638 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
642 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
643 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
644 cos_Ti
= cosf(i
*theta_per_segment
);
645 sin_Ti
= sinf(i
*theta_per_segment
);
646 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
647 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
651 // Update arc_target location
652 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
653 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
654 arc_target
[this->plane_axis_2
] += linear_per_segment
;
656 // Append this segment to the queue
657 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
661 // Ensure last segment arrives at target location.
662 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
665 // Do the math for an arc and add it to the queue
666 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
669 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
671 // Set clockwise/counter-clockwise sign for mc_arc computations
672 bool is_clockwise
= false;
673 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
676 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
681 float Robot::theta(float x
, float y
){
682 float t
= atanf(x
/fabs(y
));
683 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
686 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
687 this->plane_axis_0
= axis_0
;
688 this->plane_axis_1
= axis_1
;
689 this->plane_axis_2
= axis_2
;