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