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"
35 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
36 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
37 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
38 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
39 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
40 #define arc_correction_checksum CHECKSUM("arc_correction")
41 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
42 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
43 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
46 #define arm_solution_checksum CHECKSUM("arm_solution")
47 #define cartesian_checksum CHECKSUM("cartesian")
48 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
49 #define rostock_checksum CHECKSUM("rostock")
50 #define delta_checksum CHECKSUM("delta")
51 #define hbot_checksum CHECKSUM("hbot")
52 #define corexy_checksum CHECKSUM("corexy")
53 #define kossel_checksum CHECKSUM("kossel")
55 // stepper motor stuff
56 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
57 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
58 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
59 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
60 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
61 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
62 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
63 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
64 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
66 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
67 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
68 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
70 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
71 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
72 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
75 // new-style actuator stuff
76 #define actuator_checksum CHEKCSUM("actuator")
78 #define step_pin_checksum CHECKSUM("step_pin")
79 #define dir_pin_checksum CHEKCSUM("dir_pin")
80 #define en_pin_checksum CHECKSUM("en_pin")
82 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
83 #define max_rate_checksum CHECKSUM("max_rate")
85 #define alpha_checksum CHECKSUM("alpha")
86 #define beta_checksum CHECKSUM("beta")
87 #define gamma_checksum CHECKSUM("gamma")
90 // 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
91 // It takes care of cutting arcs into segments, same thing for line that are too long
92 #define max(a,b) (((a) > (b)) ? (a) : (b))
95 this->inch_mode
= false;
96 this->absolute_mode
= true;
97 this->motion_mode
= MOTION_MODE_SEEK
;
98 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
99 clear_vector(this->last_milestone
);
100 this->arm_solution
= NULL
;
101 seconds_per_minute
= 60.0F
;
104 //Called when the module has just been loaded
105 void Robot::on_module_loaded() {
106 register_for_event(ON_CONFIG_RELOAD
);
107 this->register_for_event(ON_GCODE_RECEIVED
);
108 this->register_for_event(ON_GET_PUBLIC_DATA
);
109 this->register_for_event(ON_SET_PUBLIC_DATA
);
112 this->on_config_reload(this);
115 void Robot::on_config_reload(void* argument
){
117 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
118 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
119 // To make adding those solution easier, they have their own, separate object.
120 // Here we read the config to find out which arm solution to use
121 if (this->arm_solution
) delete this->arm_solution
;
122 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
123 // Note checksums are not const expressions when in debug mode, so don't use switch
124 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
125 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
127 }else if(solution_checksum
== rostock_checksum
) {
128 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
130 }else if(solution_checksum
== kossel_checksum
) {
131 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
133 }else if(solution_checksum
== delta_checksum
) {
134 // place holder for now
135 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
137 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
138 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
140 }else if(solution_checksum
== cartesian_checksum
) {
141 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
144 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
148 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
149 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
150 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
151 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
152 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.5f
)->as_number();
153 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
155 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
156 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
157 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
169 alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
170 alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
171 alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
172 beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
173 beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
174 beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
175 gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
176 gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
177 gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
179 float steps_per_mm
[3] = {
180 THEKERNEL
->config
->value(alpha_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
181 THEKERNEL
->config
->value(beta_steps_per_mm_checksum
)->by_default( 80.0F
)->as_number(),
182 THEKERNEL
->config
->value(gamma_steps_per_mm_checksum
)->by_default(2560.0F
)->as_number(),
185 // TODO: delete or detect old steppermotors
186 // Make our 3 StepperMotors
187 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(alpha_step_pin
, alpha_dir_pin
, alpha_en_pin
) );
188 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(beta_step_pin
, beta_dir_pin
, beta_en_pin
) );
189 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(gamma_step_pin
, gamma_dir_pin
, gamma_en_pin
) );
191 alpha_stepper_motor
->change_steps_per_mm(steps_per_mm
[0]);
192 beta_stepper_motor
->change_steps_per_mm(steps_per_mm
[1]);
193 gamma_stepper_motor
->change_steps_per_mm(steps_per_mm
[2]);
195 alpha_stepper_motor
->max_rate
= THEKERNEL
->config
->value(alpha_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
196 beta_stepper_motor
->max_rate
= THEKERNEL
->config
->value(beta_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
197 gamma_stepper_motor
->max_rate
= THEKERNEL
->config
->value(gamma_max_rate_checksum
)->by_default(30000.0F
)->as_number() / 60.0F
;
200 actuators
.push_back(alpha_stepper_motor
);
201 actuators
.push_back(beta_stepper_motor
);
202 actuators
.push_back(gamma_stepper_motor
);
204 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
205 // so the first move can be correct if homing is not performed
206 float actuator_pos
[3];
207 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
208 for (int i
= 0; i
< 3; i
++)
209 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
212 void Robot::on_get_public_data(void* argument
){
213 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
215 if(!pdr
->starts_with(robot_checksum
)) return;
217 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
218 static float return_data
;
219 return_data
= 100.0F
* 60.0F
/ seconds_per_minute
;
220 pdr
->set_data_ptr(&return_data
);
223 }else if(pdr
->second_element_is(current_position_checksum
)) {
224 static float return_data
[3];
225 return_data
[0]= from_millimeters(this->last_milestone
[0]);
226 return_data
[1]= from_millimeters(this->last_milestone
[1]);
227 return_data
[2]= from_millimeters(this->last_milestone
[2]);
229 pdr
->set_data_ptr(&return_data
);
234 void Robot::on_set_public_data(void* argument
){
235 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
237 if(!pdr
->starts_with(robot_checksum
)) return;
239 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
240 // NOTE do not use this while printing!
241 float t
= *static_cast<float*>(pdr
->get_data_ptr());
242 // enforce minimum 10% speed
243 if (t
< 10.0F
) t
= 10.0F
;
245 this->seconds_per_minute
= t
/ 0.6F
; // t * 60 / 100
250 //A GCode has been received
251 //See if the current Gcode line has some orders for us
252 void Robot::on_gcode_received(void * argument
){
253 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
255 //Temp variables, constant properties are stored in the object
256 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
257 this->motion_mode
= -1;
259 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
262 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
263 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
264 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
265 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
266 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
267 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
268 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
269 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
270 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
271 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
272 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
274 if(gcode
->get_num_args() == 0){
275 clear_vector(this->last_milestone
);
277 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
278 if ( gcode
->has_letter(letter
) )
279 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
283 // TODO: handle any number of actuators
284 float actuator_pos
[3];
285 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
287 for (int i
= 0; i
< 3; i
++)
288 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
290 gcode
->mark_as_taken();
294 }else if( gcode
->has_m
){
296 case 92: // M92 - set steps per mm
297 if (gcode
->has_letter('X'))
298 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
299 if (gcode
->has_letter('Y'))
300 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
301 if (gcode
->has_letter('Z'))
302 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
303 if (gcode
->has_letter('F'))
304 seconds_per_minute
= gcode
->get_value('F');
306 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
);
307 gcode
->add_nl
= true;
308 gcode
->mark_as_taken();
313 int n
= snprintf(buf
, sizeof(buf
), "C: X:%1.3f Y:%1.3f Z:%1.3f",
314 from_millimeters(this->last_milestone
[0]),
315 from_millimeters(this->last_milestone
[1]),
316 from_millimeters(this->last_milestone
[2]));
317 gcode
->txt_after_ok
.append(buf
, n
);
318 gcode
->mark_as_taken();
322 case 203: // M203 Set maximum feedrates in mm/sec
323 if (gcode
->has_letter('X'))
324 this->max_speeds
[X_AXIS
]= gcode
->get_value('X');
325 if (gcode
->has_letter('Y'))
326 this->max_speeds
[Y_AXIS
]= gcode
->get_value('Y');
327 if (gcode
->has_letter('Z'))
328 this->max_speeds
[Z_AXIS
]= gcode
->get_value('Z');
329 if (gcode
->has_letter('A'))
330 alpha_stepper_motor
->max_rate
= gcode
->get_value('A');
331 if (gcode
->has_letter('B'))
332 beta_stepper_motor
->max_rate
= gcode
->get_value('B');
333 if (gcode
->has_letter('C'))
334 gamma_stepper_motor
->max_rate
= gcode
->get_value('C');
336 gcode
->stream
->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
337 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
338 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
339 gcode
->add_nl
= true;
340 gcode
->mark_as_taken();
343 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
344 gcode
->mark_as_taken();
346 if (gcode
->has_letter('S'))
348 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
349 THEKERNEL
->conveyor
->wait_for_empty_queue();
350 float acc
= gcode
->get_value('S'); // mm/s^2
354 THEKERNEL
->planner
->acceleration
= acc
;
358 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
359 gcode
->mark_as_taken();
360 if (gcode
->has_letter('X'))
362 float jd
= gcode
->get_value('X');
366 THEKERNEL
->planner
->junction_deviation
= jd
;
368 if (gcode
->has_letter('S'))
370 float mps
= gcode
->get_value('S');
374 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
378 case 220: // M220 - speed override percentage
379 gcode
->mark_as_taken();
380 if (gcode
->has_letter('S'))
382 float factor
= gcode
->get_value('S');
383 // enforce minimum 10% speed
386 // enforce maximum 10x speed
387 if (factor
> 1000.0F
)
390 seconds_per_minute
= 6000.0F
/ factor
;
394 case 400: // wait until all moves are done up to this point
395 gcode
->mark_as_taken();
396 THEKERNEL
->conveyor
->wait_for_empty_queue();
399 case 500: // M500 saves some volatile settings to config override file
400 case 503: // M503 just prints the settings
401 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
);
402 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
);
403 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
);
404 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",
405 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
406 alpha_stepper_motor
->max_rate
, beta_stepper_motor
->max_rate
, gamma_stepper_motor
->max_rate
);
407 gcode
->mark_as_taken();
410 case 665: // M665 set optional arm solution variables based on arm solution. NOTE these are not saved with M500
411 gcode
->mark_as_taken();
412 // the parameter args could be any letter except S so try each one
413 for(char c
='A';c
<='Z';c
++) {
414 if(c
== 'S') continue; // used for segments per second
416 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
418 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
419 v
= gcode
->get_value(c
);
420 arm_solution
->set_optional(c
, v
);
422 if(supported
) { // print all current values of supported options
423 gcode
->stream
->printf("%c %8.3f ", c
, v
);
424 gcode
->add_nl
= true;
427 // set delta segments per second
428 if(gcode
->has_letter('S')) {
429 this->delta_segments_per_second
= gcode
->get_value('S');
435 if( this->motion_mode
< 0)
439 float target
[3], offset
[3];
440 clear_vector(offset
);
442 memcpy(target
, this->last_milestone
, sizeof(target
)); //default to last target
444 for(char letter
= 'I'; letter
<= 'K'; letter
++){
445 if( gcode
->has_letter(letter
) ){
446 offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
));
449 for(char letter
= 'X'; letter
<= 'Z'; letter
++){
450 if( gcode
->has_letter(letter
) ){
451 target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']);
455 if( gcode
->has_letter('F') )
457 if( this->motion_mode
== MOTION_MODE_SEEK
)
458 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
460 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
463 //Perform any physical actions
464 switch( next_action
){
465 case NEXT_ACTION_DEFAULT
:
466 switch(this->motion_mode
){
467 case MOTION_MODE_CANCEL
: break;
468 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
); break;
469 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
); break;
470 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
475 // As far as the parser is concerned, the position is now == target. In reality the
476 // motion control system might still be processing the action and the real tool position
477 // in any intermediate location.
478 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->position[] = target[];
482 // We received a new gcode, and one of the functions
483 // determined the distance for that given gcode. So now we can attach this gcode to the right block
485 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
487 //If the queue is empty, execute immediatly, otherwise attach to the last added block
488 THEKERNEL
->conveyor
->append_gcode(gcode
);
491 // Reset the position for all axes ( used in homing and G92 stuff )
492 void Robot::reset_axis_position(float position
, int axis
) {
493 this->last_milestone
[axis
] = position
;
495 float actuator_pos
[3];
496 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
498 for (int i
= 0; i
< 3; i
++)
499 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
503 // Convert target from millimeters to steps, and append this to the planner
504 void Robot::append_milestone( float target
[], float rate_mm_s
)
508 float actuator_pos
[3];
509 float millimeters_of_travel
;
511 // find distance moved by each axis
512 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
513 deltas
[axis
] = target
[axis
] - last_milestone
[axis
];
515 // Compute how long this move moves, so we can attach it to the block for later use
516 millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
518 // find distance unit vector
519 for (int i
= 0; i
< 3; i
++)
520 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
522 // Do not move faster than the configured cartesian limits
523 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++)
525 if ( max_speeds
[axis
] > 0 )
527 float axis_speed
= fabs(unit_vec
[axis
] * rate_mm_s
);
529 if (axis_speed
> max_speeds
[axis
])
530 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
534 // find actuator position given cartesian position
535 arm_solution
->cartesian_to_actuator( target
, actuator_pos
);
537 // check per-actuator speed limits
538 for (int actuator
= 0; actuator
<= 2; actuator
++)
540 float actuator_rate
= fabs(actuator_pos
[actuator
] - actuators
[actuator
]->last_milestone_mm
) * rate_mm_s
/ millimeters_of_travel
;
542 if (actuator_rate
> actuators
[actuator
]->max_rate
)
543 rate_mm_s
*= (actuators
[actuator
]->max_rate
/ actuator_rate
);
546 // Append the block to the planner
547 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, unit_vec
);
549 // Update the last_milestone to the current target for the next time we use last_milestone
550 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
)); // this->last_milestone[] = target[];
554 // Append a move to the queue ( cutting it into segments if needed )
555 void Robot::append_line(Gcode
* gcode
, float target
[], float rate_mm_s
){
557 // Find out the distance for this gcode
558 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 );
560 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
561 if( gcode
->millimeters_of_travel
< 1e-8F
){
565 gcode
->millimeters_of_travel
= sqrtf(gcode
->millimeters_of_travel
);
567 // Mark the gcode as having a known distance
568 this->distance_in_gcode_is_known( gcode
);
570 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
571 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
572 // 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
575 if(this->delta_segments_per_second
> 1.0F
) {
576 // enabled if set to something > 1, it is set to 0.0 by default
577 // segment based on current speed and requested segments per second
578 // the faster the travel speed the fewer segments needed
579 // NOTE rate is mm/sec and we take into account any speed override
580 float seconds
= gcode
->millimeters_of_travel
/ rate_mm_s
;
581 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
582 // TODO if we are only moving in Z on a delta we don't really need to segment at all
585 if(this->mm_per_line_segment
== 0.0F
){
586 segments
= 1; // don't split it up
588 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
594 // A vector to keep track of the endpoint of each segment
595 float segment_delta
[3];
596 float segment_end
[3];
598 // How far do we move each segment?
599 for (int i
= X_AXIS
; i
<= Z_AXIS
; i
++)
600 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
602 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
603 // We always add another point after this loop so we stop at segments-1, ie i < segments
604 for (int i
= 1; i
< segments
; i
++)
606 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ )
607 segment_end
[axis
] = last_milestone
[axis
] + segment_delta
[axis
];
609 // Append the end of this segment to the queue
610 this->append_milestone(segment_end
, rate_mm_s
);
614 // Append the end of this full move to the queue
615 this->append_milestone(target
, rate_mm_s
);
617 // if adding these blocks didn't start executing, do that now
618 THEKERNEL
->conveyor
->ensure_running();
622 // Append an arc to the queue ( cutting it into segments as needed )
623 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
626 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
627 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
628 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
629 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
630 float r_axis1
= -offset
[this->plane_axis_1
];
631 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
632 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
634 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
635 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
636 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
637 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
639 // Find the distance for this gcode
640 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
642 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
643 if( gcode
->millimeters_of_travel
< 0.0001F
){ return; }
645 // Mark the gcode as having a known distance
646 this->distance_in_gcode_is_known( gcode
);
648 // Figure out how many segments for this gcode
649 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
651 float theta_per_segment
= angular_travel
/segments
;
652 float linear_per_segment
= linear_travel
/segments
;
654 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
655 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
656 r_T = [cos(phi) -sin(phi);
657 sin(phi) cos(phi] * r ;
658 For arc generation, the center of the circle is the axis of rotation and the radius vector is
659 defined from the circle center to the initial position. Each line segment is formed by successive
660 vector rotations. This requires only two cos() and sin() computations to form the rotation
661 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
662 all float numbers are single precision on the Arduino. (True float precision will not have
663 round off issues for CNC applications.) Single precision error can accumulate to be greater than
664 tool precision in some cases. Therefore, arc path correction is implemented.
666 Small angle approximation may be used to reduce computation overhead further. This approximation
667 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
668 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
669 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
670 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
671 issue for CNC machines with the single precision Arduino calculations.
672 This approximation also allows mc_arc to immediately insert a line segment into the planner
673 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
674 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
675 This is important when there are successive arc motions.
677 // Vector rotation matrix values
678 float cos_T
= 1-0.5F
*theta_per_segment
*theta_per_segment
; // Small angle approximation
679 float sin_T
= theta_per_segment
;
688 // Initialize the linear axis
689 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
691 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
693 if (count
< this->arc_correction
) {
694 // Apply vector rotation matrix
695 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
696 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
700 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
701 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
702 cos_Ti
= cosf(i
*theta_per_segment
);
703 sin_Ti
= sinf(i
*theta_per_segment
);
704 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
705 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
709 // Update arc_target location
710 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
711 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
712 arc_target
[this->plane_axis_2
] += linear_per_segment
;
714 // Append this segment to the queue
715 this->append_milestone(arc_target
, this->feed_rate
/ seconds_per_minute
);
719 // Ensure last segment arrives at target location.
720 this->append_milestone(target
, this->feed_rate
/ seconds_per_minute
);
723 // Do the math for an arc and add it to the queue
724 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
727 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
729 // Set clockwise/counter-clockwise sign for mc_arc computations
730 bool is_clockwise
= false;
731 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
734 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
739 float Robot::theta(float x
, float y
){
740 float t
= atanf(x
/fabs(y
));
741 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
744 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
745 this->plane_axis_0
= axis_0
;
746 this->plane_axis_1
= axis_1
;
747 this->plane_axis_2
= axis_2
;