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 // 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
49 // It takes care of cutting arcs into segments, same thing for line that are too long
50 #define max(a,b) (((a) > (b)) ? (a) : (b))
53 this->inch_mode
= false;
54 this->absolute_mode
= true;
55 this->motion_mode
= MOTION_MODE_SEEK
;
56 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
57 clear_vector(this->current_position
);
58 clear_vector(this->last_milestone
);
59 this->arm_solution
= NULL
;
60 seconds_per_minute
= 60.0;
63 //Called when the module has just been loaded
64 void Robot::on_module_loaded() {
65 register_for_event(ON_CONFIG_RELOAD
);
66 this->register_for_event(ON_GCODE_RECEIVED
);
67 this->register_for_event(ON_GET_PUBLIC_DATA
);
68 this->register_for_event(ON_SET_PUBLIC_DATA
);
71 this->on_config_reload(this);
73 // Make our 3 StepperMotors
74 this->alpha_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(&alpha_step_pin
,&alpha_dir_pin
,&alpha_en_pin
) );
75 this->beta_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(&beta_step_pin
, &beta_dir_pin
, &beta_en_pin
) );
76 this->gamma_stepper_motor
= THEKERNEL
->step_ticker
->add_stepper_motor( new StepperMotor(&gamma_step_pin
,&gamma_dir_pin
,&gamma_en_pin
) );
80 void Robot::on_config_reload(void* argument
){
82 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
83 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
84 // To make adding those solution easier, they have their own, separate object.
85 // Here we read the config to find out which arm solution to use
86 if (this->arm_solution
) delete this->arm_solution
;
87 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
88 // Note checksums are not const expressions when in debug mode, so don't use switch
89 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
90 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
92 }else if(solution_checksum
== rostock_checksum
) {
93 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
95 }else if(solution_checksum
== kossel_checksum
) {
96 this->arm_solution
= new JohannKosselSolution(THEKERNEL
->config
);
98 }else if(solution_checksum
== delta_checksum
) {
99 // place holder for now
100 this->arm_solution
= new RostockSolution(THEKERNEL
->config
);
102 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
103 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
105 }else if(solution_checksum
== cartesian_checksum
) {
106 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
109 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
113 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default(100 )->as_number() / 60;
114 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default(100 )->as_number() / 60;
115 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default(0.0f
)->as_number();
116 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
117 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5f
)->as_number();
118 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
119 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
120 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
121 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
122 this->alpha_step_pin
.from_string( THEKERNEL
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
123 this->alpha_dir_pin
.from_string( THEKERNEL
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
124 this->alpha_en_pin
.from_string( THEKERNEL
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
125 this->beta_step_pin
.from_string( THEKERNEL
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
126 this->gamma_step_pin
.from_string( THEKERNEL
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
127 this->gamma_dir_pin
.from_string( THEKERNEL
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
128 this->gamma_en_pin
.from_string( THEKERNEL
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
129 this->beta_dir_pin
.from_string( THEKERNEL
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
130 this->beta_en_pin
.from_string( THEKERNEL
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
134 void Robot::on_get_public_data(void* argument
){
135 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
137 if(!pdr
->starts_with(robot_checksum
)) return;
139 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
140 static float return_data
;
141 return_data
= 100*this->seconds_per_minute
/60;
142 pdr
->set_data_ptr(&return_data
);
145 }else if(pdr
->second_element_is(current_position_checksum
)) {
146 static float return_data
[3];
147 return_data
[0]= from_millimeters(this->current_position
[0]);
148 return_data
[1]= from_millimeters(this->current_position
[1]);
149 return_data
[2]= from_millimeters(this->current_position
[2]);
151 pdr
->set_data_ptr(&return_data
);
156 void Robot::on_set_public_data(void* argument
){
157 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
159 if(!pdr
->starts_with(robot_checksum
)) return;
161 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
162 // NOTE do not use this while printing!
163 float t
= *static_cast<float*>(pdr
->get_data_ptr());
164 // enforce minimum 10% speed
165 if (t
< 10.0) t
= 10.0;
167 this->seconds_per_minute
= t
* 0.6;
172 //A GCode has been received
173 //See if the current Gcode line has some orders for us
174 void Robot::on_gcode_received(void * argument
){
175 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
177 //Temp variables, constant properties are stored in the object
178 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
179 this->motion_mode
= -1;
181 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
184 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
185 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
186 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
187 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
188 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
189 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
190 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
191 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
192 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
193 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
194 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
196 if(gcode
->get_num_args() == 0){
197 clear_vector(this->last_milestone
);
199 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
200 if ( gcode
->has_letter(letter
) )
201 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
204 memcpy(this->current_position
, this->last_milestone
, sizeof(float)*3); // current_position[] = last_milestone[];
205 this->arm_solution
->millimeters_to_steps(this->current_position
, THEKERNEL
->planner
->position
);
206 gcode
->mark_as_taken();
207 return; // TODO: Wait until queue empty
210 }else if( gcode
->has_m
){
213 case 92: // M92 - set steps per mm
214 this->arm_solution
->get_steps_per_millimeter(steps
);
215 if (gcode
->has_letter('X'))
216 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
217 if (gcode
->has_letter('Y'))
218 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
219 if (gcode
->has_letter('Z'))
220 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
221 if (gcode
->has_letter('F'))
222 seconds_per_minute
= gcode
->get_value('F');
223 this->arm_solution
->set_steps_per_millimeter(steps
);
224 // update current position in steps
225 this->arm_solution
->millimeters_to_steps(this->current_position
, THEKERNEL
->planner
->position
);
226 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
227 gcode
->add_nl
= true;
228 gcode
->mark_as_taken();
230 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
231 from_millimeters(this->current_position
[0]),
232 from_millimeters(this->current_position
[1]),
233 from_millimeters(this->current_position
[2]));
234 gcode
->add_nl
= true;
235 gcode
->mark_as_taken();
238 // TODO I'm not sure if the following is safe to do here, or should it go on the block queue?
239 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
240 gcode
->mark_as_taken();
241 if (gcode
->has_letter('S'))
243 float acc
= gcode
->get_value('S') * 60 * 60; // mm/min^2
247 THEKERNEL
->planner
->acceleration
= acc
;
251 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
252 gcode
->mark_as_taken();
253 if (gcode
->has_letter('X'))
255 float jd
= gcode
->get_value('X');
259 THEKERNEL
->planner
->junction_deviation
= jd
;
261 if (gcode
->has_letter('S'))
263 float mps
= gcode
->get_value('S');
267 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
271 case 220: // M220 - speed override percentage
272 gcode
->mark_as_taken();
273 if (gcode
->has_letter('S'))
275 float factor
= gcode
->get_value('S');
276 // enforce minimum 10% speed
279 seconds_per_minute
= factor
* 0.6;
283 case 400: // wait until all moves are done up to this point
284 gcode
->mark_as_taken();
285 THEKERNEL
->conveyor
->wait_for_empty_queue();
288 case 500: // M500 saves some volatile settings to config override file
289 case 503: // M503 just prints the settings
290 this->arm_solution
->get_steps_per_millimeter(steps
);
291 gcode
->stream
->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", steps
[0], steps
[1], steps
[2]);
292 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL
->planner
->acceleration
/3600);
293 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
);
294 gcode
->mark_as_taken();
297 case 665: // M665 set optional arm solution variables based on arm solution
298 gcode
->mark_as_taken();
299 // the parameter args could be any letter so try each one
300 for(char c
='A';c
<='Z';c
++) {
302 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
304 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
305 v
= gcode
->get_value(c
);
306 arm_solution
->set_optional(c
, v
);
308 if(supported
) { // print all current values of supported options
309 gcode
->stream
->printf("%c %8.3f ", c
, v
);
310 gcode
->add_nl
= true;
318 if( this->motion_mode
< 0)
322 float target
[3], offset
[3];
323 clear_vector(target
); clear_vector(offset
);
325 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
327 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
328 for(char letter
= 'X'; letter
<= 'Z'; letter
++){ if( gcode
->has_letter(letter
) ){ target
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
)) + ( this->absolute_mode
? 0 : target
[letter
-'X']); } }
330 if( gcode
->has_letter('F') )
332 if( this->motion_mode
== MOTION_MODE_SEEK
)
333 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
335 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
338 //Perform any physical actions
339 switch( next_action
){
340 case NEXT_ACTION_DEFAULT
:
341 switch(this->motion_mode
){
342 case MOTION_MODE_CANCEL
: break;
343 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
344 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
345 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
350 // As far as the parser is concerned, the position is now == target. In reality the
351 // motion control system might still be processing the action and the real tool position
352 // in any intermediate location.
353 memcpy(this->current_position
, target
, sizeof(float)*3); // this->position[] = target[];
357 // We received a new gcode, and one of the functions
358 // determined the distance for that given gcode. So now we can attach this gcode to the right block
360 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
362 //If the queue is empty, execute immediatly, otherwise attach to the last added block
363 THEKERNEL
->conveyor
->append_gcode(gcode
);
366 // Reset the position for all axes ( used in homing and G92 stuff )
367 void Robot::reset_axis_position(float position
, int axis
) {
368 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
369 this->arm_solution
->millimeters_to_steps(this->current_position
, THEKERNEL
->planner
->position
);
373 // Convert target from millimeters to steps, and append this to the planner
374 void Robot::append_milestone( float target
[], float rate
){
375 int steps
[3]; //Holds the result of the conversion
377 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
378 this->arm_solution
->millimeters_to_steps( target
, steps
);
381 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
383 // Compute how long this move moves, so we can attach it to the block for later use
384 float millimeters_of_travel
= sqrtf( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
386 // Do not move faster than the configured limits
387 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
388 if( this->max_speeds
[axis
] > 0 ){
389 float axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
390 if( axis_speed
> this->max_speeds
[axis
] ){
391 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
396 // Append the block to the planner
397 THEKERNEL
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
399 // Update the last_milestone to the current target for the next time we use last_milestone
400 memcpy(this->last_milestone
, target
, sizeof(float)*3); // this->last_milestone[] = target[];
404 // Append a move to the queue ( cutting it into segments if needed )
405 void Robot::append_line(Gcode
* gcode
, float target
[], float rate
){
407 // Find out the distance for this gcode
408 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 ) );
410 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
411 if( gcode
->millimeters_of_travel
< 0.0001 ){
412 // an extruder only move means we stopped so we need to tell planner that previous speed and unitvector are zero
413 THEKERNEL
->planner
->previous_nominal_speed
= 0;
414 clear_vector_float(THEKERNEL
->planner
->previous_unit_vec
);
418 // Mark the gcode as having a known distance
419 this->distance_in_gcode_is_known( gcode
);
421 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
422 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
423 // 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
426 if(this->delta_segments_per_second
> 1.0) {
427 // enabled if set to something > 1, it is set to 0.0 by default
428 // segment based on current speed and requested segments per second
429 // the faster the travel speed the fewer segments needed
430 // NOTE rate is mm/sec and we take into account any speed override
431 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
432 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
433 // TODO if we are only moving in Z on a delta we don't really need to segment at all
436 if(this->mm_per_line_segment
== 0.0){
437 segments
= 1; // don't split it up
439 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
443 // A vector to keep track of the endpoint of each segment
444 float temp_target
[3];
446 memcpy( temp_target
, this->current_position
, sizeof(float)*3); // temp_target[] = this->current_position[];
449 for( int i
=0; i
<segments
-1; i
++ ){
450 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
451 // Append the end of this segment to the queue
452 this->append_milestone(temp_target
, rate
);
455 // Append the end of this full move to the queue
456 this->append_milestone(target
, rate
);
460 // Append an arc to the queue ( cutting it into segments as needed )
461 void Robot::append_arc(Gcode
* gcode
, float target
[], float offset
[], float radius
, bool is_clockwise
){
464 float center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
465 float center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
466 float linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
467 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
468 float r_axis1
= -offset
[this->plane_axis_1
];
469 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
470 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
472 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
473 float angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
474 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
475 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
477 // Find the distance for this gcode
478 gcode
->millimeters_of_travel
= hypotf(angular_travel
*radius
, fabs(linear_travel
));
480 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
481 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
483 // Mark the gcode as having a known distance
484 this->distance_in_gcode_is_known( gcode
);
486 // Figure out how many segments for this gcode
487 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
489 float theta_per_segment
= angular_travel
/segments
;
490 float linear_per_segment
= linear_travel
/segments
;
492 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
493 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
494 r_T = [cos(phi) -sin(phi);
495 sin(phi) cos(phi] * r ;
496 For arc generation, the center of the circle is the axis of rotation and the radius vector is
497 defined from the circle center to the initial position. Each line segment is formed by successive
498 vector rotations. This requires only two cos() and sin() computations to form the rotation
499 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
500 all float numbers are single precision on the Arduino. (True float precision will not have
501 round off issues for CNC applications.) Single precision error can accumulate to be greater than
502 tool precision in some cases. Therefore, arc path correction is implemented.
504 Small angle approximation may be used to reduce computation overhead further. This approximation
505 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
506 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
507 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
508 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
509 issue for CNC machines with the single precision Arduino calculations.
510 This approximation also allows mc_arc to immediately insert a line segment into the planner
511 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
512 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
513 This is important when there are successive arc motions.
515 // Vector rotation matrix values
516 float cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
517 float sin_T
= theta_per_segment
;
526 // Initialize the linear axis
527 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
529 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
531 if (count
< this->arc_correction
) {
532 // Apply vector rotation matrix
533 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
534 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
538 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
539 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
540 cos_Ti
= cosf(i
*theta_per_segment
);
541 sin_Ti
= sinf(i
*theta_per_segment
);
542 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
543 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
547 // Update arc_target location
548 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
549 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
550 arc_target
[this->plane_axis_2
] += linear_per_segment
;
552 // Append this segment to the queue
553 this->append_milestone(arc_target
, this->feed_rate
);
557 // Ensure last segment arrives at target location.
558 this->append_milestone(target
, this->feed_rate
);
561 // Do the math for an arc and add it to the queue
562 void Robot::compute_arc(Gcode
* gcode
, float offset
[], float target
[]){
565 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
567 // Set clockwise/counter-clockwise sign for mc_arc computations
568 bool is_clockwise
= false;
569 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
572 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
577 float Robot::theta(float x
, float y
){
578 float t
= atanf(x
/fabs(y
));
579 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
582 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
583 this->plane_axis_0
= axis_0
;
584 this->plane_axis_1
= axis_1
;
585 this->plane_axis_2
= axis_2
;