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