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;
241 if( this->motion_mode
< 0)
245 double target
[3], offset
[3];
246 clear_vector(target
); clear_vector(offset
);
248 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
250 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
251 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']); } }
253 if( gcode
->has_letter('F') )
255 if( this->motion_mode
== MOTION_MODE_SEEK
)
256 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
258 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
261 //Perform any physical actions
262 switch( next_action
){
263 case NEXT_ACTION_DEFAULT
:
264 switch(this->motion_mode
){
265 case MOTION_MODE_CANCEL
: break;
266 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
267 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
268 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
273 // As far as the parser is concerned, the position is now == target. In reality the
274 // motion control system might still be processing the action and the real tool position
275 // in any intermediate location.
276 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
280 // We received a new gcode, and one of the functions
281 // determined the distance for that given gcode. So now we can attach this gcode to the right block
283 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
285 //If the queue is empty, execute immediatly, otherwise attach to the last added block
286 if( this->kernel
->conveyor
->queue
.size() == 0 ){
287 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
289 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
290 block
->append_gcode(gcode
);
295 // Reset the position for all axes ( used in homing and G92 stuff )
296 void Robot::reset_axis_position(double position
, int axis
) {
297 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
298 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
302 // Convert target from millimeters to steps, and append this to the planner
303 void Robot::append_milestone( double target
[], double rate
){
304 int steps
[3]; //Holds the result of the conversion
306 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
307 this->arm_solution
->millimeters_to_steps( target
, steps
);
310 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
312 // Compute how long this move moves, so we can attach it to the block for later use
313 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
315 // Do not move faster than the configured limits
316 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
317 if( this->max_speeds
[axis
] > 0 ){
318 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
319 if( axis_speed
> this->max_speeds
[axis
] ){
320 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
325 // Append the block to the planner
326 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
328 // Update the last_milestone to the current target for the next time we use last_milestone
329 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
333 // Append a move to the queue ( cutting it into segments if needed )
334 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
336 // Find out the distance for this gcode
337 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 ) );
339 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
340 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
342 // Mark the gcode as having a known distance
343 this->distance_in_gcode_is_known( gcode
);
345 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
346 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
347 // 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
350 if(this->delta_segments_per_second
> 1.0) {
351 // enabled if set to something > 1, it is set to 0.0 by default
352 // segment based on current speed and requested segments per second
353 // the faster the travel speed the fewer segments needed
354 // NOTE rate is mm/sec and we take into account any speed override
355 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
356 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
357 // TODO if we are only moving in Z on a delta we don't really need to segment at all
360 if(this->mm_per_line_segment
== 0.0){
361 segments
= 1; // don't split it up
363 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
367 // A vector to keep track of the endpoint of each segment
368 double temp_target
[3];
370 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
373 for( int i
=0; i
<segments
-1; i
++ ){
374 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
375 // Append the end of this segment to the queue
376 this->append_milestone(temp_target
, rate
);
379 // Append the end of this full move to the queue
380 this->append_milestone(target
, rate
);
384 // Append an arc to the queue ( cutting it into segments as needed )
385 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
388 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
389 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
390 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
391 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
392 double r_axis1
= -offset
[this->plane_axis_1
];
393 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
394 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
396 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
397 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
398 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
399 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
401 // Find the distance for this gcode
402 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
404 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
405 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
407 // Mark the gcode as having a known distance
408 this->distance_in_gcode_is_known( gcode
);
410 // Figure out how many segments for this gcode
411 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
413 double theta_per_segment
= angular_travel
/segments
;
414 double linear_per_segment
= linear_travel
/segments
;
416 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
417 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
418 r_T = [cos(phi) -sin(phi);
419 sin(phi) cos(phi] * r ;
420 For arc generation, the center of the circle is the axis of rotation and the radius vector is
421 defined from the circle center to the initial position. Each line segment is formed by successive
422 vector rotations. This requires only two cos() and sin() computations to form the rotation
423 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
424 all double numbers are single precision on the Arduino. (True double precision will not have
425 round off issues for CNC applications.) Single precision error can accumulate to be greater than
426 tool precision in some cases. Therefore, arc path correction is implemented.
428 Small angle approximation may be used to reduce computation overhead further. This approximation
429 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
430 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
431 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
432 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
433 issue for CNC machines with the single precision Arduino calculations.
434 This approximation also allows mc_arc to immediately insert a line segment into the planner
435 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
436 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
437 This is important when there are successive arc motions.
439 // Vector rotation matrix values
440 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
441 double sin_T
= theta_per_segment
;
443 double arc_target
[3];
450 // Initialize the linear axis
451 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
453 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
455 if (count
< this->arc_correction
) {
456 // Apply vector rotation matrix
457 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
458 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
462 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
463 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
464 cos_Ti
= cos(i
*theta_per_segment
);
465 sin_Ti
= sin(i
*theta_per_segment
);
466 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
467 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
471 // Update arc_target location
472 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
473 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
474 arc_target
[this->plane_axis_2
] += linear_per_segment
;
476 // Append this segment to the queue
477 this->append_milestone(arc_target
, this->feed_rate
);
481 // Ensure last segment arrives at target location.
482 this->append_milestone(target
, this->feed_rate
);
485 // Do the math for an arc and add it to the queue
486 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
489 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
491 // Set clockwise/counter-clockwise sign for mc_arc computations
492 bool is_clockwise
= false;
493 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
496 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
501 double Robot::theta(double x
, double y
){
502 double t
= atan(x
/fabs(y
));
503 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
506 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
507 this->plane_axis_0
= axis_0
;
508 this->plane_axis_1
= axis_1
;
509 this->plane_axis_2
= axis_2
;