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[];
225 // We received a new gcode, and one of the functions
226 // determined the distance for that given gcode. So now we can attach this gcode to the right block
228 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
230 //If the queue is empty, execute immediatly, otherwise attach to the last added block
231 if( this->kernel
->conveyor
->queue
.size() == 0 ){
232 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
234 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
235 block
->append_gcode(gcode
);
240 // Reset the position for all axes ( used in homing and G92 stuff )
241 void Robot::reset_axis_position(double position
, int axis
) {
242 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
243 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
247 // Convert target from millimeters to steps, and append this to the planner
248 void Robot::append_milestone( double target
[], double rate
){
249 int steps
[3]; //Holds the result of the conversion
251 // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
252 this->arm_solution
->millimeters_to_steps( target
, steps
);
255 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
257 // Compute how long this move moves, so we can attach it to the block for later use
258 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
260 // Do not move faster than the configured limits
261 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
262 if( this->max_speeds
[axis
] > 0 ){
263 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
264 if( axis_speed
> this->max_speeds
[axis
] ){
265 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
270 // Append the block to the planner
271 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
273 // Update the last_milestone to the current target for the next time we use last_milestone
274 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
278 // Append a move to the queue ( cutting it into segments if needed )
279 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
281 // Find out the distance for this gcode
282 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 ) );
284 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
285 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
287 // Mark the gcode as having a known distance
288 this->distance_in_gcode_is_known( gcode
);
290 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
291 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
292 // 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
295 if(this->delta_segments_per_second
> 1.0) {
296 // enabled if set to something > 1, it is set to 0.0 by default
297 // segment based on current speed and requested segments per second
298 // the faster the travel speed the fewer segments needed
299 // NOTE rate is mm/sec and we take into account any speed override
300 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
301 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
302 // TODO if we are only moving in Z on a delta we don't really need to segment at all
305 if(this->mm_per_line_segment
== 0.0){
306 segments
= 1; // don't split it up
308 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
312 // A vector to keep track of the endpoint of each segment
313 double temp_target
[3];
315 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
318 for( int i
=0; i
<segments
-1; i
++ ){
319 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
320 // Append the end of this segment to the queue
321 this->append_milestone(temp_target
, rate
);
324 // Append the end of this full move to the queue
325 this->append_milestone(target
, rate
);
329 // Append an arc to the queue ( cutting it into segments as needed )
330 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
333 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
334 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
335 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
336 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
337 double r_axis1
= -offset
[this->plane_axis_1
];
338 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
339 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
341 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
342 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
343 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
344 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
346 // Find the distance for this gcode
347 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
349 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
350 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
352 // Mark the gcode as having a known distance
353 this->distance_in_gcode_is_known( gcode
);
355 // Figure out how many segments for this gcode
356 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
358 double theta_per_segment
= angular_travel
/segments
;
359 double linear_per_segment
= linear_travel
/segments
;
361 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
362 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
363 r_T = [cos(phi) -sin(phi);
364 sin(phi) cos(phi] * r ;
365 For arc generation, the center of the circle is the axis of rotation and the radius vector is
366 defined from the circle center to the initial position. Each line segment is formed by successive
367 vector rotations. This requires only two cos() and sin() computations to form the rotation
368 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
369 all double numbers are single precision on the Arduino. (True double precision will not have
370 round off issues for CNC applications.) Single precision error can accumulate to be greater than
371 tool precision in some cases. Therefore, arc path correction is implemented.
373 Small angle approximation may be used to reduce computation overhead further. This approximation
374 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
375 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
376 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
377 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
378 issue for CNC machines with the single precision Arduino calculations.
379 This approximation also allows mc_arc to immediately insert a line segment into the planner
380 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
381 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
382 This is important when there are successive arc motions.
384 // Vector rotation matrix values
385 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
386 double sin_T
= theta_per_segment
;
388 double arc_target
[3];
395 // Initialize the linear axis
396 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
398 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
400 if (count
< this->arc_correction
) {
401 // Apply vector rotation matrix
402 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
403 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
407 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
408 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
409 cos_Ti
= cos(i
*theta_per_segment
);
410 sin_Ti
= sin(i
*theta_per_segment
);
411 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
412 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
416 // Update arc_target location
417 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
418 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
419 arc_target
[this->plane_axis_2
] += linear_per_segment
;
421 // Append this segment to the queue
422 this->append_milestone(arc_target
, this->feed_rate
);
426 // Ensure last segment arrives at target location.
427 this->append_milestone(target
, this->feed_rate
);
430 // Do the math for an arc and add it to the queue
431 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
434 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
436 // Set clockwise/counter-clockwise sign for mc_arc computations
437 bool is_clockwise
= false;
438 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
441 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
446 // Convert from inches to millimeters ( our internal storage unit ) if needed
447 inline double Robot::to_millimeters( double value
){
448 return this->inch_mode
? value
* 25.4 : value
;
450 inline double Robot::from_millimeters( double value
){
451 return this->inch_mode
? value
/25.4 : value
;
454 double Robot::theta(double x
, double y
){
455 double t
= atan(x
/fabs(y
));
456 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
459 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
460 this->plane_axis_0
= axis_0
;
461 this->plane_axis_1
= axis_1
;
462 this->plane_axis_2
= axis_2
;