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 //See if the current Gcode line has some orders for us
117 void Robot::execute_gcode(Gcode
* gcode
){
119 //Temp variables, constant properties are stored in the object
120 uint8_t next_action
= NEXT_ACTION_DEFAULT
;
121 this->motion_mode
= -1;
123 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
124 if( gcode
->has_letter('G')){
125 switch( (int) gcode
->get_value('G') ){
126 case 0: this->motion_mode
= MOTION_MODE_SEEK
; break;
127 case 1: this->motion_mode
= MOTION_MODE_LINEAR
; break;
128 case 2: this->motion_mode
= MOTION_MODE_CW_ARC
; break;
129 case 3: this->motion_mode
= MOTION_MODE_CCW_ARC
; break;
130 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
131 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
132 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
133 case 20: this->inch_mode
= true; break;
134 case 21: this->inch_mode
= false; break;
135 case 90: this->absolute_mode
= true; break;
136 case 91: this->absolute_mode
= false; break;
138 if(gcode
->get_num_args() == 0){
139 clear_vector(this->last_milestone
);
141 for (char letter
= 'X'; letter
<= 'Z'; letter
++){
142 if ( gcode
->has_letter(letter
) )
143 this->last_milestone
[letter
-'X'] = this->to_millimeters(gcode
->get_value(letter
));
146 memcpy(this->current_position
, this->last_milestone
, sizeof(double)*3); // current_position[] = last_milestone[];
147 this->arm_solution
->millimeters_to_steps(this->current_position
, this->kernel
->planner
->position
);
148 return; // TODO: Wait until queue empty
151 }else if( gcode
->has_letter('M')){
152 switch( (int) gcode
->get_value('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;
170 case 114: gcode
->stream
->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
171 this->current_position
[0],
172 this->current_position
[1],
173 this->current_position
[2]);
174 gcode
->add_nl
= true;
176 case 220: // M220 - speed override percentage
177 if (gcode
->has_letter('S'))
179 double factor
= gcode
->get_value('S');
180 // enforce minimum 1% speed
183 seconds_per_minute
= factor
* 0.6;
187 if( this->motion_mode
< 0)
191 double target
[3], offset
[3];
192 clear_vector(target
); clear_vector(offset
);
194 memcpy(target
, this->current_position
, sizeof(target
)); //default to last target
196 for(char letter
= 'I'; letter
<= 'K'; letter
++){ if( gcode
->has_letter(letter
) ){ offset
[letter
-'I'] = this->to_millimeters(gcode
->get_value(letter
)); } }
197 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']); } }
199 if( gcode
->has_letter('F') )
201 if( this->motion_mode
== MOTION_MODE_SEEK
)
202 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
204 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') ) / 60.0;
207 //Perform any physical actions
208 switch( next_action
){
209 case NEXT_ACTION_DEFAULT
:
210 switch(this->motion_mode
){
211 case MOTION_MODE_CANCEL
: break;
212 case MOTION_MODE_SEEK
: this->append_line(gcode
, target
, this->seek_rate
); break;
213 case MOTION_MODE_LINEAR
: this->append_line(gcode
, target
, this->feed_rate
); break;
214 case MOTION_MODE_CW_ARC
: case MOTION_MODE_CCW_ARC
: this->compute_arc(gcode
, offset
, target
); break;
219 // As far as the parser is concerned, the position is now == target. In reality the
220 // motion control system might still be processing the action and the real tool position
221 // in any intermediate location.
222 memcpy(this->current_position
, target
, sizeof(double)*3); // this->position[] = target[];
226 // Convert target from millimeters to steps, and append this to the planner
227 void Robot::append_milestone( double target
[], double rate
){
228 int steps
[3]; //Holds the result of the conversion
230 this->arm_solution
->millimeters_to_steps( target
, steps
);
233 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){deltas
[axis
]=target
[axis
]-this->last_milestone
[axis
];}
236 double millimeters_of_travel
= sqrt( pow( deltas
[X_AXIS
], 2 ) + pow( deltas
[Y_AXIS
], 2 ) + pow( deltas
[Z_AXIS
], 2 ) );
238 for(int axis
=X_AXIS
;axis
<=Z_AXIS
;axis
++){
239 if( this->max_speeds
[axis
] > 0 ){
240 double axis_speed
= ( fabs(deltas
[axis
]) / ( millimeters_of_travel
/ rate
)) * seconds_per_minute
;
241 if( axis_speed
> this->max_speeds
[axis
] ){
242 rate
= rate
* ( this->max_speeds
[axis
] / axis_speed
);
247 this->kernel
->planner
->append_block( steps
, rate
* seconds_per_minute
, millimeters_of_travel
, deltas
);
249 memcpy(this->last_milestone
, target
, sizeof(double)*3); // this->last_milestone[] = target[];
253 void Robot::append_line(Gcode
* gcode
, double target
[], double rate
){
256 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
257 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
258 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 ) );
260 if( gcode
->call_on_gcode_execute_event_immediatly
== true ){
261 //printf("GCODE B: %s \r\n", gcode->command.c_str() );
262 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
263 gcode
->on_gcode_execute_event_called
= true;
266 if (gcode
->millimeters_of_travel
== 0.0) {
267 this->append_milestone(this->current_position
, 0.0);
271 uint16_t segments
= ceil( gcode
->millimeters_of_travel
/ this->mm_per_line_segment
);
272 // A vector to keep track of the endpoint of each segment
273 double temp_target
[3];
275 memcpy( temp_target
, this->current_position
, sizeof(double)*3); // temp_target[] = this->current_position[];
278 for( int i
=0; i
<segments
-1; i
++ ){
279 for(int axis
=X_AXIS
; axis
<= Z_AXIS
; axis
++ ){ temp_target
[axis
] += ( target
[axis
]-this->current_position
[axis
] )/segments
; }
280 this->append_milestone(temp_target
, rate
);
282 this->append_milestone(target
, rate
);
286 void Robot::append_arc(Gcode
* gcode
, double target
[], double offset
[], double radius
, bool is_clockwise
){
288 double center_axis0
= this->current_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
289 double center_axis1
= this->current_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
290 double linear_travel
= target
[this->plane_axis_2
] - this->current_position
[this->plane_axis_2
];
291 double r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
292 double r_axis1
= -offset
[this->plane_axis_1
];
293 double rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
294 double rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
296 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
297 double angular_travel
= atan2(r_axis0
*rt_axis1
-r_axis1
*rt_axis0
, r_axis0
*rt_axis0
+r_axis1
*rt_axis1
);
298 if (angular_travel
< 0) { angular_travel
+= 2*M_PI
; }
299 if (is_clockwise
) { angular_travel
-= 2*M_PI
; }
301 gcode
->millimeters_of_travel
= hypot(angular_travel
*radius
, fabs(linear_travel
));
303 if( gcode
->call_on_gcode_execute_event_immediatly
== true ){
304 //printf("GCODE C: %s \r\n", gcode->command.c_str() );
305 this->kernel
->call_event(ON_GCODE_EXECUTE
, gcode
);
306 gcode
->on_gcode_execute_event_called
= true;
309 if (gcode
->millimeters_of_travel
== 0.0) {
310 this->append_milestone(this->current_position
, 0.0);
314 uint16_t segments
= floor(gcode
->millimeters_of_travel
/this->mm_per_arc_segment
);
316 double theta_per_segment
= angular_travel
/segments
;
317 double linear_per_segment
= linear_travel
/segments
;
319 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
320 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
321 r_T = [cos(phi) -sin(phi);
322 sin(phi) cos(phi] * r ;
323 For arc generation, the center of the circle is the axis of rotation and the radius vector is
324 defined from the circle center to the initial position. Each line segment is formed by successive
325 vector rotations. This requires only two cos() and sin() computations to form the rotation
326 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
327 all double numbers are single precision on the Arduino. (True double precision will not have
328 round off issues for CNC applications.) Single precision error can accumulate to be greater than
329 tool precision in some cases. Therefore, arc path correction is implemented.
331 Small angle approximation may be used to reduce computation overhead further. This approximation
332 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
333 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
334 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
335 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
336 issue for CNC machines with the single precision Arduino calculations.
337 This approximation also allows mc_arc to immediately insert a line segment into the planner
338 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
339 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
340 This is important when there are successive arc motions.
342 // Vector rotation matrix values
343 double cos_T
= 1-0.5*theta_per_segment
*theta_per_segment
; // Small angle approximation
344 double sin_T
= theta_per_segment
;
346 double arc_target
[3];
353 // Initialize the linear axis
354 arc_target
[this->plane_axis_2
] = this->current_position
[this->plane_axis_2
];
356 for (i
= 1; i
<segments
; i
++) { // Increment (segments-1)
358 if (count
< this->arc_correction
) {
359 // Apply vector rotation matrix
360 r_axisi
= r_axis0
*sin_T
+ r_axis1
*cos_T
;
361 r_axis0
= r_axis0
*cos_T
- r_axis1
*sin_T
;
365 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
366 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
367 cos_Ti
= cos(i
*theta_per_segment
);
368 sin_Ti
= sin(i
*theta_per_segment
);
369 r_axis0
= -offset
[this->plane_axis_0
]*cos_Ti
+ offset
[this->plane_axis_1
]*sin_Ti
;
370 r_axis1
= -offset
[this->plane_axis_0
]*sin_Ti
- offset
[this->plane_axis_1
]*cos_Ti
;
374 // Update arc_target location
375 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
376 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
377 arc_target
[this->plane_axis_2
] += linear_per_segment
;
378 this->append_milestone(arc_target
, this->feed_rate
);
381 // Ensure last segment arrives at target location.
382 this->append_milestone(target
, this->feed_rate
);
386 void Robot::compute_arc(Gcode
* gcode
, double offset
[], double target
[]){
389 double radius
= hypot(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
391 // Set clockwise/counter-clockwise sign for mc_arc computations
392 bool is_clockwise
= false;
393 if( this->motion_mode
== MOTION_MODE_CW_ARC
){ is_clockwise
= true; }
396 this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
401 // Convert from inches to millimeters ( our internal storage unit ) if needed
402 inline double Robot::to_millimeters( double value
){
403 return this->inch_mode
? value
/25.4 : value
;
406 double Robot::theta(double x
, double y
){
407 double t
= atan(x
/fabs(y
));
408 if (y
>0) {return(t
);} else {if (t
>0){return(M_PI
-t
);} else {return(-M_PI
-t
);}}
411 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
){
412 this->plane_axis_0
= axis_0
;
413 this->plane_axis_1
= axis_1
;
414 this->plane_axis_2
= axis_2
;