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(5.0 )->as_number();
77 this->mm_per_arc_segment
= this->kernel
->config
->value(mm_per_arc_segment_checksum
)->by_default(0.5 )->as_number();
78 this->arc_correction
= this->kernel
->config
->value(arc_correction_checksum
)->by_default(5 )->as_number();
79 this->max_speeds
[X_AXIS
] = this->kernel
->config
->value(x_axis_max_speed_checksum
)->by_default(60000 )->as_number();
80 this->max_speeds
[Y_AXIS
] = this->kernel
->config
->value(y_axis_max_speed_checksum
)->by_default(60000 )->as_number();
81 this->max_speeds
[Z_AXIS
] = this->kernel
->config
->value(z_axis_max_speed_checksum
)->by_default(300 )->as_number();
82 this->alpha_step_pin
.from_string( this->kernel
->config
->value(alpha_step_pin_checksum
)->by_default("2.0" )->as_string())->as_output();
83 this->alpha_dir_pin
.from_string( this->kernel
->config
->value(alpha_dir_pin_checksum
)->by_default("0.5" )->as_string())->as_output();
84 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();
85 this->beta_step_pin
.from_string( this->kernel
->config
->value(beta_step_pin_checksum
)->by_default("2.1" )->as_string())->as_output();
86 this->gamma_step_pin
.from_string( this->kernel
->config
->value(gamma_step_pin_checksum
)->by_default("2.2" )->as_string())->as_output();
87 this->gamma_dir_pin
.from_string( this->kernel
->config
->value(gamma_dir_pin_checksum
)->by_default("0.20" )->as_string())->as_output();
88 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();
89 this->beta_dir_pin
.from_string( this->kernel
->config
->value(beta_dir_pin_checksum
)->by_default("0.11" )->as_string())->as_output();
90 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();
94 //A GCode has been received
95 void Robot::on_gcode_received(void * argument
){
96 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
97 gcode
->call_on_gcode_execute_event_immediatly
= false;
98 gcode
->on_gcode_execute_event_called
= false;
99 //If the queue is empty, execute immediatly, otherwise attach to the last added block
100 if( this->kernel
->player
->queue
.size() == 0 ){
101 gcode
->call_on_gcode_execute_event_immediatly
= true;
102 this->execute_gcode(gcode
);
103 if( gcode
->on_gcode_execute_event_called
== false ){
104 //printf("GCODE A: %s \r\n", gcode->command.c_str() );
105 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
108 Block
* block
= this->kernel
->player
->queue
.get_ref( this->kernel
->player
->queue
.size() - 1 );
109 this->execute_gcode(gcode
);
110 block
->append_gcode(gcode
);
116 void Robot::reset_axis_position(double position
, int axis
) {
117 this->last_milestone
[axis
] = this->current_position
[axis
] = position
;
118 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
122 //See if the current Gcode line has some orders for us
123 void Robot::execute_gcode(Gcode
* gcode
){
125 //Temp variables, constant properties are stored in the object
126 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
127 this->motion_mode
= -1;
129 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
130 if( gcode
->has_letter('G')){
131 switch( (int) gcode
->get_value('G') ){
132 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
133 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
134 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
135 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
136 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
137 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
138 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
139 case 20: this->inch_mode
= true; break;
140 case 21: this->inch_mode
= false; break;
141 case 90: this->absolute_mode
= true; break;
142 case 91: this->absolute_mode
= false; break;
144 if(gcode
->get_num_args() == 0){
145 clear_vector(this->last_milestone
);
147 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
148 if ( gcode
->has_letter(letter
) )
149 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
152 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
153 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
154 return; // TODO: Wait until queue empty
157 }else if( gcode
->has_letter('M')){
158 switch( (int) gcode
->get_value('M') ){
159 case 92: // M92 - set steps per mm
161 this->arm_solution
->get_steps_per_millimeter(steps
);
162 if (gcode
->has_letter('X'))
163 steps
[0] = this->to_millimeters(gcode
->get_value('X'));
164 if (gcode
->has_letter('Y'))
165 steps
[1] = this->to_millimeters(gcode
->get_value('Y'));
166 if (gcode
->has_letter('Z'))
167 steps
[2] = this->to_millimeters(gcode
->get_value('Z'));
168 if (gcode
->has_letter('F'))
169 seconds_per_minute
= gcode
->get_value('F');
170 this->arm_solution
->set_steps_per_millimeter(steps
);
171 // update current position in steps
172 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
173 gcode
->stream
->printf("X:%g Y:%g Z:%g F:%g ", steps
[0], steps
[1], steps
[2], seconds_per_minute
);
174 gcode
->add_nl
= true;
176 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
177 this->current_position
[0],
178 this->current_position
[1],
179 this->current_position
[2]);
180 gcode
->add_nl
= true;
182 case 220: // M220 - speed override percentage
183 if (gcode
->has_letter('S'))
185 double factor
= gcode
->get_value('S');
186 // enforce minimum 1% speed
189 seconds_per_minute
= factor
* 0.6;
193 if( this->motion_mode
< 0)
197 double target
[3], offset
[3];
198 clear_vector(target
); clear_vector(offset
);
200 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
202 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
203 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']); } }
205 if( gcode
->has_letter('F') )
207 if( this->motion_mode
== MOTION_MODE_SEEK
)
208 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
210 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
213 //Perform any physical actions
214 switch( next_action
){
215 case NEXT_ACTION_DEFAULT
:
216 switch(this->motion_mode
){
217 case MOTION_MODE_CANCEL
: break;
218 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
219 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
220 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
225 // As far as the parser is concerned, the position is now == target. In reality the
226 // motion control system might still be processing the action and the real tool position
227 // in any intermediate location.
228 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
232 // Convert target from millimeters to steps, and append this to the planner
233 void Robot::append_milestone( double target
[], double rate
){
234 int steps
[3]; //Holds the result of the conversion
236 this->arm_solution
->millimeters_to_steps( target
, steps
);
239 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
242 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
244 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
245 if( this->max_speeds
[axis
] > 0 ){
246 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
247 if( axis_speed
> this->max_speeds
[axis
] ){
248 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
253 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
255 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
259 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
262 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
263 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
264 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 ) );
266 if( gcode
->call_on_gcode_execute_event_immediatly
== true ){
267 //printf("GCODE B: %s \r\n", gcode->command.c_str() );
268 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
269 gcode
->on_gcode_execute_event_called
= true;
272 if (gcode
->millimeters_of_travel
== 0.0) {
273 this->append_milestone(this->current_position
, 0.0);
277 uint16_t segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
278 // A vector to keep track of the endpoint of each segment
279 double temp_target
[3];
281 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
284 for( int i
=0; i
<segments
-1; i
++ ){
285 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
286 this->append_milestone(temp_target
, rate
);
288 this->append_milestone(target
, rate
);
292 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
294 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
295 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
296 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
297 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
298 double r_axis1
= -offset
[this->plane_axis_1
];
299 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
300 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
302 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
303 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
304 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
305 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
307 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
309 if( gcode
->call_on_gcode_execute_event_immediatly
== true ){
310 //printf("GCODE C: %s \r\n", gcode->command.c_str() );
311 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
312 gcode
->on_gcode_execute_event_called
= true;
315 if (gcode
->millimeters_of_travel
== 0.0) {
316 this->append_milestone(this->current_position
, 0.0);
320 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
322 double theta_per_segment
= angular_travel
/segments
;
323 double linear_per_segment
= linear_travel
/segments
;
325 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
326 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
327 r_T = [cos(phi) -sin(phi);
328 sin(phi) cos(phi] * r ;
329 For arc generation, the center of the circle is the axis of rotation and the radius vector is
330 defined from the circle center to the initial position. Each line segment is formed by successive
331 vector rotations. This requires only two cos() and sin() computations to form the rotation
332 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
333 all double numbers are single precision on the Arduino. (True double precision will not have
334 round off issues for CNC applications.) Single precision error can accumulate to be greater than
335 tool precision in some cases. Therefore, arc path correction is implemented.
337 Small angle approximation may be used to reduce computation overhead further. This approximation
338 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
339 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
340 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
341 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
342 issue for CNC machines with the single precision Arduino calculations.
343 This approximation also allows mc_arc to immediately insert a line segment into the planner
344 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
345 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
346 This is important when there are successive arc motions.
348 // Vector rotation matrix values
349 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
350 double sin_T
= theta_per_segment
;
352 double arc_target
[3];
359 // Initialize the linear axis
360 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
362 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
364 if (count
< this->arc_correction
) {
365 // Apply vector rotation matrix
366 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
367 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
371 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
372 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
373 cos_Ti
= cos(i
*theta_per_segment
);
374 sin_Ti
= sin(i
*theta_per_segment
);
375 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
376 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
380 // Update arc_target location
381 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
382 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
383 arc_target
[this->plane_axis_2
] += linear_per_segment
;
384 this->append_milestone(arc_target
, this->feed_rate
);
387 // Ensure last segment arrives at target location.
388 this->append_milestone(target
, this->feed_rate
);
392 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
395 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
397 // Set clockwise/counter-clockwise sign for mc_arc computations
398 bool is_clockwise
= false;
399 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
402 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
407 // Convert from inches to millimeters ( our internal storage unit ) if needed
408 inline double Robot::to_millimeters( double value
){
409 return this->inch_mode
? value
/25.4 : value
;
412 double Robot::theta(double x
, double y
){
413 double t
= atan(x
/fabs(y
));
414 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
417 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
418 this->plane_axis_0
= axis_0
;
419 this->plane_axis_1
= axis_1
;
420 this->plane_axis_2
= axis_2
;