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();
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();
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();
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
; gcode
->mark_as_taken(); break;
119 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
120 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
121 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
122 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
123 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
124 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
125 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
126 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
127 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
128 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); 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 gcode
->mark_as_taken();
141 return; // TODO: Wait until queue empty
144 }else if( gcode
->has_m
){
146 case 92: // M92 - set steps per mm
148 this->arm_solution
->get_steps_per_millimeter(steps
);
149 if (gcode
->has_letter('X'))
150 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
151 if (gcode
->has_letter('Y'))
152 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
153 if (gcode
->has_letter('Z'))
154 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
155 if (gcode
->has_letter('F'))
156 seconds_per_minute
= gcode
->get_value('F');
157 this->arm_solution
->set_steps_per_millimeter(steps
);
158 // update current position in steps
159 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
160 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
161 gcode
->add_nl
= true;
162 gcode
->mark_as_taken();
164 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
165 from_millimeters(this->current_position
[0]),
166 from_millimeters(this->current_position
[1]),
167 from_millimeters(this->current_position
[2]));
168 gcode
->add_nl
= true;
169 gcode
->mark_as_taken();
171 case 220: // M220 - speed override percentage
172 gcode
->mark_as_taken();
173 if (gcode
->has_letter('S'))
175 double factor
= gcode
->get_value('S');
176 // enforce minimum 1% speed
179 seconds_per_minute
= factor
* 0.6;
183 if( this->motion_mode
< 0)
187 double target
[3], offset
[3];
188 clear_vector(target
); clear_vector(offset
);
190 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
192 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
193 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']); } }
195 if( gcode
->has_letter('F') )
197 if( this->motion_mode
== MOTION_MODE_SEEK
)
198 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
200 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
203 //Perform any physical actions
204 switch( next_action
){
205 case NEXT_ACTION_DEFAULT
:
206 switch(this->motion_mode
){
207 case MOTION_MODE_CANCEL
: break;
208 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
209 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
210 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
215 // As far as the parser is concerned, the position is now == target. In reality the
216 // motion control system might still be processing the action and the real tool position
217 // in any intermediate location.
218 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
222 // We received a new gcode, and one of the functions
223 // determined the distance for that given gcode. So now we can attach this gcode to the right block
225 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
227 //If the queue is empty, execute immediatly, otherwise attach to the last added block
228 if( this->kernel
->conveyor
->queue
.size() == 0 ){
229 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
231 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
232 block
->append_gcode(gcode
);
237 // Reset the position for all axes ( used in homing and G92 stuff )
238 void Robot::reset_axis_position(double position
, int axis
) {
239 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
240 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
244 // Convert target from millimeters to steps, and append this to the planner
245 void Robot::append_milestone( double target
[], double rate
){
246 int steps
[3]; //Holds the result of the conversion
248 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
249 this->arm_solution
->millimeters_to_steps( target
, steps
);
252 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
254 // Compute how long this move moves, so we can attach it to the block for later use
255 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
257 // Do not move faster than the configured limits
258 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
259 if( this->max_speeds
[axis
] > 0 ){
260 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
261 if( axis_speed
> this->max_speeds
[axis
] ){
262 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
267 // Append the block to the planner
268 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
270 // Update the last_milestone to the current target for the next time we use last_milestone
271 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
275 // Append a move to the queue ( cutting it into segments if needed )
276 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
278 // Find out the distance for this gcode
279 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 ) );
281 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
282 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
284 // Mark the gcode as having a known distance
285 this->distance_in_gcode_is_known( gcode
);
287 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
288 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
289 // 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
292 if(this->delta_segments_per_second
> 1.0) {
293 // enabled if set to something > 1, it is set to 0.0 by default
294 // segment based on current speed and requested segments per second
295 // the faster the travel speed the fewer segments needed
296 // NOTE rate is mm/sec and we take into account any speed override
297 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
298 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
299 // TODO if we are only moving in Z on a delta we don't really need to segment at all
302 if(this->mm_per_line_segment
== 0.0){
303 segments
= 1; // don't split it up
305 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
309 // A vector to keep track of the endpoint of each segment
310 double temp_target
[3];
312 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
315 for( int i
=0; i
<segments
-1; i
++ ){
316 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
317 // Append the end of this segment to the queue
318 this->append_milestone(temp_target
, rate
);
321 // Append the end of this full move to the queue
322 this->append_milestone(target
, rate
);
326 // Append an arc to the queue ( cutting it into segments as needed )
327 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
330 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
331 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
332 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
333 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
334 double r_axis1
= -offset
[this->plane_axis_1
];
335 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
336 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
338 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
339 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
340 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
341 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
343 // Find the distance for this gcode
344 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
346 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
347 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
349 // Mark the gcode as having a known distance
350 this->distance_in_gcode_is_known( gcode
);
352 // Figure out how many segments for this gcode
353 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
355 double theta_per_segment
= angular_travel
/segments
;
356 double linear_per_segment
= linear_travel
/segments
;
358 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
359 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
360 r_T = [cos(phi) -sin(phi);
361 sin(phi) cos(phi] * r ;
362 For arc generation, the center of the circle is the axis of rotation and the radius vector is
363 defined from the circle center to the initial position. Each line segment is formed by successive
364 vector rotations. This requires only two cos() and sin() computations to form the rotation
365 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
366 all double numbers are single precision on the Arduino. (True double precision will not have
367 round off issues for CNC applications.) Single precision error can accumulate to be greater than
368 tool precision in some cases. Therefore, arc path correction is implemented.
370 Small angle approximation may be used to reduce computation overhead further. This approximation
371 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
372 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
373 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
374 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
375 issue for CNC machines with the single precision Arduino calculations.
376 This approximation also allows mc_arc to immediately insert a line segment into the planner
377 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
378 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
379 This is important when there are successive arc motions.
381 // Vector rotation matrix values
382 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
383 double sin_T
= theta_per_segment
;
385 double arc_target
[3];
392 // Initialize the linear axis
393 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
395 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
397 if (count
< this->arc_correction
) {
398 // Apply vector rotation matrix
399 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
400 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
404 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
405 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
406 cos_Ti
= cos(i
*theta_per_segment
);
407 sin_Ti
= sin(i
*theta_per_segment
);
408 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
409 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
413 // Update arc_target location
414 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
415 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
416 arc_target
[this->plane_axis_2
] += linear_per_segment
;
418 // Append this segment to the queue
419 this->append_milestone(arc_target
, this->feed_rate
);
423 // Ensure last segment arrives at target location.
424 this->append_milestone(target
, this->feed_rate
);
427 // Do the math for an arc and add it to the queue
428 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
431 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
433 // Set clockwise/counter-clockwise sign for mc_arc computations
434 bool is_clockwise
= false;
435 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
438 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
443 double Robot::theta(double x
, double y
){
444 double t
= atan(x
/fabs(y
));
445 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
448 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
449 this->plane_axis_0
= axis_0
;
450 this->plane_axis_1
= axis_1
;
451 this->plane_axis_2
= axis_2
;