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 "arm_solutions/BaseSolution.h"
21 #include "arm_solutions/CartesianSolution.h"
22 #include "arm_solutions/RotatableCartesianSolution.h"
23 #include "arm_solutions/RostockSolution.h"
24 #include "arm_solutions/HBotSolution.h"
26 // 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
27 // It takes care of cutting arcs into segments, same thing for line that are too long
30 this->inch_mode
= false;
31 this->absolute_mode
= true;
32 this->motion_mode
= MOTION_MODE_SEEK
;
33 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
34 clear_vector(this->current_position
);
35 clear_vector(this->last_milestone
);
36 this->arm_solution
= NULL
;
37 seconds_per_minute
= 60.0;
40 //Called when the module has just been loaded
41 void Robot::on_module_loaded() {
42 register_for_event(ON_CONFIG_RELOAD
);
43 this->register_for_event(ON_GCODE_RECEIVED
);
46 this->on_config_reload(this);
48 // Make our 3 StepperMotors
49 this->alpha_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&alpha_step_pin
,&alpha_dir_pin
,&alpha_en_pin
) );
50 this->beta_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&beta_step_pin
, &beta_dir_pin
, &beta_en_pin
) );
51 this->gamma_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&gamma_step_pin
,&gamma_dir_pin
,&gamma_en_pin
) );
55 void Robot::on_config_reload(void* argument
){
57 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
58 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
59 // To make adding those solution easier, they have their own, separate object.
60 // Here we read the config to find out which arm solution to use
61 if (this->arm_solution
) delete this->arm_solution
;
62 int solution_checksum
= get_checksum(this->kernel
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
63 // Note checksums are not const expressions when in debug mode, so don't use switch
64 if(solution_checksum
== hbot_checksum
) {
65 this->arm_solution
= new HBotSolution(this->kernel
->config
);
67 }else if(solution_checksum
== rostock_checksum
) {
68 this->arm_solution
= new RostockSolution(this->kernel
->config
);
70 }else if(solution_checksum
== delta_checksum
) {
71 // place holder for now
72 this->arm_solution
= new RostockSolution(this->kernel
->config
);
74 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
75 this->arm_solution
= new RotatableCartesianSolution(this->kernel
->config
);
77 }else if(solution_checksum
== cartesian_checksum
) {
78 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
81 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
85 this->feed_rate
= this->kernel
->config
->value(default_feed_rate_checksum
)->by_default(100 )->as_number() / 60;
86 this->seek_rate
= this->kernel
->config
->value(default_seek_rate_checksum
)->by_default(100 )->as_number() / 60;
87 this->mm_per_line_segment
= this->kernel
->config
->value(mm_per_line_segment_checksum
)->by_default(0.0 )->as_number();
88 this->delta_segments_per_second
= this->kernel
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0 )->as_number();
89 this->mm_per_arc_segment
= this->kernel
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5 )->as_number();
90 this->arc_correction
= this->kernel
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
91 this->max_speeds
[X_AXIS
] = this->kernel
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
92 this->max_speeds
[Y_AXIS
] = this->kernel
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
93 this->max_speeds
[Z_AXIS
] = this->kernel
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
94 this->alpha_step_pin
.from_string( this->kernel
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
95 this->alpha_dir_pin
.from_string( this->kernel
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
96 this->alpha_en_pin
.from_string( this->kernel
->config
->value(alpha_en_pin_checksum
)->by_default("0.4" )->as_string())->as_output()->as_open_drain();
97 this->beta_step_pin
.from_string( this->kernel
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
98 this->gamma_step_pin
.from_string( this->kernel
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
99 this->gamma_dir_pin
.from_string( this->kernel
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
100 this->gamma_en_pin
.from_string( this->kernel
->config
->value(gamma_en_pin_checksum
)->by_default("0.19" )->as_string())->as_output()->as_open_drain();
101 this->beta_dir_pin
.from_string( this->kernel
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
102 this->beta_en_pin
.from_string( this->kernel
->config
->value(beta_en_pin_checksum
)->by_default("0.10" )->as_string())->as_output()->as_open_drain();
106 //A GCode has been received
107 //See if the current Gcode line has some orders for us
108 void Robot::on_gcode_received(void * argument
){
109 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
111 //Temp variables, constant properties are stored in the object
112 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
113 this->motion_mode
= -1;
115 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
118 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
119 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
120 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
121 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
122 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
123 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
124 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
125 case 20: this->inch_mode
= true; break;
126 case 21: this->inch_mode
= false; break;
127 case 90: this->absolute_mode
= true; break;
128 case 91: this->absolute_mode
= false; break;
130 if(gcode
->get_num_args() == 0){
131 clear_vector(this->last_milestone
);
133 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
134 if ( gcode
->has_letter(letter
) )
135 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
138 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
139 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
140 return; // TODO: Wait until queue empty
143 }else if( gcode
->has_m
){
145 case 92: // M92 - set steps per mm
147 this->arm_solution
->get_steps_per_millimeter(steps
);
148 if (gcode
->has_letter('X'))
149 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
150 if (gcode
->has_letter('Y'))
151 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
152 if (gcode
->has_letter('Z'))
153 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
154 if (gcode
->has_letter('F'))
155 seconds_per_minute
= gcode
->get_value('F');
156 this->arm_solution
->set_steps_per_millimeter(steps
);
157 // update current position in steps
158 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
159 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
160 gcode
->add_nl
= true;
162 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
163 this->current_position
[0],
164 this->current_position
[1],
165 this->current_position
[2]);
166 gcode
->add_nl
= true;
168 case 220: // M220 - speed override percentage
169 if (gcode
->has_letter('S'))
171 double factor
= gcode
->get_value('S');
172 // enforce minimum 1% speed
175 seconds_per_minute
= factor
* 0.6;
179 if( this->motion_mode
< 0)
183 double target
[3], offset
[3];
184 clear_vector(target
); clear_vector(offset
);
186 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
188 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
189 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']); } }
191 if( gcode
->has_letter('F') )
193 if( this->motion_mode
== MOTION_MODE_SEEK
)
194 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
196 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
199 //Perform any physical actions
200 switch( next_action
){
201 case NEXT_ACTION_DEFAULT
:
202 switch(this->motion_mode
){
203 case MOTION_MODE_CANCEL
: break;
204 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
205 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
206 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
211 // As far as the parser is concerned, the position is now == target. In reality the
212 // motion control system might still be processing the action and the real tool position
213 // in any intermediate location.
214 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
221 // We received a new gcode, and one of the functions
222 // determined the distance for that given gcode. So now we can attach this gcode to the right block
224 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
226 //If the queue is empty, execute immediatly, otherwise attach to the last added block
227 if( this->kernel
->conveyor
->queue
.size() == 0 ){
228 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
230 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
231 block
->append_gcode(gcode
);
236 // Reset the position for all axes ( used in homing and G92 stuff )
237 void Robot::reset_axis_position(double position
, int axis
) {
238 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
239 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
243 // Convert target from millimeters to steps, and append this to the planner
244 void Robot::append_milestone( double target
[], double rate
){
245 int steps
[3]; //Holds the result of the conversion
247 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
248 this->arm_solution
->millimeters_to_steps( target
, steps
);
251 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
253 // Compute how long this move moves, so we can attach it to the block for later use
254 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
256 // Do not move faster than the configured limits
257 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
258 if( this->max_speeds
[axis
] > 0 ){
259 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
260 if( axis_speed
> this->max_speeds
[axis
] ){
261 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
266 // Append the block to the planner
267 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
269 // Update the last_milestone to the current target for the next time we use last_milestone
270 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
274 // Append a move to the queue ( cutting it into segments if needed )
275 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
277 // Find out the distance for this gcode
278 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 ) );
280 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
281 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
283 // Mark the gcode as having a known distance
284 this->distance_in_gcode_is_known( gcode
);
286 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
287 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
288 // 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
291 if(this->delta_segments_per_second
> 1.0) {
292 // enabled if set to something > 1, it is set to 0.0 by default
293 // segment based on current speed and requested segments per second
294 // the faster the travel speed the fewer segments needed
295 // NOTE rate is mm/sec and we take into account any speed override
296 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
297 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
298 // TODO if we are only moving in Z on a delta we don't really need to segment at all
301 if(this->mm_per_line_segment
== 0.0){
302 segments
= 1; // don't split it up
304 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
308 // A vector to keep track of the endpoint of each segment
309 double temp_target
[3];
311 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
314 for( int i
=0; i
<segments
-1; i
++ ){
315 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
316 // Append the end of this segment to the queue
317 this->append_milestone(temp_target
, rate
);
320 // Append the end of this full move to the queue
321 this->append_milestone(target
, rate
);
325 // Append an arc to the queue ( cutting it into segments as needed )
326 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
329 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
330 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
331 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
332 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
333 double r_axis1
= -offset
[this->plane_axis_1
];
334 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
335 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
337 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
338 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
339 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
340 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
342 // Find the distance for this gcode
343 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
345 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
346 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
348 // Mark the gcode as having a known distance
349 this->distance_in_gcode_is_known( gcode
);
351 // Figure out how many segments for this gcode
352 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
354 double theta_per_segment
= angular_travel
/segments
;
355 double linear_per_segment
= linear_travel
/segments
;
357 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
358 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
359 r_T = [cos(phi) -sin(phi);
360 sin(phi) cos(phi] * r ;
361 For arc generation, the center of the circle is the axis of rotation and the radius vector is
362 defined from the circle center to the initial position. Each line segment is formed by successive
363 vector rotations. This requires only two cos() and sin() computations to form the rotation
364 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
365 all double numbers are single precision on the Arduino. (True double precision will not have
366 round off issues for CNC applications.) Single precision error can accumulate to be greater than
367 tool precision in some cases. Therefore, arc path correction is implemented.
369 Small angle approximation may be used to reduce computation overhead further. This approximation
370 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
371 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
372 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
373 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
374 issue for CNC machines with the single precision Arduino calculations.
375 This approximation also allows mc_arc to immediately insert a line segment into the planner
376 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
377 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
378 This is important when there are successive arc motions.
380 // Vector rotation matrix values
381 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
382 double sin_T
= theta_per_segment
;
384 double arc_target
[3];
391 // Initialize the linear axis
392 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
394 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
396 if (count
< this->arc_correction
) {
397 // Apply vector rotation matrix
398 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
399 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
403 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
404 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
405 cos_Ti
= cos(i
*theta_per_segment
);
406 sin_Ti
= sin(i
*theta_per_segment
);
407 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
408 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
412 // Update arc_target location
413 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
414 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
415 arc_target
[this->plane_axis_2
] += linear_per_segment
;
417 // Append this segment to the queue
418 this->append_milestone(arc_target
, this->feed_rate
);
422 // Ensure last segment arrives at target location.
423 this->append_milestone(target
, this->feed_rate
);
426 // Do the math for an arc and add it to the queue
427 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
430 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
432 // Set clockwise/counter-clockwise sign for mc_arc computations
433 bool is_clockwise
= false;
434 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
437 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
442 // Convert from inches to millimeters ( our internal storage unit ) if needed
443 inline double Robot::to_millimeters( double value
){
444 return this->inch_mode
? value
/25.4 : value
;
447 double Robot::theta(double x
, double y
){
448 double t
= atan(x
/fabs(y
));
449 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
452 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
453 this->plane_axis_0
= axis_0
;
454 this->plane_axis_1
= axis_1
;
455 this->plane_axis_2
= axis_2
;