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 // new-style actuator stuff
64 #define actuator_checksum CHEKCSUM("actuator")
66 #define step_pin_checksum CHECKSUM("step_pin")
67 #define dir_pin_checksum CHEKCSUM("dir_pin")
68 #define en_pin_checksum CHECKSUM("en_pin")
70 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
72 #define alpha_checksum CHECKSUM("alpha")
73 #define beta_checksum CHECKSUM("beta")
74 #define gamma_checksum CHECKSUM("gamma")
77 // 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
78 // It takes care of cutting arcs into segments, same thing for line that are too long
79 #define max(a,b) (((a) > (b)) ? (a) : (b))
82 this->inch_mode
= false;
83 this->absolute_mode
= true;
84 this->motion_mode
= MOTION_MODE_SEEK
;
85 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
86 clear_vector(this->current_position
);
87 clear_vector(this->last_milestone
);
88 this->arm_solution
= NULL
;
89 seconds_per_minute
= 60.0;
92 //Called when the module has just been loaded
93 void Robot::on_module_loaded() {
94 register_for_event(ON_CONFIG_RELOAD
);
95 this->register_for_event(ON_GCODE_RECEIVED
);
96 this->register_for_event(ON_GET_PUBLIC_DATA
);
97 this->register_for_event(ON_SET_PUBLIC_DATA
);
100 this->on_config_reload(this);
103 void Robot::on_config_reload(void* argument
){
105 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
106 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
107 // To make adding those solution easier, they have their own, separate object.
108 // Here we read the config to find out which arm solution to use
109 if (this->arm_solution
) delete this->arm_solution
;
110 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
111 // Note checksums are not const expressions when in debug mode, so don't use switch
112 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
113 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
115 }else if(solution_checksum
== rostock_checksum
) {
116 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
118 }else if(solution_checksum
== kossel_checksum
) {
119 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
121 }else if(solution_checksum
== delta_checksum
) {
122 // place holder for now
123 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
125 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
126 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
128 }else if(solution_checksum
== cartesian_checksum
) {
129 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
132 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
136 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default(100 )->as_number() / 60;
137 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default(100 )->as_number() / 60;
138 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default(0.0f
)->as_number();
139 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
140 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5f
)->as_number();
141 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
143 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
144 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
145 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
157 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
158 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
159 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
160 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
161 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
162 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
163 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
164 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
165 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
167 float steps_per_mm
[3] = {
168 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
169 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
170 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
173 // TODO: delete or detect old steppermotors
174 // Make our 3 StepperMotors
175 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
176 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
177 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
179 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
180 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
181 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
184 actuators
.push_back(alpha_stepper_motor
);
185 actuators
.push_back(beta_stepper_motor
);
186 actuators
.push_back(gamma_stepper_motor
);
189 void Robot::on_get_public_data(void* argument
){
190 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
192 if(!pdr
->starts_with(robot_checksum
)) return;
194 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
195 static float return_data
;
196 return_data
= 100*this->seconds_per_minute
/60;
197 pdr
->set_data_ptr(&return_data
);
200 }else if(pdr
->second_element_is(current_position_checksum
)) {
201 static float return_data
[3];
202 return_data
[0]= from_millimeters(this->current_position
[0]);
203 return_data
[1]= from_millimeters(this->current_position
[1]);
204 return_data
[2]= from_millimeters(this->current_position
[2]);
206 pdr
->set_data_ptr(&return_data
);
211 void Robot::on_set_public_data(void* argument
){
212 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
214 if(!pdr
->starts_with(robot_checksum
)) return;
216 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
217 // NOTE do not use this while printing!
218 float t
= *static_cast<float*>(pdr
->get_data_ptr());
219 // enforce minimum 10% speed
220 if (t
< 10.0) t
= 10.0;
222 this->seconds_per_minute
= t
* 0.6;
227 //A GCode has been received
228 //See if the current Gcode line has some orders for us
229 void Robot::on_gcode_received(void * argument
){
230 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
232 //Temp variables, constant properties are stored in the object
233 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
234 this->motion_mode
= -1;
236 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
239 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
240 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
241 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
242 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
243 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
244 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
245 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
246 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
247 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
248 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
249 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
251 if(gcode
->get_num_args() == 0){
252 clear_vector(this->last_milestone
);
254 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
255 if ( gcode
->has_letter(letter
) )
256 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
259 memcpy(this->current_position
, this->last_milestone
, sizeof(float)*3); // current_position[] = last_milestone[];
261 // TODO: handle any number of actuators
262 float actuator_pos
[3];
263 arm_solution
->cartesian_to_actuator(current_position
, actuator_pos
);
265 for (int i
= 0; i
< 3; i
++)
266 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
268 gcode
->mark_as_taken();
272 }else if( gcode
->has_m
){
274 case 92: // M92 - set steps per mm
275 if (gcode
->has_letter('X'))
276 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
277 if (gcode
->has_letter('Y'))
278 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
279 if (gcode
->has_letter('Z'))
280 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
281 if (gcode
->has_letter('F'))
282 seconds_per_minute
= gcode
->get_value('F');
284 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
);
285 gcode
->add_nl
= true;
286 gcode
->mark_as_taken();
288 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
289 from_millimeters(this->current_position
[0]),
290 from_millimeters(this->current_position
[1]),
291 from_millimeters(this->current_position
[2]));
292 gcode
->add_nl
= true;
293 gcode
->mark_as_taken();
296 // TODO I'm not sure if the following is safe to do here, or should it go on the block queue?
297 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
298 gcode
->mark_as_taken();
299 if (gcode
->has_letter('S'))
301 float acc
= gcode
->get_value('S') * 60 * 60; // mm/min^2
305 THEKERNEL
->planner
->acceleration
= acc
;
309 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
310 gcode
->mark_as_taken();
311 if (gcode
->has_letter('X'))
313 float jd
= gcode
->get_value('X');
317 THEKERNEL
->planner
->junction_deviation
= jd
;
319 if (gcode
->has_letter('S'))
321 float mps
= gcode
->get_value('S');
325 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
329 case 220: // M220 - speed override percentage
330 gcode
->mark_as_taken();
331 if (gcode
->has_letter('S'))
333 float factor
= gcode
->get_value('S');
334 // enforce minimum 10% speed
337 seconds_per_minute
= factor
* 0.6;
341 case 400: // wait until all moves are done up to this point
342 gcode
->mark_as_taken();
343 THEKERNEL
->conveyor
->wait_for_empty_queue();
346 case 500: // M500 saves some volatile settings to config override file
347 case 503: // M503 just prints the settings
348 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
);
349 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
/3600);
350 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
);
351 gcode
->mark_as_taken();
354 case 665: // M665 set optional arm solution variables based on arm solution
355 gcode
->mark_as_taken();
356 // the parameter args could be any letter so try each one
357 for(char c
='A';c
<='Z';c
++) {
359 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
361 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
362 v
= gcode
->get_value(c
);
363 arm_solution
->set_optional(c
, v
);
365 if(supported
) { // print all current values of supported options
366 gcode
->stream
->printf("%c %8.3f ", c
, v
);
367 gcode
->add_nl
= true;
375 if( this->motion_mode
< 0)
379 float target
[3], offset
[3];
380 clear_vector(offset
);
382 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
384 for(char letter
= 'I'; letter
<= 'K'; letter
++){
385 if( gcode
->has_letter(letter
) ){
386 offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
));
389 for(char letter
= 'X'; letter
<= 'Z'; letter
++){
390 if( gcode
->has_letter(letter
) ){
391 target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']);
395 if( gcode
->has_letter('F') )
397 if( this->motion_mode
== MOTION_MODE_SEEK
)
398 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0F
;
400 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0F
;
403 //Perform any physical actions
404 switch( next_action
){
405 case NEXT_ACTION_DEFAULT
:
406 switch(this->motion_mode
){
407 case MOTION_MODE_CANCEL
: break;
408 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
409 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
410 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
415 // As far as the parser is concerned, the position is now == target. In reality the
416 // motion control system might still be processing the action and the real tool position
417 // in any intermediate location.
418 memcpy(this->current_position
, target
, sizeof(this->current_position
)); // this->position[] = target[];
422 // We received a new gcode, and one of the functions
423 // determined the distance for that given gcode. So now we can attach this gcode to the right block
425 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
427 //If the queue is empty, execute immediatly, otherwise attach to the last added block
428 THEKERNEL
->conveyor
->append_gcode(gcode
);
431 // Reset the position for all axes ( used in homing and G92 stuff )
432 void Robot::reset_axis_position(float position
, int axis
) {
433 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
434 actuators
[axis
]->change_last_milestone(position
);
438 // Convert target from millimeters to steps, and append this to the planner
439 void Robot::append_milestone( float target
[], float rate
){
440 float actuator_pos
[3]; //Holds the result of the conversion
442 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
443 this->arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
446 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
448 // Compute how long this move moves, so we can attach it to the block for later use
449 float millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
451 // Do not move faster than the configured limits
452 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
453 if( this->max_speeds
[axis
] > 0 ){
454 float axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
455 if( axis_speed
> this->max_speeds
[axis
] ){
456 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
461 // Append the block to the planner
462 THEKERNEL
->planner
->append_block( actuator_pos
, rate
* seconds_per_minute
, millimeters_of_travel
, unit_vec
);
464 // Update the last_milestone to the current target for the next time we use last_milestone
465 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
469 // Append a move to the queue ( cutting it into segments if needed )
470 void Robot::append_line(Gcode
* gcode
, float target
[], float rate
){
472 // Find out the distance for this gcode
473 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 ) );
475 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
476 if( gcode
->millimeters_of_travel
< 0.0001F
){
480 // Mark the gcode as having a known distance
481 this->distance_in_gcode_is_known( gcode
);
483 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
484 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
485 // 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
488 if(this->delta_segments_per_second
> 1.0F
) {
489 // enabled if set to something > 1, it is set to 0.0 by default
490 // segment based on current speed and requested segments per second
491 // the faster the travel speed the fewer segments needed
492 // NOTE rate is mm/sec and we take into account any speed override
493 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
494 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
495 // TODO if we are only moving in Z on a delta we don't really need to segment at all
498 if(this->mm_per_line_segment
== 0.0F
){
499 segments
= 1; // don't split it up
501 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
505 // A vector to keep track of the endpoint of each segment
506 float temp_target
[3];
508 memcpy( temp_target
, this->current_position
, sizeof(temp_target
)); // temp_target[] = this->current_position[];
511 for( int i
=0; i
<segments
-1; i
++ ){
512 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
513 // Append the end of this segment to the queue
514 this->append_milestone(temp_target
, rate
);
517 // Append the end of this full move to the queue
518 this->append_milestone(target
, rate
);
520 // if adding these blocks didn't start executing, do that now
521 THEKERNEL
->conveyor
->ensure_running();
525 // Append an arc to the queue ( cutting it into segments as needed )
526 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
529 float center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
530 float center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
531 float linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
532 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
533 float r_axis1
= -offset
[this->plane_axis_1
];
534 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
535 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
537 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
538 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
539 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
540 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
542 // Find the distance for this gcode
543 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
545 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
546 if( gcode
->millimeters_of_travel
< 0.0001F
){ return; }
548 // Mark the gcode as having a known distance
549 this->distance_in_gcode_is_known( gcode
);
551 // Figure out how many segments for this gcode
552 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
554 float theta_per_segment
= angular_travel
/segments
;
555 float linear_per_segment
= linear_travel
/segments
;
557 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
558 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
559 r_T = [cos(phi) -sin(phi);
560 sin(phi) cos(phi] * r ;
561 For arc generation, the center of the circle is the axis of rotation and the radius vector is
562 defined from the circle center to the initial position. Each line segment is formed by successive
563 vector rotations. This requires only two cos() and sin() computations to form the rotation
564 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
565 all float numbers are single precision on the Arduino. (True float precision will not have
566 round off issues for CNC applications.) Single precision error can accumulate to be greater than
567 tool precision in some cases. Therefore, arc path correction is implemented.
569 Small angle approximation may be used to reduce computation overhead further. This approximation
570 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
571 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
572 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
573 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
574 issue for CNC machines with the single precision Arduino calculations.
575 This approximation also allows mc_arc to immediately insert a line segment into the planner
576 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
577 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
578 This is important when there are successive arc motions.
580 // Vector rotation matrix values
581 float cos_T
= 1-0.5F
*theta_per_segment
*theta_per_segment
; // Small angle approximation
582 float sin_T
= theta_per_segment
;
591 // Initialize the linear axis
592 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
594 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
596 if (count
< this->arc_correction
) {
597 // Apply vector rotation matrix
598 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
599 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
603 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
604 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
605 cos_Ti
= cosf(i
*theta_per_segment
);
606 sin_Ti
= sinf(i
*theta_per_segment
);
607 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
608 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
612 // Update arc_target location
613 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
614 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
615 arc_target
[this->plane_axis_2
] += linear_per_segment
;
617 // Append this segment to the queue
618 this->append_milestone(arc_target
, this->feed_rate
);
622 // Ensure last segment arrives at target location.
623 this->append_milestone(target
, this->feed_rate
);
626 // Do the math for an arc and add it to the queue
627 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
630 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
632 // Set clockwise/counter-clockwise sign for mc_arc computations
633 bool is_clockwise
= false;
634 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
637 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
642 float Robot::theta(float x
, float y
){
643 float t
= atanf(x
/fabs(y
));
644 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
647 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
648 this->plane_axis_0
= axis_0
;
649 this->plane_axis_1
= axis_1
;
650 this->plane_axis_2
= axis_2
;