HCoop / clinton / Smoothieware.git / blame_incremental
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (non-incremental) | history | HEAD
modified: ConfigSamples/FirePick.delta/config
[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp
... / ...
CommitLineData
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 "mbed.h" // for us_ticker_read()
12
13#include <math.h>
14#include <string>
15using std::string;
16
17#include "Planner.h"
18#include "Conveyor.h"
19#include "Robot.h"
20#include "nuts_bolts.h"
21#include "Pin.h"
22#include "StepperMotor.h"
23#include "Gcode.h"
24#include "PublicDataRequest.h"
25#include "RobotPublicAccess.h"
26#include "arm_solutions/BaseSolution.h"
27#include "arm_solutions/CartesianSolution.h"
28#include "arm_solutions/RotatableCartesianSolution.h"
29#include "arm_solutions/LinearDeltaSolution.h"
30#include "arm_solutions/RotatableDeltaSolution.h"
31#include "arm_solutions/HBotSolution.h"
32#include "arm_solutions/MorganSCARASolution.h"
33#include "StepTicker.h"
34#include "checksumm.h"
35#include "utils.h"
36#include "ConfigValue.h"
37#include "libs/StreamOutput.h"
38#include "StreamOutputPool.h"
39
40#define default_seek_rate_checksum CHECKSUM("default_seek_rate")
41#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
42#define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
43#define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
44#define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
45#define arc_correction_checksum CHECKSUM("arc_correction")
46#define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
47#define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
48#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
49
50// arm solutions
51#define arm_solution_checksum CHECKSUM("arm_solution")
52#define cartesian_checksum CHECKSUM("cartesian")
53#define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
54#define rostock_checksum CHECKSUM("rostock")
55#define linear_delta_checksum CHECKSUM("linear_delta")
56#define rotatable_delta_checksum CHECKSUM("rotatable_delta")
57#define delta_checksum CHECKSUM("delta")
58#define hbot_checksum CHECKSUM("hbot")
59#define corexy_checksum CHECKSUM("corexy")
60#define kossel_checksum CHECKSUM("kossel")
61#define morgan_checksum CHECKSUM("morgan")
62
63// stepper motor stuff
64#define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
65#define beta_step_pin_checksum CHECKSUM("beta_step_pin")
66#define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
67#define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
68#define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
69#define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
70#define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
71#define beta_en_pin_checksum CHECKSUM("beta_en_pin")
72#define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
73
74#define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
75#define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
76#define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
77
78#define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
79#define beta_max_rate_checksum CHECKSUM("beta_max_rate")
80#define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
81
82
83// new-style actuator stuff
84#define actuator_checksum CHEKCSUM("actuator")
85
86#define step_pin_checksum CHECKSUM("step_pin")
87#define dir_pin_checksum CHEKCSUM("dir_pin")
88#define en_pin_checksum CHECKSUM("en_pin")
89
90#define steps_per_mm_checksum CHECKSUM("steps_per_mm")
91#define max_rate_checksum CHECKSUM("max_rate")
92
93#define alpha_checksum CHECKSUM("alpha")
94#define beta_checksum CHECKSUM("beta")
95#define gamma_checksum CHECKSUM("gamma")
96
97
98#define NEXT_ACTION_DEFAULT 0
99#define NEXT_ACTION_DWELL 1
100#define NEXT_ACTION_GO_HOME 2
101
102#define MOTION_MODE_SEEK 0 // G0
103#define MOTION_MODE_LINEAR 1 // G1
104#define MOTION_MODE_CW_ARC 2 // G2
105#define MOTION_MODE_CCW_ARC 3 // G3
106#define MOTION_MODE_CANCEL 4 // G80
107
108#define PATH_CONTROL_MODE_EXACT_PATH 0
109#define PATH_CONTROL_MODE_EXACT_STOP 1
110#define PATH_CONTROL_MODE_CONTINOUS 2
111
112#define PROGRAM_FLOW_RUNNING 0
113#define PROGRAM_FLOW_PAUSED 1
114#define PROGRAM_FLOW_COMPLETED 2
115
116#define SPINDLE_DIRECTION_CW 0
117#define SPINDLE_DIRECTION_CCW 1
118
119// 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
120// It takes care of cutting arcs into segments, same thing for line that are too long
121
122Robot::Robot()
123{
124 this->inch_mode = false;
125 this->absolute_mode = true;
126 this->motion_mode = MOTION_MODE_SEEK;
127 this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
128 clear_vector(this->last_milestone);
129 clear_vector(this->transformed_last_milestone);
130 this->arm_solution = NULL;
131 seconds_per_minute = 60.0F;
132 this->clearToolOffset();
133 this->compensationTransform= nullptr;
134 this->halted= false;
135}
136
137//Called when the module has just been loaded
138void Robot::on_module_loaded()
139{
140 this->register_for_event(ON_GCODE_RECEIVED);
141 this->register_for_event(ON_GET_PUBLIC_DATA);
142 this->register_for_event(ON_SET_PUBLIC_DATA);
143 this->register_for_event(ON_HALT);
144
145 // Configuration
146 this->on_config_reload(this);
147}
148
149void Robot::on_config_reload(void *argument)
150{
151
152 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
153 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
154 // To make adding those solution easier, they have their own, separate object.
155 // Here we read the config to find out which arm solution to use
156 if (this->arm_solution) delete this->arm_solution;
157 int solution_checksum = get_checksum(THEKERNEL->config->value((unsigned int)arm_solution_checksum)->by_default("cartesian")->as_string());
158 // Note checksums are not const expressions when in debug mode, so don't use switch
159 if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
160 this->arm_solution = new HBotSolution(THEKERNEL->config);
161
162 } else if(solution_checksum == rostock_checksum || solution_checksum == kossel_checksum || solution_checksum == delta_checksum || solution_checksum == linear_delta_checksum) {
163 this->arm_solution = new LinearDeltaSolution(THEKERNEL->config);
164
165 } else if(solution_checksum == rotatable_cartesian_checksum) {
166 this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
167
168 } else if(solution_checksum == rotatable_delta_checksum) {
169 this->arm_solution = new RotatableDeltaSolution(THEKERNEL->config);
170
171
172 } else if(solution_checksum == morgan_checksum) {
173 this->arm_solution = new MorganSCARASolution(THEKERNEL->config);
174
175 } else if(solution_checksum == cartesian_checksum) {
176 this->arm_solution = new CartesianSolution(THEKERNEL->config);
177
178 } else {
179 this->arm_solution = new CartesianSolution(THEKERNEL->config);
180 }
181
182
183 this->feed_rate = THEKERNEL->config->value((unsigned int)default_feed_rate_checksum )->by_default( 100.0F)->as_number();
184 this->seek_rate = THEKERNEL->config->value((unsigned int)default_seek_rate_checksum )->by_default( 100.0F)->as_number();
185 this->mm_per_line_segment = THEKERNEL->config->value((unsigned int)mm_per_line_segment_checksum )->by_default( 0.0F)->as_number();
186 this->delta_segments_per_second = THEKERNEL->config->value((unsigned int)delta_segments_per_second_checksum )->by_default(0.0f )->as_number();
187 this->mm_per_arc_segment = THEKERNEL->config->value((unsigned int)mm_per_arc_segment_checksum )->by_default( 0.5f)->as_number();
188 this->arc_correction = THEKERNEL->config->value((unsigned int)arc_correction_checksum )->by_default( 5 )->as_number();
189
190 this->max_speeds[X_AXIS] = THEKERNEL->config->value((unsigned int)x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
191 this->max_speeds[Y_AXIS] = THEKERNEL->config->value((unsigned int)y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
192 this->max_speeds[Z_AXIS] = THEKERNEL->config->value((unsigned int)z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
193
194 Pin alpha_step_pin;
195 Pin alpha_dir_pin;
196 Pin alpha_en_pin;
197 Pin beta_step_pin;
198 Pin beta_dir_pin;
199 Pin beta_en_pin;
200 Pin gamma_step_pin;
201 Pin gamma_dir_pin;
202 Pin gamma_en_pin;
203
204 alpha_step_pin.from_string( THEKERNEL->config->value((unsigned int)alpha_step_pin_checksum )->by_default("2.0" )->as_string())->as_output();
205 alpha_dir_pin.from_string( THEKERNEL->config->value((unsigned int)alpha_dir_pin_checksum )->by_default("0.5" )->as_string())->as_output();
206 alpha_en_pin.from_string( THEKERNEL->config->value((unsigned int)alpha_en_pin_checksum )->by_default("0.4" )->as_string())->as_output();
207 beta_step_pin.from_string( THEKERNEL->config->value((unsigned int)beta_step_pin_checksum )->by_default("2.1" )->as_string())->as_output();
208 beta_dir_pin.from_string( THEKERNEL->config->value((unsigned int)beta_dir_pin_checksum )->by_default("0.11" )->as_string())->as_output();
209 beta_en_pin.from_string( THEKERNEL->config->value((unsigned int)beta_en_pin_checksum )->by_default("0.10" )->as_string())->as_output();
210 gamma_step_pin.from_string( THEKERNEL->config->value((unsigned int)gamma_step_pin_checksum )->by_default("2.2" )->as_string())->as_output();
211 gamma_dir_pin.from_string( THEKERNEL->config->value((unsigned int)gamma_dir_pin_checksum )->by_default("0.20" )->as_string())->as_output();
212 gamma_en_pin.from_string( THEKERNEL->config->value((unsigned int)gamma_en_pin_checksum )->by_default("0.19" )->as_string())->as_output();
213
214 float steps_per_mm[3] = {
215 THEKERNEL->config->value((unsigned int)alpha_steps_per_mm_checksum)->by_default( 80.0F)->as_number(),
216 THEKERNEL->config->value((unsigned int)beta_steps_per_mm_checksum )->by_default( 80.0F)->as_number(),
217 THEKERNEL->config->value((unsigned int)gamma_steps_per_mm_checksum)->by_default(2560.0F)->as_number(),
218 };
219
220 // TODO: delete or detect old steppermotors
221 // Make our 3 StepperMotors
222 this->alpha_stepper_motor = new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin);
223 this->beta_stepper_motor = new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin );
224 this->gamma_stepper_motor = new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin);
225
226 alpha_stepper_motor->change_steps_per_mm(steps_per_mm[0]);
227 beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
228 gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
229
230 alpha_stepper_motor->set_max_rate(THEKERNEL->config->value((unsigned int)alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F);
231 beta_stepper_motor->set_max_rate(THEKERNEL->config->value((unsigned int)beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F);
232 gamma_stepper_motor->set_max_rate(THEKERNEL->config->value((unsigned int)gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F);
233 check_max_actuator_speeds(); // check the configs are sane
234
235 actuators.clear();
236 actuators.push_back(alpha_stepper_motor);
237 actuators.push_back(beta_stepper_motor);
238 actuators.push_back(gamma_stepper_motor);
239
240
241 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
242 // so the first move can be correct if homing is not performed
243 float actuator_pos[3];
244 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
245 for (int i = 0; i < 3; i++)
246 actuators[i]->change_last_milestone(actuator_pos[i]);
247
248 //this->clearToolOffset();
249}
250
251// this does a sanity check that actuator speeds do not exceed steps rate capability
252// we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
253void Robot::check_max_actuator_speeds()
254{
255 float step_freq= alpha_stepper_motor->get_max_rate() * alpha_stepper_motor->get_steps_per_mm();
256 if(step_freq > THEKERNEL->base_stepping_frequency) {
257 alpha_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / alpha_stepper_motor->get_steps_per_mm()));
258 THEKERNEL->streams->printf("WARNING: alpha_max_rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", step_freq, alpha_stepper_motor->max_rate);
259 }
260
261 step_freq= beta_stepper_motor->get_max_rate() * beta_stepper_motor->get_steps_per_mm();
262 if(step_freq > THEKERNEL->base_stepping_frequency) {
263 beta_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / beta_stepper_motor->get_steps_per_mm()));
264 THEKERNEL->streams->printf("WARNING: beta_max_rate exceeds base_stepping_frequency * beta_steps_per_mm: %f, setting to %f\n", step_freq, beta_stepper_motor->max_rate);
265 }
266
267 step_freq= gamma_stepper_motor->get_max_rate() * gamma_stepper_motor->get_steps_per_mm();
268 if(step_freq > THEKERNEL->base_stepping_frequency) {
269 gamma_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / gamma_stepper_motor->get_steps_per_mm()));
270 THEKERNEL->streams->printf("WARNING: gamma_max_rate exceeds base_stepping_frequency * gamma_steps_per_mm: %f, setting to %f\n", step_freq, gamma_stepper_motor->max_rate);
271 }
272}
273
274void Robot::on_halt(void *arg)
275{
276 halted= (arg == nullptr);
277}
278
279void Robot::on_get_public_data(void *argument)
280{
281 PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
282
283 if(!pdr->starts_with(robot_checksum)) return;
284
285 if(pdr->second_element_is(speed_override_percent_checksum)) {
286 static float return_data;
287 return_data = 100.0F * 60.0F / seconds_per_minute;
288 pdr->set_data_ptr(&return_data);
289 pdr->set_taken();
290
291 } else if(pdr->second_element_is(current_position_checksum)) {
292 static float return_data[3];
293 return_data[0] = from_millimeters(this->last_milestone[0]);
294 return_data[1] = from_millimeters(this->last_milestone[1]);
295 return_data[2] = from_millimeters(this->last_milestone[2]);
296
297 pdr->set_data_ptr(&return_data);
298 pdr->set_taken();
299 }
300}
301
302void Robot::on_set_public_data(void *argument)
303{
304 PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
305
306 if(!pdr->starts_with(robot_checksum)) return;
307
308 if(pdr->second_element_is(speed_override_percent_checksum)) {
309 // NOTE do not use this while printing!
310 float t = *static_cast<float *>(pdr->get_data_ptr());
311 // enforce minimum 10% speed
312 if (t < 10.0F) t = 10.0F;
313
314 this->seconds_per_minute = t / 0.6F; // t * 60 / 100
315 pdr->set_taken();
316 } else if(pdr->second_element_is(current_position_checksum)) {
317 float *t = static_cast<float *>(pdr->get_data_ptr());
318 for (int i = 0; i < 3; i++) {
319 this->last_milestone[i] = this->to_millimeters(t[i]);
320 }
321
322 float actuator_pos[3];
323 arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
324 for (int i = 0; i < 3; i++)
325 actuators[i]->change_last_milestone(actuator_pos[i]);
326
327 pdr->set_taken();
328 }
329}
330
331//A GCode has been received
332//See if the current Gcode line has some orders for us
333void Robot::on_gcode_received(void *argument)
334{
335 Gcode *gcode = static_cast<Gcode *>(argument);
336
337 this->motion_mode = -1;
338
339 //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
340 if( gcode->has_g) {
341 switch( gcode->g ) {
342 case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break;
343 case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break;
344 case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break;
345 case 3: this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken(); break;
346 case 4: {
347 uint32_t delay_ms= 0;
348 if (gcode->has_letter('P')) {
349 delay_ms= gcode->get_int('P');
350 }
351 if (gcode->has_letter('S')) {
352 delay_ms += gcode->get_int('S') * 1000;
353 }
354 if (delay_ms > 0){
355 // drain queue
356 THEKERNEL->conveyor->wait_for_empty_queue();
357 // wait for specified time
358 uint32_t start= us_ticker_read(); // mbed call
359 while ((us_ticker_read() - start) < delay_ms*1000) {
360 THEKERNEL->call_event(ON_IDLE, this);
361 }
362 }
363 gcode->mark_as_taken();
364 }
365 break; //added by DHP to suppress warning 20150324
366 case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken(); break;
367 case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken(); break;
368 case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken(); break;
369 case 20: this->inch_mode = true; gcode->mark_as_taken(); break;
370 case 21: this->inch_mode = false; gcode->mark_as_taken(); break;
371 case 90: this->absolute_mode = true; gcode->mark_as_taken(); break;
372 case 91: this->absolute_mode = false; gcode->mark_as_taken(); break;
373 case 92: {
374 if(gcode->get_num_args() == 0) {
375 for (int i = X_AXIS; i <= Z_AXIS; ++i) {
376 reset_axis_position(0, i);
377 }
378
379 } else {
380 for (char letter = 'X'; letter <= 'Z'; letter++) {
381 if ( gcode->has_letter(letter) ) {
382 reset_axis_position(this->to_millimeters(gcode->get_value(letter)), letter - 'X');
383 }
384 }
385 }
386
387 gcode->mark_as_taken();
388 return;
389 }
390 }
391 } else if( gcode->has_m) {
392 switch( gcode->m ) {
393 case 92: // M92 - set steps per mm
394 if (gcode->has_letter('X'))
395 actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X')));
396 if (gcode->has_letter('Y'))
397 actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y')));
398 if (gcode->has_letter('Z'))
399 actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z')));
400 if (gcode->has_letter('F'))
401 seconds_per_minute = gcode->get_value('F');
402
403 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);
404 gcode->add_nl = true;
405 gcode->mark_as_taken();
406 check_max_actuator_speeds();
407 return;
408
409 case 114: {
410 char buf[64];
411 int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f A:%1.3f B:%1.3f C:%1.3f ",
412 from_millimeters(this->last_milestone[0]),
413 from_millimeters(this->last_milestone[1]),
414 from_millimeters(this->last_milestone[2]),
415 actuators[X_AXIS]->get_current_position(),
416 actuators[Y_AXIS]->get_current_position(),
417 actuators[Z_AXIS]->get_current_position() );
418 gcode->txt_after_ok.append(buf, n);
419 gcode->mark_as_taken();
420 }
421 return;
422
423 case 120: { // push state
424 gcode->mark_as_taken();
425 bool b= this->absolute_mode;
426 saved_state_t s(this->feed_rate, this->seek_rate, b);
427 state_stack.push(s);
428 }
429 break;
430
431 case 121: // pop state
432 gcode->mark_as_taken();
433 if(!state_stack.empty()) {
434 auto s= state_stack.top();
435 state_stack.pop();
436 this->feed_rate= std::get<0>(s);
437 this->seek_rate= std::get<1>(s);
438 this->absolute_mode= std::get<2>(s);
439 }
440 break;
441
442 case 203: // M203 Set maximum feedrates in mm/sec
443 if (gcode->has_letter('X'))
444 this->max_speeds[X_AXIS] = gcode->get_value('X');
445 if (gcode->has_letter('Y'))
446 this->max_speeds[Y_AXIS] = gcode->get_value('Y');
447 if (gcode->has_letter('Z'))
448 this->max_speeds[Z_AXIS] = gcode->get_value('Z');
449 if (gcode->has_letter('A'))
450 alpha_stepper_motor->set_max_rate(gcode->get_value('A'));
451 if (gcode->has_letter('B'))
452 beta_stepper_motor->set_max_rate(gcode->get_value('B'));
453 if (gcode->has_letter('C'))
454 gamma_stepper_motor->set_max_rate(gcode->get_value('C'));
455
456 check_max_actuator_speeds();
457
458 gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
459 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
460 alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate());
461 gcode->add_nl = true;
462 gcode->mark_as_taken();
463 break;
464
465 case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
466 gcode->mark_as_taken();
467
468 if (gcode->has_letter('S')) {
469 float acc = gcode->get_value('S'); // mm/s^2
470 // enforce minimum
471 if (acc < 1.0F)
472 acc = 1.0F;
473 THEKERNEL->planner->acceleration = acc;
474 }
475 if (gcode->has_letter('Z')) {
476 float acc = gcode->get_value('Z'); // mm/s^2
477 // enforce positive
478 if (acc < 0.0F)
479 acc = 0.0F;
480 THEKERNEL->planner->z_acceleration = acc;
481 }
482 break;
483
484 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
485 gcode->mark_as_taken();
486 if (gcode->has_letter('X')) {
487 float jd = gcode->get_value('X');
488 // enforce minimum
489 if (jd < 0.0F)
490 jd = 0.0F;
491 THEKERNEL->planner->junction_deviation = jd;
492 }
493 if (gcode->has_letter('Z')) {
494 float jd = gcode->get_value('Z');
495 // enforce minimum, -1 disables it and uses regular junction deviation
496 if (jd < -1.0F)
497 jd = -1.0F;
498 THEKERNEL->planner->z_junction_deviation = jd;
499 }
500 if (gcode->has_letter('S')) {
501 float mps = gcode->get_value('S');
502 // enforce minimum
503 if (mps < 0.0F)
504 mps = 0.0F;
505 THEKERNEL->planner->minimum_planner_speed = mps;
506 }
507 if (gcode->has_letter('Y')) {
508 alpha_stepper_motor->default_minimum_actuator_rate = gcode->get_value('Y');
509 }
510 break;
511
512 case 220: // M220 - speed override percentage
513 gcode->mark_as_taken();
514 if (gcode->has_letter('S')) {
515 float factor = gcode->get_value('S');
516 // enforce minimum 10% speed
517 if (factor < 10.0F)
518 factor = 10.0F;
519 // enforce maximum 10x speed
520 if (factor > 1000.0F)
521 factor = 1000.0F;
522
523 seconds_per_minute = 6000.0F / factor;
524 }
525 break;
526
527 case 400: // wait until all moves are done up to this point
528 gcode->mark_as_taken();
529 THEKERNEL->conveyor->wait_for_empty_queue();
530 break;
531
532 case 500: // M500 saves some volatile settings to config override file
533 case 503: { // M503 just prints the settings
534 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);
535 gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL->planner->acceleration, THEKERNEL->planner->z_acceleration);
536 gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
537 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",
538 this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
539 alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate());
540
541 // get or save any arm solution specific optional values
542 BaseSolution::arm_options_t options;
543 if(arm_solution->get_optional(options) && !options.empty()) {
544 gcode->stream->printf(";Optional arm solution specific settings:\nM665");
545 for(auto &i : options) {
546 gcode->stream->printf(" %c%1.4f", i.first, i.second);
547 }
548 gcode->stream->printf("\n");
549 }
550 gcode->mark_as_taken();
551 break;
552 }
553
554 case 665: { // M665 set optional arm solution variables based on arm solution.
555 gcode->mark_as_taken();
556 // the parameter args could be any letter each arm solution only accepts certain ones
557 BaseSolution::arm_options_t options= gcode->get_args();
558 options.erase('S'); // don't include the S
559 options.erase('U'); // don't include the U
560 if(options.size() > 0) {
561 // set the specified options
562 arm_solution->set_optional(options);
563 }
564 options.clear();
565 if(arm_solution->get_optional(options)) {
566 // foreach optional value
567 for(auto &i : options) {
568 // print all current values of supported options
569 gcode->stream->printf("%c: %8.4f ", i.first, i.second);
570 gcode->add_nl = true;
571 }
572 }
573
574 if(gcode->has_letter('S')) { // set delta segments per second, not saved by M500
575 this->delta_segments_per_second = gcode->get_value('S');
576 gcode->stream->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second);
577
578 }else if(gcode->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
579 this->mm_per_line_segment = gcode->get_value('U');
580 this->delta_segments_per_second = 0;
581 gcode->stream->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment);
582 }
583
584 break;
585 }
586 }
587 }
588
589 if( this->motion_mode < 0)
590 return;
591
592 //Get parameters
593 float target[3], offset[3];
594 clear_vector(offset);
595
596 memcpy(target, this->last_milestone, sizeof(target)); //default to last target
597
598 for(char letter = 'I'; letter <= 'K'; letter++) {
599 if( gcode->has_letter(letter) ) {
600 offset[letter - 'I'] = this->to_millimeters(gcode->get_value(letter));
601 }
602 }
603 for(char letter = 'X'; letter <= 'Z'; letter++) {
604 if( gcode->has_letter(letter) ) {
605 target[letter - 'X'] = this->to_millimeters(gcode->get_value(letter)) + (this->absolute_mode ? this->toolOffset[letter - 'X'] : target[letter - 'X']);
606 }
607 }
608
609 if( gcode->has_letter('F') ) {
610 if( this->motion_mode == MOTION_MODE_SEEK )
611 this->seek_rate = this->to_millimeters( gcode->get_value('F') );
612 else
613 this->feed_rate = this->to_millimeters( gcode->get_value('F') );
614 }
615
616 //Perform any physical actions
617 switch(this->motion_mode) {
618 case MOTION_MODE_CANCEL: break;
619 case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break;
620 case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break;
621 case MOTION_MODE_CW_ARC:
622 case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
623 }
624
625 // last_milestone was set to target in append_milestone, no need to do it again
626
627}
628
629// We received a new gcode, and one of the functions
630// determined the distance for that given gcode. So now we can attach this gcode to the right block
631// and continue
632void Robot::distance_in_gcode_is_known(Gcode *gcode)
633{
634 //If the queue is empty, execute immediatly, otherwise attach to the last added block
635 THEKERNEL->conveyor->append_gcode(gcode);
636}
637
638// reset the position for all axis (used in homing for delta as last_milestone may be bogus)
639void Robot::reset_axis_position(float x, float y, float z)
640{
641 this->last_milestone[X_AXIS] = x;
642 this->last_milestone[Y_AXIS] = y;
643 this->last_milestone[Z_AXIS] = z;
644 this->transformed_last_milestone[X_AXIS] = x;
645 this->transformed_last_milestone[Y_AXIS] = y;
646 this->transformed_last_milestone[Z_AXIS] = z;
647
648 float actuator_pos[3];
649 arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
650 for (int i = 0; i < 3; i++)
651 actuators[i]->change_last_milestone(actuator_pos[i]);
652}
653
654// Reset the position for an axis (used in homing and G92)
655void Robot::reset_axis_position(float position, int axis)
656{
657 this->last_milestone[axis] = position;
658 this->transformed_last_milestone[axis] = position;
659
660 float actuator_pos[3];
661 arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
662
663 for (int i = 0; i < 3; i++)
664 actuators[i]->change_last_milestone(actuator_pos[i]);
665}
666
667// Use FK to find out where actuator is and reset lastmilestone to match
668void Robot::reset_position_from_current_actuator_position()
669{
670 float actuator_pos[]= {actuators[X_AXIS]->get_current_position(), actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position()};
671 arm_solution->actuator_to_cartesian(actuator_pos, this->last_milestone);
672 memcpy(this->transformed_last_milestone, this->last_milestone, sizeof(this->transformed_last_milestone));
673
674 // now reset actuator correctly, NOTE this may lose a little precision
675 arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
676 for (int i = 0; i < 3; i++)
677 actuators[i]->change_last_milestone(actuator_pos[i]);
678}
679
680// Convert target from millimeters to steps, and append this to the planner
681void Robot::append_milestone( float target[], float rate_mm_s )
682{
683 float deltas[3];
684 float unit_vec[3];
685 float actuator_pos[3];
686 float transformed_target[3]; // adjust target for bed compensation
687 float millimeters_of_travel;
688
689 // unity transform by default
690 memcpy(transformed_target, target, sizeof(transformed_target));
691
692 // check function pointer and call if set to transform the target to compensate for bed
693 if(compensationTransform) {
694 // some compensation strategies can transform XYZ, some just change Z
695 compensationTransform(transformed_target);
696 }
697
698 // find distance moved by each axis, use transformed target from last_transformed_target
699 for (int axis = X_AXIS; axis <= Z_AXIS; axis++){
700 deltas[axis] = transformed_target[axis] - transformed_last_milestone[axis];
701 }
702 // store last transformed
703 memcpy(this->transformed_last_milestone, transformed_target, sizeof(this->transformed_last_milestone));
704
705 // Compute how long this move moves, so we can attach it to the block for later use
706 millimeters_of_travel = sqrtf( powf( deltas[X_AXIS], 2 ) + powf( deltas[Y_AXIS], 2 ) + powf( deltas[Z_AXIS], 2 ) );
707
708 // find distance unit vector
709 for (int i = 0; i < 3; i++)
710 unit_vec[i] = deltas[i] / millimeters_of_travel;
711
712 // Do not move faster than the configured cartesian limits
713 for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
714 if ( max_speeds[axis] > 0 ) {
715 float axis_speed = fabs(unit_vec[axis] * rate_mm_s);
716
717 if (axis_speed > max_speeds[axis])
718 rate_mm_s *= ( max_speeds[axis] / axis_speed );
719 }
720 }
721
722 // find actuator position given cartesian position, use actual adjusted target
723 arm_solution->cartesian_to_actuator( transformed_target, actuator_pos );
724
725 // check per-actuator speed limits
726 for (int actuator = 0; actuator <= 2; actuator++) {
727 float actuator_rate = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel;
728
729 if (actuator_rate > actuators[actuator]->get_max_rate())
730 rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
731 }
732
733 // Append the block to the planner
734 THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
735
736 // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
737 memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->last_milestone[] = target[];
738
739}
740
741// Append a move to the queue ( cutting it into segments if needed )
742void Robot::append_line(Gcode *gcode, float target[], float rate_mm_s )
743{
744 // Find out the distance for this gcode
745 // NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
746 gcode->millimeters_of_travel = sqrtf(powf( target[X_AXIS] - this->last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - this->last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - this->last_milestone[Z_AXIS], 2 ));
747
748 // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
749 if( gcode->millimeters_of_travel < 0.00001F ) {
750 return;
751 }
752
753 // Mark the gcode as having a known distance
754 this->distance_in_gcode_is_known( gcode );
755
756 // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
757 // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
758 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
759 // 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
760 uint16_t segments;
761
762 if(this->delta_segments_per_second > 1.0F) {
763 // enabled if set to something > 1, it is set to 0.0 by default
764 // segment based on current speed and requested segments per second
765 // the faster the travel speed the fewer segments needed
766 // NOTE rate is mm/sec and we take into account any speed override
767 float seconds = gcode->millimeters_of_travel / rate_mm_s;
768 segments = max(1.0F, ceilf(this->delta_segments_per_second * seconds));
769 // TODO if we are only moving in Z on a delta we don't really need to segment at all
770
771 } else {
772 if(this->mm_per_line_segment == 0.0F) {
773 segments = 1; // don't split it up
774 } else {
775 segments = ceilf( gcode->millimeters_of_travel / this->mm_per_line_segment);
776 }
777 }
778
779 if (segments > 1) {
780 // A vector to keep track of the endpoint of each segment
781 float segment_delta[3];
782 float segment_end[3];
783
784 // How far do we move each segment?
785 for (int i = X_AXIS; i <= Z_AXIS; i++)
786 segment_delta[i] = (target[i] - last_milestone[i]) / segments;
787
788 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
789 // We always add another point after this loop so we stop at segments-1, ie i < segments
790 for (int i = 1; i < segments; i++) {
791 if(halted) return; // don't queue any more segments
792 for(int axis = X_AXIS; axis <= Z_AXIS; axis++ )
793 segment_end[axis] = last_milestone[axis] + segment_delta[axis];
794
795 // Append the end of this segment to the queue
796 this->append_milestone(segment_end, rate_mm_s);
797 }
798 }
799
800 // Append the end of this full move to the queue
801 this->append_milestone(target, rate_mm_s);
802
803 // if adding these blocks didn't start executing, do that now
804 THEKERNEL->conveyor->ensure_running();
805}
806
807
808// Append an arc to the queue ( cutting it into segments as needed )
809void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radius, bool is_clockwise )
810{
811
812 // Scary math
813 float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
814 float center_axis1 = this->last_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
815 float linear_travel = target[this->plane_axis_2] - this->last_milestone[this->plane_axis_2];
816 float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
817 float r_axis1 = -offset[this->plane_axis_1];
818 float rt_axis0 = target[this->plane_axis_0] - center_axis0;
819 float rt_axis1 = target[this->plane_axis_1] - center_axis1;
820
821 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
822 float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
823 if (angular_travel < 0) {
824 angular_travel += 2 * M_PI;
825 }
826 if (is_clockwise) {
827 angular_travel -= 2 * M_PI;
828 }
829
830 // Find the distance for this gcode
831 gcode->millimeters_of_travel = hypotf(angular_travel * radius, fabs(linear_travel));
832
833 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
834 if( gcode->millimeters_of_travel < 0.00001F ) {
835 return;
836 }
837
838 // Mark the gcode as having a known distance
839 this->distance_in_gcode_is_known( gcode );
840
841 // Figure out how many segments for this gcode
842 uint16_t segments = floorf(gcode->millimeters_of_travel / this->mm_per_arc_segment);
843
844 float theta_per_segment = angular_travel / segments;
845 float linear_per_segment = linear_travel / segments;
846
847 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
848 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
849 r_T = [cos(phi) -sin(phi);
850 sin(phi) cos(phi] * r ;
851 For arc generation, the center of the circle is the axis of rotation and the radius vector is
852 defined from the circle center to the initial position. Each line segment is formed by successive
853 vector rotations. This requires only two cos() and sin() computations to form the rotation
854 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
855 all float numbers are single precision on the Arduino. (True float precision will not have
856 round off issues for CNC applications.) Single precision error can accumulate to be greater than
857 tool precision in some cases. Therefore, arc path correction is implemented.
858
859 Small angle approximation may be used to reduce computation overhead further. This approximation
860 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
861 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
862 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
863 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
864 issue for CNC machines with the single precision Arduino calculations.
865 This approximation also allows mc_arc to immediately insert a line segment into the planner
866 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
867 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
868 This is important when there are successive arc motions.
869 */
870 // Vector rotation matrix values
871 float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation
872 float sin_T = theta_per_segment;
873
874 float arc_target[3];
875 float sin_Ti;
876 float cos_Ti;
877 float r_axisi;
878 uint16_t i;
879 int8_t count = 0;
880
881 // Initialize the linear axis
882 arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
883
884 for (i = 1; i < segments; i++) { // Increment (segments-1)
885 if(halted) return; // don't queue any more segments
886
887 if (count < this->arc_correction ) {
888 // Apply vector rotation matrix
889 r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
890 r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
891 r_axis1 = r_axisi;
892 count++;
893 } else {
894 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
895 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
896 cos_Ti = cosf(i * theta_per_segment);
897 sin_Ti = sinf(i * theta_per_segment);
898 r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti;
899 r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti;
900 count = 0;
901 }
902
903 // Update arc_target location
904 arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
905 arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
906 arc_target[this->plane_axis_2] += linear_per_segment;
907
908 // Append this segment to the queue
909 this->append_milestone(arc_target, this->feed_rate / seconds_per_minute);
910
911 }
912
913 // Ensure last segment arrives at target location.
914 this->append_milestone(target, this->feed_rate / seconds_per_minute);
915}
916
917// Do the math for an arc and add it to the queue
918void Robot::compute_arc(Gcode *gcode, float offset[], float target[])
919{
920
921 // Find the radius
922 float radius = hypotf(offset[this->plane_axis_0], offset[this->plane_axis_1]);
923
924 // Set clockwise/counter-clockwise sign for mc_arc computations
925 bool is_clockwise = false;
926 if( this->motion_mode == MOTION_MODE_CW_ARC ) {
927 is_clockwise = true;
928 }
929
930 // Append arc
931 this->append_arc(gcode, target, offset, radius, is_clockwise );
932
933}
934
935
936float Robot::theta(float x, float y)
937{
938 float t = atanf(x / fabs(y));
939 if (y > 0) {
940 return(t);
941 } else {
942 if (t > 0) {
943 return(M_PI - t);
944 } else {
945 return(-M_PI - t);
946 }
947 }
948}
949
950void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
951{
952 this->plane_axis_0 = axis_0;
953 this->plane_axis_1 = axis_1;
954 this->plane_axis_2 = axis_2;
955}
956
957void Robot::clearToolOffset()
958{
959 memset(this->toolOffset, 0, sizeof(this->toolOffset));
960}
961
962void Robot::setToolOffset(const float offset[3])
963{
964 memcpy(this->toolOffset, offset, sizeof(this->toolOffset));
965}
966
Unnamed repository; edit this file 'description' to name the repository.