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
) {
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 double t
= *static_cast<double*>(pdr
->get_data_ptr());
142 this->seconds_per_minute
= t
* 0.6;
147 //A GCode has been received
148 //See if the current Gcode line has some orders for us
149 void Robot::on_gcode_received(void * argument
){
150 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
152 //Temp variables, constant properties are stored in the object
153 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
154 this->motion_mode
= -1;
156 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
159 case 0: this->motion_mode
= MOTION_MODE_SEEK
; gcode
->mark_as_taken(); break;
160 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
161 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
162 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
163 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
164 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
165 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
166 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
167 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
168 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
169 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
171 if(gcode
->get_num_args() == 0){
172 clear_vector(this->last_milestone
);
174 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
175 if ( gcode
->has_letter(letter
) )
176 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
179 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
180 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
181 gcode
->mark_as_taken();
182 return; // TODO: Wait until queue empty
185 }else if( gcode
->has_m
){
187 case 92: // M92 - set steps per mm
189 this->arm_solution
->get_steps_per_millimeter(steps
);
190 if (gcode
->has_letter('X'))
191 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
192 if (gcode
->has_letter('Y'))
193 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
194 if (gcode
->has_letter('Z'))
195 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
196 if (gcode
->has_letter('F'))
197 seconds_per_minute
= gcode
->get_value('F');
198 this->arm_solution
->set_steps_per_millimeter(steps
);
199 // update current position in steps
200 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
201 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
202 gcode
->add_nl
= true;
203 gcode
->mark_as_taken();
205 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
206 from_millimeters(this->current_position
[0]),
207 from_millimeters(this->current_position
[1]),
208 from_millimeters(this->current_position
[2]));
209 gcode
->add_nl
= true;
210 gcode
->mark_as_taken();
212 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
213 gcode
->mark_as_taken();
214 if (gcode
->has_letter('S'))
216 double acc
= gcode
->get_value('S');
220 this->kernel
->planner
->acceleration
= acc
;
224 case 220: // M220 - speed override percentage
225 gcode
->mark_as_taken();
226 if (gcode
->has_letter('S'))
228 double factor
= gcode
->get_value('S');
229 // enforce minimum 1% speed
232 seconds_per_minute
= factor
* 0.6;
237 if( this->motion_mode
< 0)
241 double target
[3], offset
[3];
242 clear_vector(target
); clear_vector(offset
);
244 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
246 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
247 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']); } }
249 if( gcode
->has_letter('F') )
251 if( this->motion_mode
== MOTION_MODE_SEEK
)
252 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
254 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
257 //Perform any physical actions
258 switch( next_action
){
259 case NEXT_ACTION_DEFAULT
:
260 switch(this->motion_mode
){
261 case MOTION_MODE_CANCEL
: break;
262 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
263 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
264 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
269 // As far as the parser is concerned, the position is now == target. In reality the
270 // motion control system might still be processing the action and the real tool position
271 // in any intermediate location.
272 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
276 // We received a new gcode, and one of the functions
277 // determined the distance for that given gcode. So now we can attach this gcode to the right block
279 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
281 //If the queue is empty, execute immediatly, otherwise attach to the last added block
282 if( this->kernel
->conveyor
->queue
.size() == 0 ){
283 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
285 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
286 block
->append_gcode(gcode
);
291 // Reset the position for all axes ( used in homing and G92 stuff )
292 void Robot::reset_axis_position(double position
, int axis
) {
293 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
294 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
298 // Convert target from millimeters to steps, and append this to the planner
299 void Robot::append_milestone( double target
[], double rate
){
300 int steps
[3]; //Holds the result of the conversion
302 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
303 this->arm_solution
->millimeters_to_steps( target
, steps
);
306 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
308 // Compute how long this move moves, so we can attach it to the block for later use
309 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
311 // Do not move faster than the configured limits
312 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
313 if( this->max_speeds
[axis
] > 0 ){
314 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
315 if( axis_speed
> this->max_speeds
[axis
] ){
316 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
321 // Append the block to the planner
322 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
324 // Update the last_milestone to the current target for the next time we use last_milestone
325 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
329 // Append a move to the queue ( cutting it into segments if needed )
330 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
332 // Find out the distance for this gcode
333 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 ) );
335 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
336 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
338 // Mark the gcode as having a known distance
339 this->distance_in_gcode_is_known( gcode
);
341 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
342 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
343 // 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
346 if(this->delta_segments_per_second
> 1.0) {
347 // enabled if set to something > 1, it is set to 0.0 by default
348 // segment based on current speed and requested segments per second
349 // the faster the travel speed the fewer segments needed
350 // NOTE rate is mm/sec and we take into account any speed override
351 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
352 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
353 // TODO if we are only moving in Z on a delta we don't really need to segment at all
356 if(this->mm_per_line_segment
== 0.0){
357 segments
= 1; // don't split it up
359 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
363 // A vector to keep track of the endpoint of each segment
364 double temp_target
[3];
366 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
369 for( int i
=0; i
<segments
-1; i
++ ){
370 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
371 // Append the end of this segment to the queue
372 this->append_milestone(temp_target
, rate
);
375 // Append the end of this full move to the queue
376 this->append_milestone(target
, rate
);
380 // Append an arc to the queue ( cutting it into segments as needed )
381 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
384 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
385 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
386 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
387 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
388 double r_axis1
= -offset
[this->plane_axis_1
];
389 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
390 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
392 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
393 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
394 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
395 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
397 // Find the distance for this gcode
398 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
400 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
401 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
403 // Mark the gcode as having a known distance
404 this->distance_in_gcode_is_known( gcode
);
406 // Figure out how many segments for this gcode
407 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
409 double theta_per_segment
= angular_travel
/segments
;
410 double linear_per_segment
= linear_travel
/segments
;
412 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
413 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
414 r_T = [cos(phi) -sin(phi);
415 sin(phi) cos(phi] * r ;
416 For arc generation, the center of the circle is the axis of rotation and the radius vector is
417 defined from the circle center to the initial position. Each line segment is formed by successive
418 vector rotations. This requires only two cos() and sin() computations to form the rotation
419 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
420 all double numbers are single precision on the Arduino. (True double precision will not have
421 round off issues for CNC applications.) Single precision error can accumulate to be greater than
422 tool precision in some cases. Therefore, arc path correction is implemented.
424 Small angle approximation may be used to reduce computation overhead further. This approximation
425 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
426 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
427 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
428 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
429 issue for CNC machines with the single precision Arduino calculations.
430 This approximation also allows mc_arc to immediately insert a line segment into the planner
431 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
432 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
433 This is important when there are successive arc motions.
435 // Vector rotation matrix values
436 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
437 double sin_T
= theta_per_segment
;
439 double arc_target
[3];
446 // Initialize the linear axis
447 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
449 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
451 if (count
< this->arc_correction
) {
452 // Apply vector rotation matrix
453 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
454 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
458 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
459 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
460 cos_Ti
= cos(i
*theta_per_segment
);
461 sin_Ti
= sin(i
*theta_per_segment
);
462 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
463 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
467 // Update arc_target location
468 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
469 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
470 arc_target
[this->plane_axis_2
] += linear_per_segment
;
472 // Append this segment to the queue
473 this->append_milestone(arc_target
, this->feed_rate
);
477 // Ensure last segment arrives at target location.
478 this->append_milestone(target
, this->feed_rate
);
481 // Do the math for an arc and add it to the queue
482 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
485 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
487 // Set clockwise/counter-clockwise sign for mc_arc computations
488 bool is_clockwise
= false;
489 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
492 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
497 double Robot::theta(double x
, double y
){
498 double t
= atan(x
/fabs(y
));
499 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
502 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
503 this->plane_axis_0
= axis_0
;
504 this->plane_axis_1
= axis_1
;
505 this->plane_axis_2
= axis_2
;