HCoop / clinton / Smoothieware.git / blob
? search:


1be517aa2d332022f422ecec1da074e0288f8434
[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp
1 /*
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/>.
6 */
7
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
10
11 #include <math.h>
12 #include <string>
13 using std::string;
14
15 #include "Planner.h"
16 #include "Conveyor.h"
17 #include "Robot.h"
18 #include "nuts_bolts.h"
19 #include "Pin.h"
20 #include "StepperMotor.h"
21 #include "Gcode.h"
22 #include "PublicDataRequest.h"
23 #include "RobotPublicAccess.h"
24 #include "arm_solutions/BaseSolution.h"
25 #include "arm_solutions/CartesianSolution.h"
26 #include "arm_solutions/RotatableCartesianSolution.h"
27 #include "arm_solutions/RostockSolution.h"
28 #include "arm_solutions/JohannKosselSolution.h"
29 #include "arm_solutions/HBotSolution.h"
30 #include "StepTicker.h"
31 #include "checksumm.h"
32 #include "utils.h"
33 #include "ConfigValue.h"
34
35 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
36 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
37 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
38 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
39 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
40 #define arc_correction_checksum CHECKSUM("arc_correction")
41 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
42 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
43 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
44
45 // arm solutions
46 #define arm_solution_checksum CHECKSUM("arm_solution")
47 #define cartesian_checksum CHECKSUM("cartesian")
48 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
49 #define rostock_checksum CHECKSUM("rostock")
50 #define delta_checksum CHECKSUM("delta")
51 #define hbot_checksum CHECKSUM("hbot")
52 #define corexy_checksum CHECKSUM("corexy")
53 #define kossel_checksum CHECKSUM("kossel")
54
55 // stepper motor stuff
56 #define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
57 #define beta_step_pin_checksum CHECKSUM("beta_step_pin")
58 #define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
59 #define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
60 #define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
61 #define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
62 #define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
63 #define beta_en_pin_checksum CHECKSUM("beta_en_pin")
64 #define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
65
66 #define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
67 #define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
68 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
69
70 #define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
71 #define beta_max_rate_checksum CHECKSUM("beta_max_rate")
72 #define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
73
74
75 // new-style actuator stuff
76 #define actuator_checksum CHEKCSUM("actuator")
77
78 #define step_pin_checksum CHECKSUM("step_pin")
79 #define dir_pin_checksum CHEKCSUM("dir_pin")
80 #define en_pin_checksum CHECKSUM("en_pin")
81
82 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
83 #define max_rate_checksum CHECKSUM("max_rate")
84
85 #define alpha_checksum CHECKSUM("alpha")
86 #define beta_checksum CHECKSUM("beta")
87 #define gamma_checksum CHECKSUM("gamma")
88
89
90 // 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
91 // It takes care of cutting arcs into segments, same thing for line that are too long
92 #define max(a,b) (((a) > (b)) ? (a) : (b))
93
94 Robot::Robot(){
95 this->inch_mode = false;
96 this->absolute_mode = true;
97 this->motion_mode = MOTION_MODE_SEEK;
98 this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
99 clear_vector(this->last_milestone);
100 this->arm_solution = NULL;
101 seconds_per_minute = 60.0F;
102 }
103
104 //Called when the module has just been loaded
105 void Robot::on_module_loaded() {
106 register_for_event(ON_CONFIG_RELOAD);
107 this->register_for_event(ON_GCODE_RECEIVED);
108 this->register_for_event(ON_GET_PUBLIC_DATA);
109 this->register_for_event(ON_SET_PUBLIC_DATA);
110
111 // Configuration
112 this->on_config_reload(this);
113 }
114
115 void Robot::on_config_reload(void* argument){
116
117 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
118 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
119 // To make adding those solution easier, they have their own, separate object.
120 // Here we read the config to find out which arm solution to use
121 if (this->arm_solution) delete this->arm_solution;
122 int solution_checksum = get_checksum(THEKERNEL->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
123 // Note checksums are not const expressions when in debug mode, so don't use switch
124 if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
125 this->arm_solution = new HBotSolution(THEKERNEL->config);
126
127 }else if(solution_checksum == rostock_checksum) {
128 this->arm_solution = new RostockSolution(THEKERNEL->config);
129
130 }else if(solution_checksum == kossel_checksum) {
131 this->arm_solution = new JohannKosselSolution(THEKERNEL->config);
132
133 }else if(solution_checksum == delta_checksum) {
134 // place holder for now
135 this->arm_solution = new RostockSolution(THEKERNEL->config);
136
137 }else if(solution_checksum == rotatable_cartesian_checksum) {
138 this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
139
140 }else if(solution_checksum == cartesian_checksum) {
141 this->arm_solution = new CartesianSolution(THEKERNEL->config);
142
143 }else{
144 this->arm_solution = new CartesianSolution(THEKERNEL->config);
145 }
146
147
148 this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default( 100.0F)->as_number();
149 this->seek_rate = THEKERNEL->config->value(default_seek_rate_checksum )->by_default( 100.0F)->as_number();
150 this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default( 0.0F)->as_number();
151 this->delta_segments_per_second = THEKERNEL->config->value(delta_segments_per_second_checksum )->by_default(0.0f )->as_number();
152 this->mm_per_arc_segment = THEKERNEL->config->value(mm_per_arc_segment_checksum )->by_default( 0.5f)->as_number();
153 this->arc_correction = THEKERNEL->config->value(arc_correction_checksum )->by_default( 5 )->as_number();
154
155 this->max_speeds[X_AXIS] = THEKERNEL->config->value(x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
156 this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
157 this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
158
159 Pin alpha_step_pin;
160 Pin alpha_dir_pin;
161 Pin alpha_en_pin;
162 Pin beta_step_pin;
163 Pin beta_dir_pin;
164 Pin beta_en_pin;
165 Pin gamma_step_pin;
166 Pin gamma_dir_pin;
167 Pin gamma_en_pin;
168
169 alpha_step_pin.from_string( THEKERNEL->config->value(alpha_step_pin_checksum )->by_default("2.0" )->as_string())->as_output();
170 alpha_dir_pin.from_string( THEKERNEL->config->value(alpha_dir_pin_checksum )->by_default("0.5" )->as_string())->as_output();
171 alpha_en_pin.from_string( THEKERNEL->config->value(alpha_en_pin_checksum )->by_default("0.4" )->as_string())->as_output();
172 beta_step_pin.from_string( THEKERNEL->config->value(beta_step_pin_checksum )->by_default("2.1" )->as_string())->as_output();
173 beta_dir_pin.from_string( THEKERNEL->config->value(beta_dir_pin_checksum )->by_default("0.11" )->as_string())->as_output();
174 beta_en_pin.from_string( THEKERNEL->config->value(beta_en_pin_checksum )->by_default("0.10" )->as_string())->as_output();
175 gamma_step_pin.from_string( THEKERNEL->config->value(gamma_step_pin_checksum )->by_default("2.2" )->as_string())->as_output();
176 gamma_dir_pin.from_string( THEKERNEL->config->value(gamma_dir_pin_checksum )->by_default("0.20" )->as_string())->as_output();
177 gamma_en_pin.from_string( THEKERNEL->config->value(gamma_en_pin_checksum )->by_default("0.19" )->as_string())->as_output();
178
179 float steps_per_mm[3] = {
180 THEKERNEL->config->value(alpha_steps_per_mm_checksum)->by_default( 80.0F)->as_number(),
181 THEKERNEL->config->value(beta_steps_per_mm_checksum )->by_default( 80.0F)->as_number(),
182 THEKERNEL->config->value(gamma_steps_per_mm_checksum)->by_default(2560.0F)->as_number(),
183 };
184
185 // TODO: delete or detect old steppermotors
186 // Make our 3 StepperMotors
187 this->alpha_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin) );
188 this->beta_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin ) );
189 this->gamma_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin) );
190
191 alpha_stepper_motor->change_steps_per_mm(steps_per_mm[0]);
192 beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
193 gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
194
195 alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
196 beta_stepper_motor->max_rate = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F;
197 gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
198
199 actuators.clear();
200 actuators.push_back(alpha_stepper_motor);
201 actuators.push_back(beta_stepper_motor);
202 actuators.push_back(gamma_stepper_motor);
203
204 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
205 // so the first move can be correct if homing is not performed
206 float actuator_pos[3];
207 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
208 for (int i = 0; i < 3; i++)
209 actuators[i]->change_last_milestone(actuator_pos[i]);
210 }
211
212 void Robot::on_get_public_data(void* argument){
213 PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
214
215 if(!pdr->starts_with(robot_checksum)) return;
216
217 if(pdr->second_element_is(speed_override_percent_checksum)) {
218 static float return_data;
219 return_data = 100.0F * 60.0F / seconds_per_minute;
220 pdr->set_data_ptr(&return_data);
221 pdr->set_taken();
222
223 }else if(pdr->second_element_is(current_position_checksum)) {
224 static float return_data[3];
225 return_data[0]= from_millimeters(this->last_milestone[0]);
226 return_data[1]= from_millimeters(this->last_milestone[1]);
227 return_data[2]= from_millimeters(this->last_milestone[2]);
228
229 pdr->set_data_ptr(&return_data);
230 pdr->set_taken();
231 }
232 }
233
234 void Robot::on_set_public_data(void* argument){
235 PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
236
237 if(!pdr->starts_with(robot_checksum)) return;
238
239 if(pdr->second_element_is(speed_override_percent_checksum)) {
240 // NOTE do not use this while printing!
241 float t= *static_cast<float*>(pdr->get_data_ptr());
242 // enforce minimum 10% speed
243 if (t < 10.0F) t= 10.0F;
244
245 this->seconds_per_minute = t / 0.6F; // t * 60 / 100
246 pdr->set_taken();
247 }
248 }
249
250 //A GCode has been received
251 //See if the current Gcode line has some orders for us
252 void Robot::on_gcode_received(void * argument){
253 Gcode* gcode = static_cast<Gcode*>(argument);
254
255 //Temp variables, constant properties are stored in the object
256 uint8_t next_action = NEXT_ACTION_DEFAULT;
257 this->motion_mode = -1;
258
259 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
260 if( gcode->has_g){
261 switch( gcode->g ){
262 case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break;
263 case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break;
264 case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break;
265 case 3: this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken(); break;
266 case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken(); break;
267 case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken(); break;
268 case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken(); break;
269 case 20: this->inch_mode = true; gcode->mark_as_taken(); break;
270 case 21: this->inch_mode = false; gcode->mark_as_taken(); break;
271 case 90: this->absolute_mode = true; gcode->mark_as_taken(); break;
272 case 91: this->absolute_mode = false; gcode->mark_as_taken(); break;
273 case 92: {
274 if(gcode->get_num_args() == 0){
275 clear_vector(this->last_milestone);
276 }else{
277 for (char letter = 'X'; letter <= 'Z'; letter++){
278 if ( gcode->has_letter(letter) )
279 this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
280 }
281 }
282
283 // TODO: handle any number of actuators
284 float actuator_pos[3];
285 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
286
287 for (int i = 0; i < 3; i++)
288 actuators[i]->change_last_milestone(actuator_pos[i]);
289
290 gcode->mark_as_taken();
291 return;
292 }
293 }
294 }else if( gcode->has_m){
295 switch( gcode->m ){
296 case 92: // M92 - set steps per mm
297 if (gcode->has_letter('X'))
298 actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X')));
299 if (gcode->has_letter('Y'))
300 actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y')));
301 if (gcode->has_letter('Z'))
302 actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z')));
303 if (gcode->has_letter('F'))
304 seconds_per_minute = gcode->get_value('F');
305
306 gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm, seconds_per_minute);
307 gcode->add_nl = true;
308 gcode->mark_as_taken();
309 return;
310 case 114:
311 {
312 char buf[32];
313 int n= snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
314 from_millimeters(this->last_milestone[0]),
315 from_millimeters(this->last_milestone[1]),
316 from_millimeters(this->last_milestone[2]));
317 gcode->txt_after_ok.append(buf, n);
318 gcode->mark_as_taken();
319 }
320 return;
321
322 case 203: // M203 Set maximum feedrates in mm/sec
323 if (gcode->has_letter('X'))
324 this->max_speeds[X_AXIS]= gcode->get_value('X');
325 if (gcode->has_letter('Y'))
326 this->max_speeds[Y_AXIS]= gcode->get_value('Y');
327 if (gcode->has_letter('Z'))
328 this->max_speeds[Z_AXIS]= gcode->get_value('Z');
329 if (gcode->has_letter('A'))
330 alpha_stepper_motor->max_rate= gcode->get_value('A');
331 if (gcode->has_letter('B'))
332 beta_stepper_motor->max_rate= gcode->get_value('B');
333 if (gcode->has_letter('C'))
334 gamma_stepper_motor->max_rate= gcode->get_value('C');
335
336 gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
337 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
338 alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
339 gcode->add_nl = true;
340 gcode->mark_as_taken();
341 break;
342
343 case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
344 gcode->mark_as_taken();
345
346 if (gcode->has_letter('S'))
347 {
348 // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
349 THEKERNEL->conveyor->wait_for_empty_queue();
350 float acc= gcode->get_value('S'); // mm/s^2
351 // enforce minimum
352 if (acc < 1.0F)
353 acc = 1.0F;
354 THEKERNEL->planner->acceleration= acc;
355 }
356 break;
357
358 case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
359 gcode->mark_as_taken();
360 if (gcode->has_letter('X'))
361 {
362 float jd= gcode->get_value('X');
363 // enforce minimum
364 if (jd < 0.0F)
365 jd = 0.0F;
366 THEKERNEL->planner->junction_deviation= jd;
367 }
368 if (gcode->has_letter('S'))
369 {
370 float mps= gcode->get_value('S');
371 // enforce minimum
372 if (mps < 0.0F)
373 mps = 0.0F;
374 THEKERNEL->planner->minimum_planner_speed= mps;
375 }
376 break;
377
378 case 220: // M220 - speed override percentage
379 gcode->mark_as_taken();
380 if (gcode->has_letter('S'))
381 {
382 float factor = gcode->get_value('S');
383 // enforce minimum 10% speed
384 if (factor < 10.0F)
385 factor = 10.0F;
386 // enforce maximum 10x speed
387 if (factor > 1000.0F)
388 factor = 1000.0F;
389
390 seconds_per_minute = 6000.0F / factor;
391 }
392 break;
393
394 case 400: // wait until all moves are done up to this point
395 gcode->mark_as_taken();
396 THEKERNEL->conveyor->wait_for_empty_queue();
397 break;
398
399 case 500: // M500 saves some volatile settings to config override file
400 case 503: { // M503 just prints the settings
401 gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
402 gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL->planner->acceleration);
403 gcode->stream->printf(";X- Junction Deviation, S - Minimum Planner speed:\nM205 X%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->minimum_planner_speed);
404 gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n",
405 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
406 alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
407
408 // get or save any arm solution specific optional values
409 BaseSolution::arm_options_t options;
410 if(arm_solution->get_optional(options) && !options.empty()) {
411 gcode->stream->printf(";Optional arm solution specific settings:\nM665");
412 for(auto& i : options) {
413 gcode->stream->printf(" %c%1.4f", i.first, i.second);
414 }
415 gcode->stream->printf("\n");
416 }
417 gcode->mark_as_taken();
418 break;
419 }
420
421 case 665: { // M665 set optional arm solution variables based on arm solution.
422 gcode->mark_as_taken();
423 // the parameter args could be any letter except S so ask solution what options it supports
424 BaseSolution::arm_options_t options;
425 if(arm_solution->get_optional(options)) {
426 for(auto& i : options) {
427 // foreach optional value
428 char c= i.first;
429 if(gcode->has_letter(c)) { // set new value
430 i.second= gcode->get_value(c);
431 }
432 // print all current values of supported options
433 gcode->stream->printf("%c: %8.4f ", i.first, i.second);
434 gcode->add_nl = true;
435 }
436 // set the new options
437 arm_solution->set_optional(options);
438 }
439
440 // set delta segments per second, not saved by M500
441 if(gcode->has_letter('S')) {
442 this->delta_segments_per_second= gcode->get_value('S');
443 }
444 break;
445 }
446 }
447 }
448
449 if( this->motion_mode < 0)
450 return;
451
452 //Get parameters
453 float target[3], offset[3];
454 clear_vector(offset);
455
456 memcpy(target, this->last_milestone, sizeof(target)); //default to last target
457
458 for(char letter = 'I'; letter <= 'K'; letter++){
459 if( gcode->has_letter(letter) ){
460 offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));
461 }
462 }
463 for(char letter = 'X'; letter <= 'Z'; letter++){
464 if( gcode->has_letter(letter) ){
465 target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']);
466 }
467 }
468
469 if( gcode->has_letter('F') )
470 {
471 if( this->motion_mode == MOTION_MODE_SEEK )
472 this->seek_rate = this->to_millimeters( gcode->get_value('F') );
473 else
474 this->feed_rate = this->to_millimeters( gcode->get_value('F') );
475 }
476
477 //Perform any physical actions
478 switch( next_action ){
479 case NEXT_ACTION_DEFAULT:
480 switch(this->motion_mode){
481 case MOTION_MODE_CANCEL: break;
482 case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break;
483 case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break;
484 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
485 }
486 break;
487 }
488
489 // As far as the parser is concerned, the position is now == target. In reality the
490 // motion control system might still be processing the action and the real tool position
491 // in any intermediate location.
492 memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->position[] = target[];
493
494 }
495
496 // We received a new gcode, and one of the functions
497 // determined the distance for that given gcode. So now we can attach this gcode to the right block
498 // and continue
499 void Robot::distance_in_gcode_is_known(Gcode* gcode){
500
501 //If the queue is empty, execute immediatly, otherwise attach to the last added block
502 THEKERNEL->conveyor->append_gcode(gcode);
503 }
504
505 // Reset the position for all axes ( used in homing and G92 stuff )
506 void Robot::reset_axis_position(float position, int axis) {
507 this->last_milestone[axis] = position;
508
509 float actuator_pos[3];
510 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
511
512 for (int i = 0; i < 3; i++)
513 actuators[i]->change_last_milestone(actuator_pos[i]);
514 }
515
516
517 // Convert target from millimeters to steps, and append this to the planner
518 void Robot::append_milestone( float target[], float rate_mm_s )
519 {
520 float deltas[3];
521 float unit_vec[3];
522 float actuator_pos[3];
523 float millimeters_of_travel;
524
525 // find distance moved by each axis
526 for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
527 deltas[axis] = target[axis] - last_milestone[axis];
528
529 // Compute how long this move moves, so we can attach it to the block for later use
530 millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
531
532 // find distance unit vector
533 for (int i = 0; i < 3; i++)
534 unit_vec[i] = deltas[i] / millimeters_of_travel;
535
536 // Do not move faster than the configured cartesian limits
537 for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
538 {
539 if ( max_speeds[axis] > 0 )
540 {
541 float axis_speed = fabs(unit_vec[axis] * rate_mm_s);
542
543 if (axis_speed > max_speeds[axis])
544 rate_mm_s *= ( max_speeds[axis] / axis_speed );
545 }
546 }
547
548 // find actuator position given cartesian position
549 arm_solution->cartesian_to_actuator( target, actuator_pos );
550
551 // check per-actuator speed limits
552 for (int actuator = 0; actuator <= 2; actuator++)
553 {
554 float actuator_rate = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel;
555
556 if (actuator_rate > actuators[actuator]->max_rate)
557 rate_mm_s *= (actuators[actuator]->max_rate / actuator_rate);
558 }
559
560 // Append the block to the planner
561 THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
562
563 // Update the last_milestone to the current target for the next time we use last_milestone
564 memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->last_milestone[] = target[];
565
566 }
567
568 // Append a move to the queue ( cutting it into segments if needed )
569 void Robot::append_line(Gcode* gcode, float target[], float rate_mm_s ){
570
571 // Find out the distance for this gcode
572 gcode->millimeters_of_travel = pow( target[X_AXIS]-this->last_milestone[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->last_milestone[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->last_milestone[Z_AXIS], 2 );
573
574 // We ignore non-moves ( for example, extruder moves are not XYZ moves )
575 if( gcode->millimeters_of_travel < 1e-8F ){
576 return;
577 }
578
579 gcode->millimeters_of_travel = sqrtf(gcode->millimeters_of_travel);
580
581 // Mark the gcode as having a known distance
582 this->distance_in_gcode_is_known( gcode );
583
584 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
585 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
586 // 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
587 uint16_t segments;
588
589 if(this->delta_segments_per_second > 1.0F) {
590 // enabled if set to something > 1, it is set to 0.0 by default
591 // segment based on current speed and requested segments per second
592 // the faster the travel speed the fewer segments needed
593 // NOTE rate is mm/sec and we take into account any speed override
594 float seconds = gcode->millimeters_of_travel / rate_mm_s;
595 segments= max(1, ceil(this->delta_segments_per_second * seconds));
596 // TODO if we are only moving in Z on a delta we don't really need to segment at all
597
598 }else{
599 if(this->mm_per_line_segment == 0.0F){
600 segments= 1; // don't split it up
601 }else{
602 segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
603 }
604 }
605
606 if (segments > 1)
607 {
608 // A vector to keep track of the endpoint of each segment
609 float segment_delta[3];
610 float segment_end[3];
611
612 // How far do we move each segment?
613 for (int i = X_AXIS; i <= Z_AXIS; i++)
614 segment_delta[i] = (target[i] - last_milestone[i]) / segments;
615
616 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
617 // We always add another point after this loop so we stop at segments-1, ie i < segments
618 for (int i = 1; i < segments; i++)
619 {
620 for(int axis=X_AXIS; axis <= Z_AXIS; axis++ )
621 segment_end[axis] = last_milestone[axis] + segment_delta[axis];
622
623 // Append the end of this segment to the queue
624 this->append_milestone(segment_end, rate_mm_s);
625 }
626 }
627
628 // Append the end of this full move to the queue
629 this->append_milestone(target, rate_mm_s);
630
631 // if adding these blocks didn't start executing, do that now
632 THEKERNEL->conveyor->ensure_running();
633 }
634
635
636 // Append an arc to the queue ( cutting it into segments as needed )
637 void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ){
638
639 // Scary math
640 float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
641 float center_axis1 = this->last_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
642 float linear_travel = target[this->plane_axis_2] - this->last_milestone[this->plane_axis_2];
643 float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
644 float r_axis1 = -offset[this->plane_axis_1];
645 float rt_axis0 = target[this->plane_axis_0] - center_axis0;
646 float rt_axis1 = target[this->plane_axis_1] - center_axis1;
647
648 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
649 float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
650 if (angular_travel < 0) { angular_travel += 2*M_PI; }
651 if (is_clockwise) { angular_travel -= 2*M_PI; }
652
653 // Find the distance for this gcode
654 gcode->millimeters_of_travel = hypotf(angular_travel*radius, fabs(linear_travel));
655
656 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
657 if( gcode->millimeters_of_travel < 0.0001F ){ return; }
658
659 // Mark the gcode as having a known distance
660 this->distance_in_gcode_is_known( gcode );
661
662 // Figure out how many segments for this gcode
663 uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
664
665 float theta_per_segment = angular_travel/segments;
666 float linear_per_segment = linear_travel/segments;
667
668 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
669 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
670 r_T = [cos(phi) -sin(phi);
671 sin(phi) cos(phi] * r ;
672 For arc generation, the center of the circle is the axis of rotation and the radius vector is
673 defined from the circle center to the initial position. Each line segment is formed by successive
674 vector rotations. This requires only two cos() and sin() computations to form the rotation
675 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
676 all float numbers are single precision on the Arduino. (True float precision will not have
677 round off issues for CNC applications.) Single precision error can accumulate to be greater than
678 tool precision in some cases. Therefore, arc path correction is implemented.
679
680 Small angle approximation may be used to reduce computation overhead further. This approximation
681 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
682 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
683 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
684 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
685 issue for CNC machines with the single precision Arduino calculations.
686 This approximation also allows mc_arc to immediately insert a line segment into the planner
687 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
688 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
689 This is important when there are successive arc motions.
690 */
691 // Vector rotation matrix values
692 float cos_T = 1-0.5F*theta_per_segment*theta_per_segment; // Small angle approximation
693 float sin_T = theta_per_segment;
694
695 float arc_target[3];
696 float sin_Ti;
697 float cos_Ti;
698 float r_axisi;
699 uint16_t i;
700 int8_t count = 0;
701
702 // Initialize the linear axis
703 arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
704
705 for (i = 1; i<segments; i++) { // Increment (segments-1)
706
707 if (count < this->arc_correction ) {
708 // Apply vector rotation matrix
709 r_axisi = r_axis0*sin_T + r_axis1*cos_T;
710 r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
711 r_axis1 = r_axisi;
712 count++;
713 } else {
714 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
715 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
716 cos_Ti = cosf(i*theta_per_segment);
717 sin_Ti = sinf(i*theta_per_segment);
718 r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
719 r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
720 count = 0;
721 }
722
723 // Update arc_target location
724 arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
725 arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
726 arc_target[this->plane_axis_2] += linear_per_segment;
727
728 // Append this segment to the queue
729 this->append_milestone(arc_target, this->feed_rate / seconds_per_minute);
730
731 }
732
733 // Ensure last segment arrives at target location.
734 this->append_milestone(target, this->feed_rate / seconds_per_minute);
735 }
736
737 // Do the math for an arc and add it to the queue
738 void Robot::compute_arc(Gcode* gcode, float offset[], float target[]){
739
740 // Find the radius
741 float radius = hypotf(offset[this->plane_axis_0], offset[this->plane_axis_1]);
742
743 // Set clockwise/counter-clockwise sign for mc_arc computations
744 bool is_clockwise = false;
745 if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
746
747 // Append arc
748 this->append_arc(gcode, target, offset, radius, is_clockwise );
749
750 }
751
752
753 float Robot::theta(float x, float y){
754 float t = atanf(x/fabs(y));
755 if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
756 }
757
758 void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
759 this->plane_axis_0 = axis_0;
760 this->plane_axis_1 = axis_1;
761 this->plane_axis_2 = axis_2;
762 }
763
764
Unnamed repository; edit this file 'description' to name the repository.