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 // 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
29 // It takes care of cutting arcs into segments, same thing for line that are too long
32 this->inch_mode
= false;
33 this->absolute_mode
= true;
34 this->motion_mode
= MOTION_MODE_SEEK
;
35 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
36 clear_vector(this->current_position
);
37 clear_vector(this->last_milestone
);
38 this->arm_solution
= NULL
;
39 seconds_per_minute
= 60.0;
42 //Called when the module has just been loaded
43 void Robot::on_module_loaded() {
44 register_for_event(ON_CONFIG_RELOAD
);
45 this->register_for_event(ON_GCODE_RECEIVED
);
46 this->register_for_event(ON_GET_PUBLIC_DATA
);
47 this->register_for_event(ON_SET_PUBLIC_DATA
);
50 this->on_config_reload(this);
52 // Make our 3 StepperMotors
53 this->alpha_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&alpha_step_pin
,&alpha_dir_pin
,&alpha_en_pin
) );
54 this->beta_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&beta_step_pin
, &beta_dir_pin
, &beta_en_pin
) );
55 this->gamma_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&gamma_step_pin
,&gamma_dir_pin
,&gamma_en_pin
) );
59 void Robot::on_config_reload(void* argument
){
61 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
62 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
63 // To make adding those solution easier, they have their own, separate object.
64 // Here we read the config to find out which arm solution to use
65 if (this->arm_solution
) delete this->arm_solution
;
66 int solution_checksum
= get_checksum(this->kernel
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
67 // Note checksums are not const expressions when in debug mode, so don't use switch
68 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
69 this->arm_solution
= new HBotSolution(this->kernel
->config
);
71 }else if(solution_checksum
== rostock_checksum
) {
72 this->arm_solution
= new RostockSolution(this->kernel
->config
);
74 }else if(solution_checksum
== kossel_checksum
) {
75 this->arm_solution
= new JohannKosselSolution(this->kernel
->config
);
77 }else if(solution_checksum
== delta_checksum
) {
78 // place holder for now
79 this->arm_solution
= new RostockSolution(this->kernel
->config
);
81 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
82 this->arm_solution
= new RotatableCartesianSolution(this->kernel
->config
);
84 }else if(solution_checksum
== cartesian_checksum
) {
85 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
88 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
92 this->feed_rate
= this->kernel
->config
->value(default_feed_rate_checksum
)->by_default(100 )->as_number() / 60;
93 this->seek_rate
= this->kernel
->config
->value(default_seek_rate_checksum
)->by_default(100 )->as_number() / 60;
94 this->mm_per_line_segment
= this->kernel
->config
->value(mm_per_line_segment_checksum
)->by_default(0.0 )->as_number();
95 this->delta_segments_per_second
= this->kernel
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0 )->as_number();
96 this->mm_per_arc_segment
= this->kernel
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5 )->as_number();
97 this->arc_correction
= this->kernel
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
98 this->max_speeds
[X_AXIS
] = this->kernel
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
99 this->max_speeds
[Y_AXIS
] = this->kernel
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
100 this->max_speeds
[Z_AXIS
] = this->kernel
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
101 this->alpha_step_pin
.from_string( this->kernel
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
102 this->alpha_dir_pin
.from_string( this->kernel
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
103 this->alpha_en_pin
.from_string( this->kernel
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output();
104 this->beta_step_pin
.from_string( this->kernel
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
105 this->gamma_step_pin
.from_string( this->kernel
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
106 this->gamma_dir_pin
.from_string( this->kernel
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
107 this->gamma_en_pin
.from_string( this->kernel
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output();
108 this->beta_dir_pin
.from_string( this->kernel
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
109 this->beta_en_pin
.from_string( this->kernel
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output();
113 void Robot::on_get_public_data(void* argument
){
114 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
116 if(!pdr
->starts_with(robot_checksum
)) return;
118 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
119 static double return_data
;
120 return_data
= 100*this->seconds_per_minute
/60;
121 pdr
->set_data_ptr(&return_data
);
124 }else if(pdr
->second_element_is(current_position_checksum
)) {
125 static double return_data
[3];
126 return_data
[0]= from_millimeters(this->current_position
[0]);
127 return_data
[1]= from_millimeters(this->current_position
[1]);
128 return_data
[2]= from_millimeters(this->current_position
[2]);
130 pdr
->set_data_ptr(&return_data
);
135 void Robot::on_set_public_data(void* argument
){
136 PublicDataRequest
* pdr
= static_cast<PublicDataRequest
*>(argument
);
138 if(!pdr
->starts_with(robot_checksum
)) return;
140 if(pdr
->second_element_is(speed_override_percent_checksum
)) {
141 // NOTE do not use this while printing!
142 double t
= *static_cast<double*>(pdr
->get_data_ptr());
143 // enforce minimum 10% speed
144 if (t
< 10.0) t
= 10.0;
146 this->seconds_per_minute
= t
* 0.6;
151 //A GCode has been received
152 //See if the current Gcode line has some orders for us
153 void Robot::on_gcode_received(void * argument
){
154 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
156 //Temp variables, constant properties are stored in the object
157 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
158 this->motion_mode
= -1;
160 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
163 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
164 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
165 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
166 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
167 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
168 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
169 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
170 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
171 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
172 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
173 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
175 if(gcode
->get_num_args() == 0){
176 clear_vector(this->last_milestone
);
178 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
179 if ( gcode
->has_letter(letter
) )
180 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
183 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
184 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
185 gcode
->mark_as_taken();
186 return; // TODO: Wait until queue empty
189 }else if( gcode
->has_m
){
191 case 92: // M92 - set steps per mm
193 this->arm_solution
->get_steps_per_millimeter(steps
);
194 if (gcode
->has_letter('X'))
195 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
196 if (gcode
->has_letter('Y'))
197 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
198 if (gcode
->has_letter('Z'))
199 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
200 if (gcode
->has_letter('F'))
201 seconds_per_minute
= gcode
->get_value('F');
202 this->arm_solution
->set_steps_per_millimeter(steps
);
203 // update current position in steps
204 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
205 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
206 gcode
->add_nl
= true;
207 gcode
->mark_as_taken();
209 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
210 from_millimeters(this->current_position
[0]),
211 from_millimeters(this->current_position
[1]),
212 from_millimeters(this->current_position
[2]));
213 gcode
->add_nl
= true;
214 gcode
->mark_as_taken();
216 // case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
217 // gcode->mark_as_taken();
218 // if (gcode->has_letter('S'))
220 // double acc= gcode->get_value('S') * 60 * 60; // mm/min^2
221 // // enforce minimum
224 // this->kernel->planner->acceleration= acc;
228 case 220: // M220 - speed override percentage
229 gcode
->mark_as_taken();
230 if (gcode
->has_letter('S'))
232 double factor
= gcode
->get_value('S');
233 // enforce minimum 10% speed
236 seconds_per_minute
= factor
* 0.6;
240 case 665: // M665 set optional arm solution variables based on arm solution
241 gcode
->mark_as_taken();
242 // the parameter args could be any letter so try each one
243 for(char c
='A';c
<='Z';c
++) {
245 bool supported
= arm_solution
->get_optional(c
, &v
); // retrieve current value if supported
247 if(supported
&& gcode
->has_letter(c
)) { // set new value if supported
248 v
= gcode
->get_value(c
);
249 arm_solution
->set_optional(c
, v
);
251 if(supported
) { // print all current values of supported options
252 gcode
->stream
->printf("%c %8.3f ", c
, v
);
253 gcode
->add_nl
= true;
260 if( this->motion_mode
< 0)
264 double target
[3], offset
[3];
265 clear_vector(target
); clear_vector(offset
);
267 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
269 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
270 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']); } }
272 if( gcode
->has_letter('F') )
274 if( this->motion_mode
== MOTION_MODE_SEEK
)
275 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
277 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
280 //Perform any physical actions
281 switch( next_action
){
282 case NEXT_ACTION_DEFAULT
:
283 switch(this->motion_mode
){
284 case MOTION_MODE_CANCEL
: break;
285 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
286 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
287 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
292 // As far as the parser is concerned, the position is now == target. In reality the
293 // motion control system might still be processing the action and the real tool position
294 // in any intermediate location.
295 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
299 // We received a new gcode, and one of the functions
300 // determined the distance for that given gcode. So now we can attach this gcode to the right block
302 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
304 //If the queue is empty, execute immediatly, otherwise attach to the last added block
305 if( this->kernel
->conveyor
->queue
.size() == 0 ){
306 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
308 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
309 block
->append_gcode(gcode
);
314 // Reset the position for all axes ( used in homing and G92 stuff )
315 void Robot::reset_axis_position(double position
, int axis
) {
316 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
317 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
321 // Convert target from millimeters to steps, and append this to the planner
322 void Robot::append_milestone( double target
[], double rate
){
323 int steps
[3]; //Holds the result of the conversion
325 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
326 this->arm_solution
->millimeters_to_steps( target
, steps
);
329 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
331 // Compute how long this move moves, so we can attach it to the block for later use
332 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
334 // Do not move faster than the configured limits
335 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
336 if( this->max_speeds
[axis
] > 0 ){
337 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
338 if( axis_speed
> this->max_speeds
[axis
] ){
339 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
344 // Append the block to the planner
345 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
347 // Update the last_milestone to the current target for the next time we use last_milestone
348 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
352 // Append a move to the queue ( cutting it into segments if needed )
353 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
355 // Find out the distance for this gcode
356 gcode
->millimeters_of_travel
= sqrt( 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 ) );
358 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
359 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
361 // Mark the gcode as having a known distance
362 this->distance_in_gcode_is_known( gcode
);
364 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
365 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
366 // 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
369 if(this->delta_segments_per_second
> 1.0) {
370 // enabled if set to something > 1, it is set to 0.0 by default
371 // segment based on current speed and requested segments per second
372 // the faster the travel speed the fewer segments needed
373 // NOTE rate is mm/sec and we take into account any speed override
374 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
375 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
376 // TODO if we are only moving in Z on a delta we don't really need to segment at all
379 if(this->mm_per_line_segment
== 0.0){
380 segments
= 1; // don't split it up
382 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
386 // A vector to keep track of the endpoint of each segment
387 double temp_target
[3];
389 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
392 for( int i
=0; i
<segments
-1; i
++ ){
393 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
394 // Append the end of this segment to the queue
395 this->append_milestone(temp_target
, rate
);
398 // Append the end of this full move to the queue
399 this->append_milestone(target
, rate
);
403 // Append an arc to the queue ( cutting it into segments as needed )
404 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
407 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
408 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
409 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
410 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
411 double r_axis1
= -offset
[this->plane_axis_1
];
412 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
413 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
415 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
416 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
417 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
418 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
420 // Find the distance for this gcode
421 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
423 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
424 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
426 // Mark the gcode as having a known distance
427 this->distance_in_gcode_is_known( gcode
);
429 // Figure out how many segments for this gcode
430 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
432 double theta_per_segment
= angular_travel
/segments
;
433 double linear_per_segment
= linear_travel
/segments
;
435 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
436 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
437 r_T = [cos(phi) -sin(phi);
438 sin(phi) cos(phi] * r ;
439 For arc generation, the center of the circle is the axis of rotation and the radius vector is
440 defined from the circle center to the initial position. Each line segment is formed by successive
441 vector rotations. This requires only two cos() and sin() computations to form the rotation
442 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
443 all double numbers are single precision on the Arduino. (True double precision will not have
444 round off issues for CNC applications.) Single precision error can accumulate to be greater than
445 tool precision in some cases. Therefore, arc path correction is implemented.
447 Small angle approximation may be used to reduce computation overhead further. This approximation
448 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
449 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
450 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
451 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
452 issue for CNC machines with the single precision Arduino calculations.
453 This approximation also allows mc_arc to immediately insert a line segment into the planner
454 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
455 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
456 This is important when there are successive arc motions.
458 // Vector rotation matrix values
459 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
460 double sin_T
= theta_per_segment
;
462 double arc_target
[3];
469 // Initialize the linear axis
470 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
472 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
474 if (count
< this->arc_correction
) {
475 // Apply vector rotation matrix
476 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
477 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
481 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
482 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
483 cos_Ti
= cos(i
*theta_per_segment
);
484 sin_Ti
= sin(i
*theta_per_segment
);
485 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
486 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
490 // Update arc_target location
491 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
492 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
493 arc_target
[this->plane_axis_2
] += linear_per_segment
;
495 // Append this segment to the queue
496 this->append_milestone(arc_target
, this->feed_rate
);
500 // Ensure last segment arrives at target location.
501 this->append_milestone(target
, this->feed_rate
);
504 // Do the math for an arc and add it to the queue
505 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
508 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
510 // Set clockwise/counter-clockwise sign for mc_arc computations
511 bool is_clockwise
= false;
512 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
515 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
520 double Robot::theta(double x
, double y
){
521 double t
= atan(x
/fabs(y
));
522 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
525 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
526 this->plane_axis_0
= axis_0
;
527 this->plane_axis_1
= axis_1
;
528 this->plane_axis_2
= axis_2
;