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;
120 if( Touchprobe::enabled
){
121 this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken();
123 // First wait for the queue to be empty
124 this->kernel
->conveyor
->wait_for_empty_queue();
126 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; gcode
->mark_as_taken(); break;
127 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; gcode
->mark_as_taken(); break;
128 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; gcode
->mark_as_taken(); break;
129 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); gcode
->mark_as_taken(); break;
130 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); gcode
->mark_as_taken(); break;
131 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); gcode
->mark_as_taken(); break;
132 case 20: this->inch_mode
= true; gcode
->mark_as_taken(); break;
133 case 21: this->inch_mode
= false; gcode
->mark_as_taken(); break;
134 case 90: this->absolute_mode
= true; gcode
->mark_as_taken(); break;
135 case 91: this->absolute_mode
= false; gcode
->mark_as_taken(); break;
137 if(gcode
->get_num_args() == 0){
138 clear_vector(this->last_milestone
);
140 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
141 if ( gcode
->has_letter(letter
) )
142 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
145 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
146 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
147 gcode
->mark_as_taken();
148 return; // TODO: Wait until queue empty
151 }else if( gcode
->has_m
){
153 case 92: // M92 - set steps per mm
155 this->arm_solution
->get_steps_per_millimeter(steps
);
156 if (gcode
->has_letter('X'))
157 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
158 if (gcode
->has_letter('Y'))
159 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
160 if (gcode
->has_letter('Z'))
161 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
162 if (gcode
->has_letter('F'))
163 seconds_per_minute
= gcode
->get_value('F');
164 this->arm_solution
->set_steps_per_millimeter(steps
);
165 // update current position in steps
166 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
167 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
168 gcode
->add_nl
= true;
169 gcode
->mark_as_taken();
171 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
172 from_millimeters(this->current_position
[0]),
173 from_millimeters(this->current_position
[1]),
174 from_millimeters(this->current_position
[2]));
175 gcode
->add_nl
= true;
176 gcode
->mark_as_taken();
178 case 220: // M220 - speed override percentage
179 gcode
->mark_as_taken();
180 if (gcode
->has_letter('S'))
182 double factor
= gcode
->get_value('S');
183 // enforce minimum 1% speed
186 seconds_per_minute
= factor
* 0.6;
190 if( this->motion_mode
< 0)
194 double target
[3], offset
[3];
195 clear_vector(target
); clear_vector(offset
);
197 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
199 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
200 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']); } }
202 if( gcode
->has_letter('F') )
204 if( this->motion_mode
== MOTION_MODE_SEEK
)
205 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
207 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
210 //Perform any physical actions
211 switch( next_action
){
212 case NEXT_ACTION_DEFAULT
:
213 switch(this->motion_mode
){
214 case MOTION_MODE_CANCEL
: break;
215 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
216 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
217 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
222 // As far as the parser is concerned, the position is now == target. In reality the
223 // motion control system might still be processing the action and the real tool position
224 // in any intermediate location.
225 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
232 // We received a new gcode, and one of the functions
233 // determined the distance for that given gcode. So now we can attach this gcode to the right block
235 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
237 //If the queue is empty, execute immediatly, otherwise attach to the last added block
238 if( this->kernel
->conveyor
->queue
.size() == 0 ){
239 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
241 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
242 block
->append_gcode(gcode
);
247 // Reset the position for all axes ( used in homing and G92 stuff )
248 void Robot::reset_axis_position(double position
, int axis
) {
249 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
250 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
254 // Convert target from millimeters to steps, and append this to the planner
255 void Robot::append_milestone( double target
[], double rate
){
256 int steps
[3]; //Holds the result of the conversion
258 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
259 this->arm_solution
->millimeters_to_steps( target
, steps
);
262 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
264 // Compute how long this move moves, so we can attach it to the block for later use
265 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
267 // Do not move faster than the configured limits
268 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
269 if( this->max_speeds
[axis
] > 0 ){
270 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
271 if( axis_speed
> this->max_speeds
[axis
] ){
272 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
277 // Append the block to the planner
278 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
280 // Update the last_milestone to the current target for the next time we use last_milestone
281 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
285 // Append a move to the queue ( cutting it into segments if needed )
286 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
288 // Find out the distance for this gcode
289 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 ) );
291 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
292 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
294 // Mark the gcode as having a known distance
295 this->distance_in_gcode_is_known( gcode
);
297 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
298 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
299 // 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
302 if(this->delta_segments_per_second
> 1.0) {
303 // enabled if set to something > 1, it is set to 0.0 by default
304 // segment based on current speed and requested segments per second
305 // the faster the travel speed the fewer segments needed
306 // NOTE rate is mm/sec and we take into account any speed override
307 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
308 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
309 // TODO if we are only moving in Z on a delta we don't really need to segment at all
312 if(this->mm_per_line_segment
== 0.0){
313 segments
= 1; // don't split it up
315 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
319 // A vector to keep track of the endpoint of each segment
320 double temp_target
[3];
322 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
325 for( int i
=0; i
<segments
-1; i
++ ){
326 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
327 // Append the end of this segment to the queue
328 this->append_milestone(temp_target
, rate
);
331 // Append the end of this full move to the queue
332 this->append_milestone(target
, rate
);
336 // Append an arc to the queue ( cutting it into segments as needed )
337 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
340 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
341 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
342 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
343 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
344 double r_axis1
= -offset
[this->plane_axis_1
];
345 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
346 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
348 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
349 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
350 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
351 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
353 // Find the distance for this gcode
354 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
356 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
357 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
359 // Mark the gcode as having a known distance
360 this->distance_in_gcode_is_known( gcode
);
362 // Figure out how many segments for this gcode
363 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
365 double theta_per_segment
= angular_travel
/segments
;
366 double linear_per_segment
= linear_travel
/segments
;
368 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
369 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
370 r_T = [cos(phi) -sin(phi);
371 sin(phi) cos(phi] * r ;
372 For arc generation, the center of the circle is the axis of rotation and the radius vector is
373 defined from the circle center to the initial position. Each line segment is formed by successive
374 vector rotations. This requires only two cos() and sin() computations to form the rotation
375 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
376 all double numbers are single precision on the Arduino. (True double precision will not have
377 round off issues for CNC applications.) Single precision error can accumulate to be greater than
378 tool precision in some cases. Therefore, arc path correction is implemented.
380 Small angle approximation may be used to reduce computation overhead further. This approximation
381 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
382 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
383 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
384 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
385 issue for CNC machines with the single precision Arduino calculations.
386 This approximation also allows mc_arc to immediately insert a line segment into the planner
387 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
388 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
389 This is important when there are successive arc motions.
391 // Vector rotation matrix values
392 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
393 double sin_T
= theta_per_segment
;
395 double arc_target
[3];
402 // Initialize the linear axis
403 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
405 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
407 if (count
< this->arc_correction
) {
408 // Apply vector rotation matrix
409 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
410 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
414 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
415 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
416 cos_Ti
= cos(i
*theta_per_segment
);
417 sin_Ti
= sin(i
*theta_per_segment
);
418 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
419 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
423 // Update arc_target location
424 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
425 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
426 arc_target
[this->plane_axis_2
] += linear_per_segment
;
428 // Append this segment to the queue
429 this->append_milestone(arc_target
, this->feed_rate
);
433 // Ensure last segment arrives at target location.
434 this->append_milestone(target
, this->feed_rate
);
437 // Do the math for an arc and add it to the queue
438 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
441 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
443 // Set clockwise/counter-clockwise sign for mc_arc computations
444 bool is_clockwise
= false;
445 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
448 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
453 // Convert from inches to millimeters ( our internal storage unit ) if needed
454 inline double Robot::to_millimeters( double value
){
455 return this->inch_mode
? value
* 25.4 : value
;
457 inline double Robot::from_millimeters( double value
){
458 return this->inch_mode
? value
/25.4 : value
;
461 double Robot::theta(double x
, double y
){
462 double t
= atan(x
/fabs(y
));
463 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
466 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
467 this->plane_axis_0
= axis_0
;
468 this->plane_axis_1
= axis_1
;
469 this->plane_axis_2
= axis_2
;