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"
26 this->inch_mode
= false;
27 this->absolute_mode
= true;
28 this->motion_mode
= MOTION_MODE_SEEK
;
29 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
30 clear_vector(this->current_position
);
31 clear_vector(this->last_milestone
);
32 this->arm_solution
= NULL
;
33 seconds_per_minute
= 60.0;
36 //Called when the module has just been loaded
37 void Robot::on_module_loaded() {
38 register_for_event(ON_CONFIG_RELOAD
);
39 this->register_for_event(ON_GCODE_RECEIVED
);
42 this->on_config_reload(this);
44 // Make our 3 StepperMotors
45 this->alpha_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&alpha_step_pin
,&alpha_dir_pin
,&alpha_en_pin
) );
46 this->beta_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&beta_step_pin
, &beta_dir_pin
, &beta_en_pin
) );
47 this->gamma_stepper_motor
= this->kernel
->step_ticker
->add_stepper_motor( new StepperMotor(&gamma_step_pin
,&gamma_dir_pin
,&gamma_en_pin
) );
51 void Robot::on_config_reload(void* argument
){
52 if (this->arm_solution
) delete this->arm_solution
;
53 int solution_checksum
= get_checksum(this->kernel
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
55 // Note checksums are not const expressions when in debug mode, so don't use switch
56 if(solution_checksum
== rostock_checksum
) {
57 this->arm_solution
= new RostockSolution(this->kernel
->config
);
59 }else if(solution_checksum
== delta_checksum
) {
60 // place holder for now
61 this->arm_solution
= new RostockSolution(this->kernel
->config
);
63 }else if(solution_checksum
== rotatable_cartesian_checksum
) {
64 this->arm_solution
= new RotatableCartesianSolution(this->kernel
->config
);
66 }else if(solution_checksum
== cartesian_checksum
) {
67 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
70 this->arm_solution
= new CartesianSolution(this->kernel
->config
);
74 this->feed_rate
= this->kernel
->config
->value(default_feed_rate_checksum
)->by_default(100 )->as_number() / 60;
75 this->seek_rate
= this->kernel
->config
->value(default_seek_rate_checksum
)->by_default(100 )->as_number() / 60;
76 this->mm_per_line_segment
= this->kernel
->config
->value(mm_per_line_segment_checksum
)->by_default(0.0 )->as_number();
77 this->delta_segments_per_second
= this->kernel
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0 )->as_number();
78 this->mm_per_arc_segment
= this->kernel
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5 )->as_number();
79 this->arc_correction
= this->kernel
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
80 this->max_speeds
[X_AXIS
] = this->kernel
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
81 this->max_speeds
[Y_AXIS
] = this->kernel
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
82 this->max_speeds
[Z_AXIS
] = this->kernel
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
83 this->alpha_step_pin
.from_string( this->kernel
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
84 this->alpha_dir_pin
.from_string( this->kernel
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
85 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();
86 this->beta_step_pin
.from_string( this->kernel
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
87 this->gamma_step_pin
.from_string( this->kernel
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
88 this->gamma_dir_pin
.from_string( this->kernel
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
89 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();
90 this->beta_dir_pin
.from_string( this->kernel
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
91 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();
95 //#pragma GCC push_options
96 //#pragma GCC optimize ("O0")
99 //A GCode has been received
100 void Robot::on_gcode_received(void * argument
){
101 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
102 this->process_gcode(gcode
);
105 // We called process_gcode with a new gcode, and one of the functions
106 // determined the distance for that given gcode. So now we can attach this gcode to the right block
108 void Robot::distance_in_gcode_is_known(Gcode
* gcode
){
110 //If the queue is empty, execute immediatly, otherwise attach to the last added block
111 if( this->kernel
->conveyor
->queue
.size() == 0 ){
112 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
114 Block
* block
= this->kernel
->conveyor
->queue
.get_ref( this->kernel
->conveyor
->queue
.size() - 1 );
115 block
->append_gcode(gcode
);
120 //#pragma GCC pop_options
122 void Robot::reset_axis_position(double position
, int axis
) {
123 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
124 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
128 //See if the current Gcode line has some orders for us
129 void Robot::process_gcode(Gcode
* gcode
){
131 //Temp variables, constant properties are stored in the object
132 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
133 this->motion_mode
= -1;
135 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
138 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
139 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
140 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
141 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
142 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
143 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
144 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
145 case 20: this->inch_mode
= true; break;
146 case 21: this->inch_mode
= false; break;
147 case 90: this->absolute_mode
= true; break;
148 case 91: this->absolute_mode
= false; break;
150 if(gcode
->get_num_args() == 0){
151 clear_vector(this->last_milestone
);
153 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
154 if ( gcode
->has_letter(letter
) )
155 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
158 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
159 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
160 return; // TODO: Wait until queue empty
163 }else if( gcode
->has_m
){
165 case 92: // M92 - set steps per mm
167 this->arm_solution
->get_steps_per_millimeter(steps
);
168 if (gcode
->has_letter('X'))
169 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
170 if (gcode
->has_letter('Y'))
171 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
172 if (gcode
->has_letter('Z'))
173 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
174 if (gcode
->has_letter('F'))
175 seconds_per_minute
= gcode
->get_value('F');
176 this->arm_solution
->set_steps_per_millimeter(steps
);
177 // update current position in steps
178 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
179 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
180 gcode
->add_nl
= true;
182 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
183 this->current_position
[0],
184 this->current_position
[1],
185 this->current_position
[2]);
186 gcode
->add_nl
= true;
188 case 220: // M220 - speed override percentage
189 if (gcode
->has_letter('S'))
191 double factor
= gcode
->get_value('S');
192 // enforce minimum 1% speed
195 seconds_per_minute
= factor
* 0.6;
199 if( this->motion_mode
< 0)
203 double target
[3], offset
[3];
204 clear_vector(target
); clear_vector(offset
);
206 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
208 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
209 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']); } }
211 if( gcode
->has_letter('F') )
213 if( this->motion_mode
== MOTION_MODE_SEEK
)
214 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
216 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
219 //Perform any physical actions
220 switch( next_action
){
221 case NEXT_ACTION_DEFAULT
:
222 switch(this->motion_mode
){
223 case MOTION_MODE_CANCEL
: break;
224 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
225 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
226 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
231 // As far as the parser is concerned, the position is now == target. In reality the
232 // motion control system might still be processing the action and the real tool position
233 // in any intermediate location.
234 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
238 // Convert target from millimeters to steps, and append this to the planner
239 void Robot::append_milestone( double target
[], double rate
){
240 int steps
[3]; //Holds the result of the conversion
242 this->arm_solution
->millimeters_to_steps( target
, steps
);
245 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
248 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
250 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
251 if( this->max_speeds
[axis
] > 0 ){
252 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
253 if( axis_speed
> this->max_speeds
[axis
] ){
254 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
259 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
261 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
265 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
267 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 ) );
269 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
271 this->distance_in_gcode_is_known( gcode
);
273 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
274 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
275 // 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
279 if(this->delta_segments_per_second
> 1.0) {
280 // enabled if set to something > 1, it is set to 0.0 by default
281 // segment based on current speed and requested segments per second
282 // the faster the travel speed the fewer segments needed
283 // NOTE rate is mm/sec and we take into account any speed override
284 float seconds
= 60.0/seconds_per_minute
* gcode
->millimeters_of_travel
/ rate
;
285 segments
= max(1, ceil(this->delta_segments_per_second
* seconds
));
286 // TODO if we are only moving in Z on a delta we don't really need to segment at all
289 if(this->mm_per_line_segment
== 0.0){
290 segments
= 1; // don't split it up
292 segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
296 // A vector to keep track of the endpoint of each segment
297 double temp_target
[3];
299 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
302 for( int i
=0; i
<segments
-1; i
++ ){
303 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
304 this->append_milestone(temp_target
, rate
);
306 this->append_milestone(target
, rate
);
310 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
312 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
313 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
314 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
315 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
316 double r_axis1
= -offset
[this->plane_axis_1
];
317 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
318 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
320 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
321 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
322 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
323 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
325 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
327 if( gcode
->millimeters_of_travel
< 0.0001 ){ return; }
329 this->distance_in_gcode_is_known( gcode
);
331 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
333 double theta_per_segment
= angular_travel
/segments
;
334 double linear_per_segment
= linear_travel
/segments
;
336 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
337 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
338 r_T = [cos(phi) -sin(phi);
339 sin(phi) cos(phi] * r ;
340 For arc generation, the center of the circle is the axis of rotation and the radius vector is
341 defined from the circle center to the initial position. Each line segment is formed by successive
342 vector rotations. This requires only two cos() and sin() computations to form the rotation
343 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
344 all double numbers are single precision on the Arduino. (True double precision will not have
345 round off issues for CNC applications.) Single precision error can accumulate to be greater than
346 tool precision in some cases. Therefore, arc path correction is implemented.
348 Small angle approximation may be used to reduce computation overhead further. This approximation
349 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
350 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
351 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
352 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
353 issue for CNC machines with the single precision Arduino calculations.
354 This approximation also allows mc_arc to immediately insert a line segment into the planner
355 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
356 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
357 This is important when there are successive arc motions.
359 // Vector rotation matrix values
360 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
361 double sin_T
= theta_per_segment
;
363 double arc_target
[3];
370 // Initialize the linear axis
371 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
373 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
375 if (count
< this->arc_correction
) {
376 // Apply vector rotation matrix
377 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
378 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
382 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
383 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
384 cos_Ti
= cos(i
*theta_per_segment
);
385 sin_Ti
= sin(i
*theta_per_segment
);
386 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
387 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
391 // Update arc_target location
392 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
393 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
394 arc_target
[this->plane_axis_2
] += linear_per_segment
;
395 this->append_milestone(arc_target
, this->feed_rate
);
398 // Ensure last segment arrives at target location.
399 this->append_milestone(target
, this->feed_rate
);
403 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
406 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
408 // Set clockwise/counter-clockwise sign for mc_arc computations
409 bool is_clockwise
= false;
410 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
413 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
418 // Convert from inches to millimeters ( our internal storage unit ) if needed
419 inline double Robot::to_millimeters( double value
){
420 return this->inch_mode
? value
/25.4 : value
;
423 double Robot::theta(double x
, double y
){
424 double t
= atan(x
/fabs(y
));
425 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
428 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
429 this->plane_axis_0
= axis_0
;
430 this->plane_axis_1
= axis_1
;
431 this->plane_axis_2
= axis_2
;