Scott Lahteine
7 years ago
42 changed files with 1898 additions and 1370 deletions
File diff suppressed because it is too large
@ -0,0 +1,473 @@ |
|||
/**
|
|||
* Marlin 3D Printer Firmware |
|||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|||
* |
|||
* Based on Sprinter and grbl. |
|||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm |
|||
* |
|||
* This program 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. |
|||
* |
|||
* This program 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. |
|||
* |
|||
* You should have received a copy of the GNU General Public License |
|||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|||
* |
|||
*/ |
|||
|
|||
/**
|
|||
* queue.cpp - The G-code command queue |
|||
*/ |
|||
|
|||
#include "queue.h" |
|||
#include "gcode.h" |
|||
|
|||
#include "../lcd/ultralcd.h" |
|||
#include "../sd/cardreader.h" |
|||
#include "../module/planner.h" |
|||
#include "../Marlin.h" |
|||
|
|||
/**
|
|||
* GCode line number handling. Hosts may opt to include line numbers when |
|||
* sending commands to Marlin, and lines will be checked for sequentiality. |
|||
* M110 N<int> sets the current line number. |
|||
*/ |
|||
long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0; |
|||
|
|||
/**
|
|||
* GCode Command Queue |
|||
* A simple ring buffer of BUFSIZE command strings. |
|||
* |
|||
* Commands are copied into this buffer by the command injectors |
|||
* (immediate, serial, sd card) and they are processed sequentially by |
|||
* the main loop. The gcode.process_next_command method parses the next |
|||
* command and hands off execution to individual handler functions. |
|||
*/ |
|||
uint8_t commands_in_queue = 0, // Count of commands in the queue
|
|||
cmd_queue_index_r = 0, // Ring buffer read position
|
|||
cmd_queue_index_w = 0; // Ring buffer write position
|
|||
|
|||
char command_queue[BUFSIZE][MAX_CMD_SIZE]; |
|||
|
|||
/**
|
|||
* Serial command injection |
|||
*/ |
|||
|
|||
// Number of characters read in the current line of serial input
|
|||
static int serial_count = 0; |
|||
|
|||
bool send_ok[BUFSIZE]; |
|||
|
|||
/**
|
|||
* Next Injected Command pointer. NULL if no commands are being injected. |
|||
* Used by Marlin internally to ensure that commands initiated from within |
|||
* are enqueued ahead of any pending serial or sd card commands. |
|||
*/ |
|||
static const char *injected_commands_P = NULL; |
|||
|
|||
void queue_setup() { |
|||
// Send "ok" after commands by default
|
|||
for (uint8_t i = 0; i < COUNT(send_ok); i++) send_ok[i] = true; |
|||
} |
|||
|
|||
/**
|
|||
* Clear the Marlin command queue |
|||
*/ |
|||
void clear_command_queue() { |
|||
cmd_queue_index_r = cmd_queue_index_w; |
|||
commands_in_queue = 0; |
|||
} |
|||
|
|||
/**
|
|||
* Once a new command is in the ring buffer, call this to commit it |
|||
*/ |
|||
inline void _commit_command(bool say_ok) { |
|||
send_ok[cmd_queue_index_w] = say_ok; |
|||
if (++cmd_queue_index_w >= BUFSIZE) cmd_queue_index_w = 0; |
|||
commands_in_queue++; |
|||
} |
|||
|
|||
/**
|
|||
* Copy a command from RAM into the main command buffer. |
|||
* Return true if the command was successfully added. |
|||
* Return false for a full buffer, or if the 'command' is a comment. |
|||
*/ |
|||
inline bool _enqueuecommand(const char* cmd, bool say_ok/*=false*/) { |
|||
if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false; |
|||
strcpy(command_queue[cmd_queue_index_w], cmd); |
|||
_commit_command(say_ok); |
|||
return true; |
|||
} |
|||
|
|||
/**
|
|||
* Enqueue with Serial Echo |
|||
*/ |
|||
bool enqueue_and_echo_command(const char* cmd, bool say_ok/*=false*/) { |
|||
if (_enqueuecommand(cmd, say_ok)) { |
|||
SERIAL_ECHO_START(); |
|||
SERIAL_ECHOPAIR(MSG_ENQUEUEING, cmd); |
|||
SERIAL_CHAR('"'); |
|||
SERIAL_EOL(); |
|||
return true; |
|||
} |
|||
return false; |
|||
} |
|||
|
|||
/**
|
|||
* Inject the next "immediate" command, when possible, onto the front of the queue. |
|||
* Return true if any immediate commands remain to inject. |
|||
*/ |
|||
static bool drain_injected_commands_P() { |
|||
if (injected_commands_P != NULL) { |
|||
size_t i = 0; |
|||
char c, cmd[30]; |
|||
strncpy_P(cmd, injected_commands_P, sizeof(cmd) - 1); |
|||
cmd[sizeof(cmd) - 1] = '\0'; |
|||
while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
|
|||
cmd[i] = '\0'; |
|||
if (enqueue_and_echo_command(cmd)) // success?
|
|||
injected_commands_P = c ? injected_commands_P + i + 1 : NULL; // next command or done
|
|||
} |
|||
return (injected_commands_P != NULL); // return whether any more remain
|
|||
} |
|||
|
|||
/**
|
|||
* Record one or many commands to run from program memory. |
|||
* Aborts the current queue, if any. |
|||
* Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards |
|||
*/ |
|||
void enqueue_and_echo_commands_P(const char * const pgcode) { |
|||
injected_commands_P = pgcode; |
|||
drain_injected_commands_P(); // first command executed asap (when possible)
|
|||
} |
|||
|
|||
/**
|
|||
* Send an "ok" message to the host, indicating |
|||
* that a command was successfully processed. |
|||
* |
|||
* If ADVANCED_OK is enabled also include: |
|||
* N<int> Line number of the command, if any |
|||
* P<int> Planner space remaining |
|||
* B<int> Block queue space remaining |
|||
*/ |
|||
void ok_to_send() { |
|||
gcode.refresh_cmd_timeout(); |
|||
if (!send_ok[cmd_queue_index_r]) return; |
|||
SERIAL_PROTOCOLPGM(MSG_OK); |
|||
#if ENABLED(ADVANCED_OK) |
|||
char* p = command_queue[cmd_queue_index_r]; |
|||
if (*p == 'N') { |
|||
SERIAL_PROTOCOL(' '); |
|||
SERIAL_ECHO(*p++); |
|||
while (NUMERIC_SIGNED(*p)) |
|||
SERIAL_ECHO(*p++); |
|||
} |
|||
SERIAL_PROTOCOLPGM(" P"); SERIAL_PROTOCOL(int(BLOCK_BUFFER_SIZE - planner.movesplanned() - 1)); |
|||
SERIAL_PROTOCOLPGM(" B"); SERIAL_PROTOCOL(BUFSIZE - commands_in_queue); |
|||
#endif |
|||
SERIAL_EOL(); |
|||
} |
|||
|
|||
/**
|
|||
* Send a "Resend: nnn" message to the host to |
|||
* indicate that a command needs to be re-sent. |
|||
*/ |
|||
void flush_and_request_resend() { |
|||
//char command_queue[cmd_queue_index_r][100]="Resend:";
|
|||
MYSERIAL.flush(); |
|||
SERIAL_PROTOCOLPGM(MSG_RESEND); |
|||
SERIAL_PROTOCOLLN(gcode_LastN + 1); |
|||
ok_to_send(); |
|||
} |
|||
|
|||
void gcode_line_error(const char* err, bool doFlush = true) { |
|||
SERIAL_ERROR_START(); |
|||
serialprintPGM(err); |
|||
SERIAL_ERRORLN(gcode_LastN); |
|||
//Serial.println(gcode_N);
|
|||
if (doFlush) flush_and_request_resend(); |
|||
serial_count = 0; |
|||
} |
|||
|
|||
/**
|
|||
* Get all commands waiting on the serial port and queue them. |
|||
* Exit when the buffer is full or when no more characters are |
|||
* left on the serial port. |
|||
*/ |
|||
inline void get_serial_commands() { |
|||
static char serial_line_buffer[MAX_CMD_SIZE]; |
|||
static bool serial_comment_mode = false; |
|||
|
|||
// If the command buffer is empty for too long,
|
|||
// send "wait" to indicate Marlin is still waiting.
|
|||
#if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0 |
|||
static millis_t last_command_time = 0; |
|||
const millis_t ms = millis(); |
|||
if (commands_in_queue == 0 && !MYSERIAL.available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) { |
|||
SERIAL_ECHOLNPGM(MSG_WAIT); |
|||
last_command_time = ms; |
|||
} |
|||
#endif |
|||
|
|||
/**
|
|||
* Loop while serial characters are incoming and the queue is not full |
|||
*/ |
|||
while (commands_in_queue < BUFSIZE && MYSERIAL.available() > 0) { |
|||
|
|||
char serial_char = MYSERIAL.read(); |
|||
|
|||
/**
|
|||
* If the character ends the line |
|||
*/ |
|||
if (serial_char == '\n' || serial_char == '\r') { |
|||
|
|||
serial_comment_mode = false; // end of line == end of comment
|
|||
|
|||
if (!serial_count) continue; // skip empty lines
|
|||
|
|||
serial_line_buffer[serial_count] = 0; // terminate string
|
|||
serial_count = 0; //reset buffer
|
|||
|
|||
char* command = serial_line_buffer; |
|||
|
|||
while (*command == ' ') command++; // skip any leading spaces
|
|||
char *npos = (*command == 'N') ? command : NULL, // Require the N parameter to start the line
|
|||
*apos = strchr(command, '*'); |
|||
|
|||
if (npos) { |
|||
|
|||
bool M110 = strstr_P(command, PSTR("M110")) != NULL; |
|||
|
|||
if (M110) { |
|||
char* n2pos = strchr(command + 4, 'N'); |
|||
if (n2pos) npos = n2pos; |
|||
} |
|||
|
|||
gcode_N = strtol(npos + 1, NULL, 10); |
|||
|
|||
if (gcode_N != gcode_LastN + 1 && !M110) { |
|||
gcode_line_error(PSTR(MSG_ERR_LINE_NO)); |
|||
return; |
|||
} |
|||
|
|||
if (apos) { |
|||
byte checksum = 0, count = 0; |
|||
while (command[count] != '*') checksum ^= command[count++]; |
|||
|
|||
if (strtol(apos + 1, NULL, 10) != checksum) { |
|||
gcode_line_error(PSTR(MSG_ERR_CHECKSUM_MISMATCH)); |
|||
return; |
|||
} |
|||
// if no errors, continue parsing
|
|||
} |
|||
else { |
|||
gcode_line_error(PSTR(MSG_ERR_NO_CHECKSUM)); |
|||
return; |
|||
} |
|||
|
|||
gcode_LastN = gcode_N; |
|||
// if no errors, continue parsing
|
|||
} |
|||
else if (apos) { // No '*' without 'N'
|
|||
gcode_line_error(PSTR(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM), false); |
|||
return; |
|||
} |
|||
|
|||
// Movement commands alert when stopped
|
|||
if (IsStopped()) { |
|||
char* gpos = strchr(command, 'G'); |
|||
if (gpos) { |
|||
const int codenum = strtol(gpos + 1, NULL, 10); |
|||
switch (codenum) { |
|||
case 0: |
|||
case 1: |
|||
case 2: |
|||
case 3: |
|||
SERIAL_ERRORLNPGM(MSG_ERR_STOPPED); |
|||
LCD_MESSAGEPGM(MSG_STOPPED); |
|||
break; |
|||
} |
|||
} |
|||
} |
|||
|
|||
#if DISABLED(EMERGENCY_PARSER) |
|||
// If command was e-stop process now
|
|||
if (strcmp(command, "M108") == 0) { |
|||
wait_for_heatup = false; |
|||
#if ENABLED(ULTIPANEL) |
|||
wait_for_user = false; |
|||
#endif |
|||
} |
|||
if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED)); |
|||
if (strcmp(command, "M410") == 0) { quickstop_stepper(); } |
|||
#endif |
|||
|
|||
#if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0 |
|||
last_command_time = ms; |
|||
#endif |
|||
|
|||
// Add the command to the queue
|
|||
_enqueuecommand(serial_line_buffer, true); |
|||
} |
|||
else if (serial_count >= MAX_CMD_SIZE - 1) { |
|||
// Keep fetching, but ignore normal characters beyond the max length
|
|||
// The command will be injected when EOL is reached
|
|||
} |
|||
else if (serial_char == '\\') { // Handle escapes
|
|||
if (MYSERIAL.available() > 0) { |
|||
// if we have one more character, copy it over
|
|||
serial_char = MYSERIAL.read(); |
|||
if (!serial_comment_mode) serial_line_buffer[serial_count++] = serial_char; |
|||
} |
|||
// otherwise do nothing
|
|||
} |
|||
else { // it's not a newline, carriage return or escape char
|
|||
if (serial_char == ';') serial_comment_mode = true; |
|||
if (!serial_comment_mode) serial_line_buffer[serial_count++] = serial_char; |
|||
} |
|||
|
|||
} // queue has space, serial has data
|
|||
} |
|||
|
|||
#if ENABLED(SDSUPPORT) |
|||
|
|||
/**
|
|||
* Get commands from the SD Card until the command buffer is full |
|||
* or until the end of the file is reached. The special character '#' |
|||
* can also interrupt buffering. |
|||
*/ |
|||
inline void get_sdcard_commands() { |
|||
static bool stop_buffering = false, |
|||
sd_comment_mode = false; |
|||
|
|||
if (!IS_SD_PRINTING) return; |
|||
|
|||
/**
|
|||
* '#' stops reading from SD to the buffer prematurely, so procedural |
|||
* macro calls are possible. If it occurs, stop_buffering is triggered |
|||
* and the buffer is run dry; this character _can_ occur in serial com |
|||
* due to checksums, however, no checksums are used in SD printing. |
|||
*/ |
|||
|
|||
if (commands_in_queue == 0) stop_buffering = false; |
|||
|
|||
uint16_t sd_count = 0; |
|||
bool card_eof = card.eof(); |
|||
while (commands_in_queue < BUFSIZE && !card_eof && !stop_buffering) { |
|||
const int16_t n = card.get(); |
|||
char sd_char = (char)n; |
|||
card_eof = card.eof(); |
|||
if (card_eof || n == -1 |
|||
|| sd_char == '\n' || sd_char == '\r' |
|||
|| ((sd_char == '#' || sd_char == ':') && !sd_comment_mode) |
|||
) { |
|||
if (card_eof) { |
|||
SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED); |
|||
card.printingHasFinished(); |
|||
#if ENABLED(PRINTER_EVENT_LEDS) |
|||
LCD_MESSAGEPGM(MSG_INFO_COMPLETED_PRINTS); |
|||
set_led_color(0, 255, 0); // Green
|
|||
#if HAS_RESUME_CONTINUE |
|||
enqueue_and_echo_commands_P(PSTR("M0")); // end of the queue!
|
|||
#else |
|||
safe_delay(1000); |
|||
#endif |
|||
set_led_color(0, 0, 0); // OFF
|
|||
#endif |
|||
card.checkautostart(true); |
|||
} |
|||
else if (n == -1) { |
|||
SERIAL_ERROR_START(); |
|||
SERIAL_ECHOLNPGM(MSG_SD_ERR_READ); |
|||
} |
|||
if (sd_char == '#') stop_buffering = true; |
|||
|
|||
sd_comment_mode = false; // for new command
|
|||
|
|||
if (!sd_count) continue; // skip empty lines (and comment lines)
|
|||
|
|||
command_queue[cmd_queue_index_w][sd_count] = '\0'; // terminate string
|
|||
sd_count = 0; // clear sd line buffer
|
|||
|
|||
_commit_command(false); |
|||
} |
|||
else if (sd_count >= MAX_CMD_SIZE - 1) { |
|||
/**
|
|||
* Keep fetching, but ignore normal characters beyond the max length |
|||
* The command will be injected when EOL is reached |
|||
*/ |
|||
} |
|||
else { |
|||
if (sd_char == ';') sd_comment_mode = true; |
|||
if (!sd_comment_mode) command_queue[cmd_queue_index_w][sd_count++] = sd_char; |
|||
} |
|||
} |
|||
} |
|||
|
|||
#endif // SDSUPPORT
|
|||
|
|||
/**
|
|||
* Add to the circular command queue the next command from: |
|||
* - The command-injection queue (injected_commands_P) |
|||
* - The active serial input (usually USB) |
|||
* - The SD card file being actively printed |
|||
*/ |
|||
void get_available_commands() { |
|||
|
|||
// if any immediate commands remain, don't get other commands yet
|
|||
if (drain_injected_commands_P()) return; |
|||
|
|||
get_serial_commands(); |
|||
|
|||
#if ENABLED(SDSUPPORT) |
|||
get_sdcard_commands(); |
|||
#endif |
|||
} |
|||
|
|||
/**
|
|||
* Get the next command in the queue, optionally log it to SD, then dispatch it |
|||
*/ |
|||
void advance_command_queue() { |
|||
|
|||
if (!commands_in_queue) return; |
|||
|
|||
#if ENABLED(SDSUPPORT) |
|||
|
|||
if (card.saving) { |
|||
char* command = command_queue[cmd_queue_index_r]; |
|||
if (strstr_P(command, PSTR("M29"))) { |
|||
// M29 closes the file
|
|||
card.closefile(); |
|||
SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED); |
|||
ok_to_send(); |
|||
} |
|||
else { |
|||
// Write the string from the read buffer to SD
|
|||
card.write_command(command); |
|||
if (card.logging) |
|||
gcode.process_next_command(); // The card is saving because it's logging
|
|||
else |
|||
ok_to_send(); |
|||
} |
|||
} |
|||
else |
|||
gcode.process_next_command(); |
|||
|
|||
#else |
|||
|
|||
gcode.process_next_command(); |
|||
|
|||
#endif // SDSUPPORT
|
|||
|
|||
// The queue may be reset by a command handler or by code invoked by idle() within a handler
|
|||
if (commands_in_queue) { |
|||
--commands_in_queue; |
|||
if (++cmd_queue_index_r >= BUFSIZE) cmd_queue_index_r = 0; |
|||
} |
|||
|
|||
} |
|||
@ -0,0 +1,106 @@ |
|||
/**
|
|||
* Marlin 3D Printer Firmware |
|||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|||
* |
|||
* Based on Sprinter and grbl. |
|||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm |
|||
* |
|||
* This program 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. |
|||
* |
|||
* This program 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. |
|||
* |
|||
* You should have received a copy of the GNU General Public License |
|||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|||
* |
|||
*/ |
|||
|
|||
/**
|
|||
* queue.h - The G-code command queue, which holds commands before they |
|||
* go to the parser and dispatcher. |
|||
*/ |
|||
|
|||
#ifndef GCODE_QUEUE_H |
|||
#define GCODE_QUEUE_H |
|||
|
|||
#include "../inc/MarlinConfig.h" |
|||
|
|||
/**
|
|||
* GCode line number handling. Hosts may include line numbers when sending |
|||
* commands to Marlin, and lines will be checked for sequentiality. |
|||
* M110 N<int> sets the current line number. |
|||
*/ |
|||
extern long gcode_LastN, Stopped_gcode_LastN; |
|||
|
|||
/**
|
|||
* GCode Command Queue |
|||
* A simple ring buffer of BUFSIZE command strings. |
|||
* |
|||
* Commands are copied into this buffer by the command injectors |
|||
* (immediate, serial, sd card) and they are processed sequentially by |
|||
* the main loop. The gcode.process_next_command method parses the next |
|||
* command and hands off execution to individual handler functions. |
|||
*/ |
|||
extern uint8_t commands_in_queue, // Count of commands in the queue
|
|||
cmd_queue_index_r; // Ring buffer read position
|
|||
|
|||
extern char command_queue[BUFSIZE][MAX_CMD_SIZE]; |
|||
|
|||
/**
|
|||
* Initialization of queue for setup() |
|||
*/ |
|||
void queue_setup(); |
|||
|
|||
/**
|
|||
* Clear the Marlin command queue |
|||
*/ |
|||
void clear_command_queue(); |
|||
|
|||
/**
|
|||
* Clear the serial line and request a resend of |
|||
* the next expected line number. |
|||
*/ |
|||
void flush_and_request_resend(); |
|||
|
|||
/**
|
|||
* Send an "ok" message to the host, indicating |
|||
* that a command was successfully processed. |
|||
* |
|||
* If ADVANCED_OK is enabled also include: |
|||
* N<int> Line number of the command, if any |
|||
* P<int> Planner space remaining |
|||
* B<int> Block queue space remaining |
|||
*/ |
|||
void ok_to_send(); |
|||
|
|||
/**
|
|||
* Record one or many commands to run from program memory. |
|||
* Aborts the current queue, if any. |
|||
* Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards |
|||
*/ |
|||
void enqueue_and_echo_commands_P(const char * const pgcode); |
|||
|
|||
/**
|
|||
* Enqueue with Serial Echo |
|||
*/ |
|||
bool enqueue_and_echo_command(const char* cmd, bool say_ok=false); |
|||
|
|||
/**
|
|||
* Add to the circular command queue the next command from: |
|||
* - The command-injection queue (injected_commands_P) |
|||
* - The active serial input (usually USB) |
|||
* - The SD card file being actively printed |
|||
*/ |
|||
void get_available_commands(); |
|||
|
|||
/**
|
|||
* Get the next command in the queue, optionally log it to SD, then dispatch it |
|||
*/ |
|||
void advance_command_queue(); |
|||
|
|||
#endif // GCODE_QUEUE_H
|
|||
@ -0,0 +1,574 @@ |
|||
/**
|
|||
* Marlin 3D Printer Firmware |
|||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|||
* |
|||
* Based on Sprinter and grbl. |
|||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm |
|||
* |
|||
* This program 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. |
|||
* |
|||
* This program 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. |
|||
* |
|||
* You should have received a copy of the GNU General Public License |
|||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|||
* |
|||
*/ |
|||
|
|||
/**
|
|||
* motion.cpp |
|||
*/ |
|||
|
|||
#include "motion.h" |
|||
|
|||
#include "../gcode/gcode.h" |
|||
// #include "../module/planner.h"
|
|||
// #include "../Marlin.h"
|
|||
// #include "../inc/MarlinConfig.h"
|
|||
|
|||
#include "../core/serial.h" |
|||
#include "../module/stepper.h" |
|||
#include "../module/temperature.h" |
|||
|
|||
#if IS_SCARA |
|||
#include "../libs/buzzer.h" |
|||
#include "../lcd/ultralcd.h" |
|||
#endif |
|||
|
|||
#if ENABLED(AUTO_BED_LEVELING_UBL) |
|||
#include "../feature/ubl/ubl.h" |
|||
#endif |
|||
|
|||
#define XYZ_CONSTS(type, array, CONFIG) const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG } |
|||
|
|||
XYZ_CONSTS(float, base_min_pos, MIN_POS); |
|||
XYZ_CONSTS(float, base_max_pos, MAX_POS); |
|||
XYZ_CONSTS(float, base_home_pos, HOME_POS); |
|||
XYZ_CONSTS(float, max_length, MAX_LENGTH); |
|||
XYZ_CONSTS(float, home_bump_mm, HOME_BUMP_MM); |
|||
XYZ_CONSTS(signed char, home_dir, HOME_DIR); |
|||
|
|||
// Relative Mode. Enable with G91, disable with G90.
|
|||
bool relative_mode = false; |
|||
|
|||
/**
|
|||
* Cartesian Current Position |
|||
* Used to track the logical position as moves are queued. |
|||
* Used by 'line_to_current_position' to do a move after changing it. |
|||
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'. |
|||
*/ |
|||
float current_position[XYZE] = { 0.0 }; |
|||
|
|||
/**
|
|||
* Cartesian Destination |
|||
* A temporary position, usually applied to 'current_position'. |
|||
* Set with 'get_destination_from_command' or 'set_destination_to_current'. |
|||
* 'line_to_destination' sets 'current_position' to 'destination'. |
|||
*/ |
|||
float destination[XYZE] = { 0.0 }; |
|||
|
|||
// The active extruder (tool). Set with T<extruder> command.
|
|||
uint8_t active_extruder = 0; |
|||
|
|||
// The feedrate for the current move, often used as the default if
|
|||
// no other feedrate is specified. Overridden for special moves.
|
|||
// Set by the last G0 through G5 command's "F" parameter.
|
|||
// Functions that override this for custom moves *must always* restore it!
|
|||
float feedrate_mm_s = MMM_TO_MMS(1500.0); |
|||
|
|||
/**
|
|||
* sync_plan_position |
|||
* |
|||
* Set the planner/stepper positions directly from current_position with |
|||
* no kinematic translation. Used for homing axes and cartesian/core syncing. |
|||
*/ |
|||
void sync_plan_position() { |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position); |
|||
#endif |
|||
planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); |
|||
} |
|||
|
|||
void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } |
|||
|
|||
/**
|
|||
* Move the planner to the current position from wherever it last moved |
|||
* (or from wherever it has been told it is located). |
|||
*/ |
|||
void line_to_current_position() { |
|||
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder); |
|||
} |
|||
|
|||
/**
|
|||
* Move the planner to the position stored in the destination array, which is |
|||
* used by G0/G1/G2/G3/G5 and many other functions to set a destination. |
|||
*/ |
|||
void line_to_destination(const float fr_mm_s) { |
|||
planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder); |
|||
} |
|||
|
|||
#if IS_KINEMATIC |
|||
|
|||
void sync_plan_position_kinematic() { |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position); |
|||
#endif |
|||
planner.set_position_mm_kinematic(current_position); |
|||
} |
|||
|
|||
/**
|
|||
* Calculate delta, start a line, and set current_position to destination |
|||
*/ |
|||
void prepare_uninterpolated_move_to_destination(const float fr_mm_s/*=0.0*/) { |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination); |
|||
#endif |
|||
|
|||
gcode.refresh_cmd_timeout(); |
|||
|
|||
#if UBL_DELTA |
|||
// ubl segmented line will do z-only moves in single segment
|
|||
ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s)); |
|||
#else |
|||
if ( current_position[X_AXIS] == destination[X_AXIS] |
|||
&& current_position[Y_AXIS] == destination[Y_AXIS] |
|||
&& current_position[Z_AXIS] == destination[Z_AXIS] |
|||
&& current_position[E_AXIS] == destination[E_AXIS] |
|||
) return; |
|||
|
|||
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder); |
|||
#endif |
|||
|
|||
set_current_to_destination(); |
|||
} |
|||
|
|||
#endif // IS_KINEMATIC
|
|||
|
|||
// Software Endstops are based on the configured limits.
|
|||
float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, |
|||
soft_endstop_max[XYZ] = { X_MAX_BED, Y_MAX_BED, Z_MAX_POS }; |
|||
|
|||
#if HAS_SOFTWARE_ENDSTOPS |
|||
|
|||
// Software Endstops are based on the configured limits.
|
|||
bool soft_endstops_enabled = true; |
|||
|
|||
/**
|
|||
* Constrain the given coordinates to the software endstops. |
|||
*/ |
|||
|
|||
// NOTE: This makes no sense for delta beds other than Z-axis.
|
|||
// For delta the X/Y would need to be clamped at
|
|||
// DELTA_PRINTABLE_RADIUS from center of bed, but delta
|
|||
// now enforces is_position_reachable for X/Y regardless
|
|||
// of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
|
|||
// redundant here.
|
|||
|
|||
void clamp_to_software_endstops(float target[XYZ]) { |
|||
if (!soft_endstops_enabled) return; |
|||
#if ENABLED(MIN_SOFTWARE_ENDSTOPS) |
|||
#if DISABLED(DELTA) |
|||
NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]); |
|||
NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]); |
|||
#endif |
|||
NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]); |
|||
#endif |
|||
#if ENABLED(MAX_SOFTWARE_ENDSTOPS) |
|||
#if DISABLED(DELTA) |
|||
NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]); |
|||
NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]); |
|||
#endif |
|||
NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]); |
|||
#endif |
|||
} |
|||
|
|||
#endif |
|||
|
|||
#if ENABLED(AUTO_BED_LEVELING_BILINEAR) && !IS_KINEMATIC |
|||
|
|||
#define CELL_INDEX(A,V) ((RAW_##A##_POSITION(V) - bilinear_start[A##_AXIS]) * ABL_BG_FACTOR(A##_AXIS)) |
|||
|
|||
/**
|
|||
* Prepare a bilinear-leveled linear move on Cartesian, |
|||
* splitting the move where it crosses grid borders. |
|||
*/ |
|||
void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF); |
|||
int cx1 = CELL_INDEX(X, current_position[X_AXIS]), |
|||
cy1 = CELL_INDEX(Y, current_position[Y_AXIS]), |
|||
cx2 = CELL_INDEX(X, destination[X_AXIS]), |
|||
cy2 = CELL_INDEX(Y, destination[Y_AXIS]); |
|||
cx1 = constrain(cx1, 0, ABL_BG_POINTS_X - 2); |
|||
cy1 = constrain(cy1, 0, ABL_BG_POINTS_Y - 2); |
|||
cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2); |
|||
cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2); |
|||
|
|||
if (cx1 == cx2 && cy1 == cy2) { |
|||
// Start and end on same mesh square
|
|||
line_to_destination(fr_mm_s); |
|||
set_current_to_destination(); |
|||
return; |
|||
} |
|||
|
|||
#define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) |
|||
|
|||
float normalized_dist, end[XYZE]; |
|||
|
|||
// Split at the left/front border of the right/top square
|
|||
const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); |
|||
if (cx2 != cx1 && TEST(x_splits, gcx)) { |
|||
COPY(end, destination); |
|||
destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx); |
|||
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); |
|||
destination[Y_AXIS] = LINE_SEGMENT_END(Y); |
|||
CBI(x_splits, gcx); |
|||
} |
|||
else if (cy2 != cy1 && TEST(y_splits, gcy)) { |
|||
COPY(end, destination); |
|||
destination[Y_AXIS] = LOGICAL_Y_POSITION(bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy); |
|||
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); |
|||
destination[X_AXIS] = LINE_SEGMENT_END(X); |
|||
CBI(y_splits, gcy); |
|||
} |
|||
else { |
|||
// Already split on a border
|
|||
line_to_destination(fr_mm_s); |
|||
set_current_to_destination(); |
|||
return; |
|||
} |
|||
|
|||
destination[Z_AXIS] = LINE_SEGMENT_END(Z); |
|||
destination[E_AXIS] = LINE_SEGMENT_END(E); |
|||
|
|||
// Do the split and look for more borders
|
|||
bilinear_line_to_destination(fr_mm_s, x_splits, y_splits); |
|||
|
|||
// Restore destination from stack
|
|||
COPY(destination, end); |
|||
bilinear_line_to_destination(fr_mm_s, x_splits, y_splits); |
|||
} |
|||
|
|||
#endif // AUTO_BED_LEVELING_BILINEAR
|
|||
|
|||
#if IS_KINEMATIC && !UBL_DELTA |
|||
|
|||
/**
|
|||
* Prepare a linear move in a DELTA or SCARA setup. |
|||
* |
|||
* This calls planner.buffer_line several times, adding |
|||
* small incremental moves for DELTA or SCARA. |
|||
*/ |
|||
inline bool prepare_kinematic_move_to(float ltarget[XYZE]) { |
|||
|
|||
// Get the top feedrate of the move in the XY plane
|
|||
const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s); |
|||
|
|||
// If the move is only in Z/E don't split up the move
|
|||
if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) { |
|||
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder); |
|||
return false; |
|||
} |
|||
|
|||
// Fail if attempting move outside printable radius
|
|||
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true; |
|||
|
|||
// Get the cartesian distances moved in XYZE
|
|||
const float difference[XYZE] = { |
|||
ltarget[X_AXIS] - current_position[X_AXIS], |
|||
ltarget[Y_AXIS] - current_position[Y_AXIS], |
|||
ltarget[Z_AXIS] - current_position[Z_AXIS], |
|||
ltarget[E_AXIS] - current_position[E_AXIS] |
|||
}; |
|||
|
|||
// Get the linear distance in XYZ
|
|||
float cartesian_mm = SQRT(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS])); |
|||
|
|||
// If the move is very short, check the E move distance
|
|||
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(difference[E_AXIS]); |
|||
|
|||
// No E move either? Game over.
|
|||
if (UNEAR_ZERO(cartesian_mm)) return true; |
|||
|
|||
// Minimum number of seconds to move the given distance
|
|||
const float seconds = cartesian_mm / _feedrate_mm_s; |
|||
|
|||
// The number of segments-per-second times the duration
|
|||
// gives the number of segments
|
|||
uint16_t segments = delta_segments_per_second * seconds; |
|||
|
|||
// For SCARA minimum segment size is 0.25mm
|
|||
#if IS_SCARA |
|||
NOMORE(segments, cartesian_mm * 4); |
|||
#endif |
|||
|
|||
// At least one segment is required
|
|||
NOLESS(segments, 1); |
|||
|
|||
// The approximate length of each segment
|
|||
const float inv_segments = 1.0 / float(segments), |
|||
segment_distance[XYZE] = { |
|||
difference[X_AXIS] * inv_segments, |
|||
difference[Y_AXIS] * inv_segments, |
|||
difference[Z_AXIS] * inv_segments, |
|||
difference[E_AXIS] * inv_segments |
|||
}; |
|||
|
|||
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
|
|||
// SERIAL_ECHOPAIR(" seconds=", seconds);
|
|||
// SERIAL_ECHOLNPAIR(" segments=", segments);
|
|||
|
|||
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING) |
|||
// SCARA needs to scale the feed rate from mm/s to degrees/s
|
|||
const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
|
|||
feed_factor = inv_segment_length * _feedrate_mm_s; |
|||
float oldA = stepper.get_axis_position_degrees(A_AXIS), |
|||
oldB = stepper.get_axis_position_degrees(B_AXIS); |
|||
#endif |
|||
|
|||
// Get the logical current position as starting point
|
|||
float logical[XYZE]; |
|||
COPY(logical, current_position); |
|||
|
|||
// Drop one segment so the last move is to the exact target.
|
|||
// If there's only 1 segment, loops will be skipped entirely.
|
|||
--segments; |
|||
|
|||
// Calculate and execute the segments
|
|||
for (uint16_t s = segments + 1; --s;) { |
|||
LOOP_XYZE(i) logical[i] += segment_distance[i]; |
|||
#if ENABLED(DELTA) |
|||
DELTA_LOGICAL_IK(); // Delta can inline its kinematics
|
|||
#else |
|||
inverse_kinematics(logical); |
|||
#endif |
|||
|
|||
ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
|||
|
|||
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING) |
|||
// For SCARA scale the feed rate from mm/s to degrees/s
|
|||
// Use ratio between the length of the move and the larger angle change
|
|||
const float adiff = abs(delta[A_AXIS] - oldA), |
|||
bdiff = abs(delta[B_AXIS] - oldB); |
|||
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder); |
|||
oldA = delta[A_AXIS]; |
|||
oldB = delta[B_AXIS]; |
|||
#else |
|||
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder); |
|||
#endif |
|||
} |
|||
|
|||
// Since segment_distance is only approximate,
|
|||
// the final move must be to the exact destination.
|
|||
|
|||
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING) |
|||
// For SCARA scale the feed rate from mm/s to degrees/s
|
|||
// With segments > 1 length is 1 segment, otherwise total length
|
|||
inverse_kinematics(ltarget); |
|||
ADJUST_DELTA(ltarget); |
|||
const float adiff = abs(delta[A_AXIS] - oldA), |
|||
bdiff = abs(delta[B_AXIS] - oldB); |
|||
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder); |
|||
#else |
|||
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder); |
|||
#endif |
|||
|
|||
return false; |
|||
} |
|||
|
|||
#else // !IS_KINEMATIC || UBL_DELTA
|
|||
|
|||
/**
|
|||
* Prepare a linear move in a Cartesian setup. |
|||
* If Mesh Bed Leveling is enabled, perform a mesh move. |
|||
* |
|||
* Returns true if the caller didn't update current_position. |
|||
*/ |
|||
inline bool prepare_move_to_destination_cartesian() { |
|||
#if ENABLED(AUTO_BED_LEVELING_UBL) |
|||
const float fr_scaled = MMS_SCALED(feedrate_mm_s); |
|||
if (ubl.state.active) { // direct use of ubl.state.active for speed
|
|||
ubl.line_to_destination_cartesian(fr_scaled, active_extruder); |
|||
return true; |
|||
} |
|||
else |
|||
line_to_destination(fr_scaled); |
|||
#else |
|||
// Do not use feedrate_percentage for E or Z only moves
|
|||
if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) |
|||
line_to_destination(); |
|||
else { |
|||
const float fr_scaled = MMS_SCALED(feedrate_mm_s); |
|||
#if ENABLED(MESH_BED_LEVELING) |
|||
if (mbl.active()) { // direct used of mbl.active() for speed
|
|||
mesh_line_to_destination(fr_scaled); |
|||
return true; |
|||
} |
|||
else |
|||
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) |
|||
if (planner.abl_enabled) { // direct use of abl_enabled for speed
|
|||
bilinear_line_to_destination(fr_scaled); |
|||
return true; |
|||
} |
|||
else |
|||
#endif |
|||
line_to_destination(fr_scaled); |
|||
} |
|||
#endif |
|||
return false; |
|||
} |
|||
|
|||
#endif // !IS_KINEMATIC || UBL_DELTA
|
|||
|
|||
#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE) |
|||
bool extruder_duplication_enabled = false; // Used in Dual X mode 2
|
|||
#endif |
|||
|
|||
#if ENABLED(DUAL_X_CARRIAGE) |
|||
|
|||
DualXMode dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; |
|||
float inactive_extruder_x_pos = X2_MAX_POS, // used in mode 0 & 1
|
|||
raised_parked_position[XYZE], // used in mode 1
|
|||
duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
|
|||
bool active_extruder_parked = false; // used in mode 1 & 2
|
|||
millis_t delayed_move_time = 0; // used in mode 1
|
|||
int16_t duplicate_extruder_temp_offset = 0; // used in mode 2
|
|||
|
|||
float x_home_pos(const int extruder) { |
|||
if (extruder == 0) |
|||
return LOGICAL_X_POSITION(base_home_pos(X_AXIS)); |
|||
else |
|||
/**
|
|||
* In dual carriage mode the extruder offset provides an override of the |
|||
* second X-carriage position when homed - otherwise X2_HOME_POS is used. |
|||
* This allows soft recalibration of the second extruder home position |
|||
* without firmware reflash (through the M218 command). |
|||
*/ |
|||
return LOGICAL_X_POSITION(hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS); |
|||
} |
|||
|
|||
/**
|
|||
* Prepare a linear move in a dual X axis setup |
|||
*/ |
|||
inline bool prepare_move_to_destination_dualx() { |
|||
if (active_extruder_parked) { |
|||
switch (dual_x_carriage_mode) { |
|||
case DXC_FULL_CONTROL_MODE: |
|||
break; |
|||
case DXC_AUTO_PARK_MODE: |
|||
if (current_position[E_AXIS] == destination[E_AXIS]) { |
|||
// This is a travel move (with no extrusion)
|
|||
// Skip it, but keep track of the current position
|
|||
// (so it can be used as the start of the next non-travel move)
|
|||
if (delayed_move_time != 0xFFFFFFFFUL) { |
|||
set_current_to_destination(); |
|||
NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]); |
|||
delayed_move_time = millis(); |
|||
return true; |
|||
} |
|||
} |
|||
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
|
|||
for (uint8_t i = 0; i < 3; i++) |
|||
planner.buffer_line( |
|||
i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS], |
|||
i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS], |
|||
i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS], |
|||
current_position[E_AXIS], |
|||
i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS], |
|||
active_extruder |
|||
); |
|||
delayed_move_time = 0; |
|||
active_extruder_parked = false; |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Clear active_extruder_parked"); |
|||
#endif |
|||
break; |
|||
case DXC_DUPLICATION_MODE: |
|||
if (active_extruder == 0) { |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) { |
|||
SERIAL_ECHOPAIR("Set planner X", LOGICAL_X_POSITION(inactive_extruder_x_pos)); |
|||
SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset); |
|||
} |
|||
#endif |
|||
// move duplicate extruder into correct duplication position.
|
|||
planner.set_position_mm( |
|||
LOGICAL_X_POSITION(inactive_extruder_x_pos), |
|||
current_position[Y_AXIS], |
|||
current_position[Z_AXIS], |
|||
current_position[E_AXIS] |
|||
); |
|||
planner.buffer_line( |
|||
current_position[X_AXIS] + duplicate_extruder_x_offset, |
|||
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], |
|||
planner.max_feedrate_mm_s[X_AXIS], 1 |
|||
); |
|||
SYNC_PLAN_POSITION_KINEMATIC(); |
|||
stepper.synchronize(); |
|||
extruder_duplication_enabled = true; |
|||
active_extruder_parked = false; |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Set extruder_duplication_enabled\nClear active_extruder_parked"); |
|||
#endif |
|||
} |
|||
else { |
|||
#if ENABLED(DEBUG_LEVELING_FEATURE) |
|||
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Active extruder not 0"); |
|||
#endif |
|||
} |
|||
break; |
|||
} |
|||
} |
|||
return false; |
|||
} |
|||
|
|||
#endif // DUAL_X_CARRIAGE
|
|||
|
|||
/**
|
|||
* Prepare a single move and get ready for the next one |
|||
* |
|||
* This may result in several calls to planner.buffer_line to |
|||
* do smaller moves for DELTA, SCARA, mesh moves, etc. |
|||
*/ |
|||
void prepare_move_to_destination() { |
|||
clamp_to_software_endstops(destination); |
|||
gcode.refresh_cmd_timeout(); |
|||
|
|||
#if ENABLED(PREVENT_COLD_EXTRUSION) |
|||
|
|||
if (!DEBUGGING(DRYRUN)) { |
|||
if (destination[E_AXIS] != current_position[E_AXIS]) { |
|||
if (thermalManager.tooColdToExtrude(active_extruder)) { |
|||
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
|
|||
SERIAL_ECHO_START(); |
|||
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); |
|||
} |
|||
#if ENABLED(PREVENT_LENGTHY_EXTRUDE) |
|||
if (destination[E_AXIS] - current_position[E_AXIS] > EXTRUDE_MAXLENGTH) { |
|||
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
|
|||
SERIAL_ECHO_START(); |
|||
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); |
|||
} |
|||
#endif |
|||
} |
|||
} |
|||
|
|||
#endif |
|||
|
|||
if ( |
|||
#if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
|
|||
ubl.prepare_segmented_line_to(destination, feedrate_mm_s) |
|||
#elif IS_KINEMATIC |
|||
prepare_kinematic_move_to(destination) |
|||
#elif ENABLED(DUAL_X_CARRIAGE) |
|||
prepare_move_to_destination_dualx() || prepare_move_to_destination_cartesian() |
|||
#else |
|||
prepare_move_to_destination_cartesian() |
|||
#endif |
|||
) return; |
|||
|
|||
set_current_to_destination(); |
|||
} |
|||
@ -0,0 +1,237 @@ |
|||
/**
|
|||
* Marlin 3D Printer Firmware |
|||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|||
* |
|||
* Based on Sprinter and grbl. |
|||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm |
|||
* |
|||
* This program 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. |
|||
* |
|||
* This program 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. |
|||
* |
|||
* You should have received a copy of the GNU General Public License |
|||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|||
* |
|||
*/ |
|||
|
|||
/**
|
|||
* motion.h |
|||
* |
|||
* High-level motion commands to feed the planner |
|||
* Some of these methods may migrate to the planner class. |
|||
*/ |
|||
|
|||
#ifndef MOTION_H |
|||
#define MOTION_H |
|||
|
|||
#include "../inc/MarlinConfig.h" |
|||
|
|||
//#include "../HAL/HAL.h"
|
|||
|
|||
// #if ENABLED(DELTA)
|
|||
// #include "../module/delta.h"
|
|||
// #endif
|
|||
|
|||
extern bool relative_mode; |
|||
|
|||
extern float current_position[XYZE], destination[XYZE]; |
|||
|
|||
extern float feedrate_mm_s; |
|||
|
|||
extern uint8_t active_extruder; |
|||
|
|||
extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ]; |
|||
|
|||
FORCE_INLINE float pgm_read_any(const float *p) { return pgm_read_float_near(p); } |
|||
FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_byte_near(p); } |
|||
|
|||
#define XYZ_DEFS(type, array, CONFIG) \ |
|||
extern const type array##_P[XYZ]; \ |
|||
FORCE_INLINE type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \ |
|||
typedef void __void_##CONFIG##__ |
|||
|
|||
XYZ_DEFS(float, base_min_pos, MIN_POS); |
|||
XYZ_DEFS(float, base_max_pos, MAX_POS); |
|||
XYZ_DEFS(float, base_home_pos, HOME_POS); |
|||
XYZ_DEFS(float, max_length, MAX_LENGTH); |
|||
XYZ_DEFS(float, home_bump_mm, HOME_BUMP_MM); |
|||
XYZ_DEFS(signed char, home_dir, HOME_DIR); |
|||
|
|||
#if HAS_SOFTWARE_ENDSTOPS |
|||
extern bool soft_endstops_enabled; |
|||
void clamp_to_software_endstops(float target[XYZ]); |
|||
#else |
|||
#define soft_endstops_enabled false |
|||
#define clamp_to_software_endstops(x) NOOP |
|||
#endif |
|||
|
|||
inline void set_current_to_destination() { COPY(current_position, destination); } |
|||
inline void set_destination_to_current() { COPY(destination, current_position); } |
|||
|
|||
/**
|
|||
* sync_plan_position |
|||
* |
|||
* Set the planner/stepper positions directly from current_position with |
|||
* no kinematic translation. Used for homing axes and cartesian/core syncing. |
|||
*/ |
|||
void sync_plan_position(); |
|||
void sync_plan_position_e(); |
|||
|
|||
#if IS_KINEMATIC |
|||
void sync_plan_position_kinematic(); |
|||
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic() |
|||
#else |
|||
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position() |
|||
#endif |
|||
|
|||
/**
|
|||
* Move the planner to the current position from wherever it last moved |
|||
* (or from wherever it has been told it is located). |
|||
*/ |
|||
void line_to_current_position(); |
|||
|
|||
/**
|
|||
* Move the planner to the position stored in the destination array, which is |
|||
* used by G0/G1/G2/G3/G5 and many other functions to set a destination. |
|||
*/ |
|||
void line_to_destination(const float fr_mm_s); |
|||
|
|||
inline void line_to_destination() { line_to_destination(feedrate_mm_s); } |
|||
|
|||
#if IS_KINEMATIC |
|||
void prepare_uninterpolated_move_to_destination(const float fr_mm_s=0.0); |
|||
#endif |
|||
|
|||
void prepare_move_to_destination(); |
|||
|
|||
void clamp_to_software_endstops(float target[XYZ]); |
|||
|
|||
//
|
|||
// Macros
|
|||
//
|
|||
|
|||
// Workspace offsets
|
|||
#if HAS_WORKSPACE_OFFSET |
|||
#if HAS_HOME_OFFSET |
|||
extern float home_offset[XYZ]; |
|||
#endif |
|||
#if HAS_POSITION_SHIFT |
|||
extern float position_shift[XYZ]; |
|||
#endif |
|||
#endif |
|||
|
|||
#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT |
|||
extern float workspace_offset[XYZ]; |
|||
#define WORKSPACE_OFFSET(AXIS) workspace_offset[AXIS] |
|||
#elif HAS_HOME_OFFSET |
|||
#define WORKSPACE_OFFSET(AXIS) home_offset[AXIS] |
|||
#elif HAS_POSITION_SHIFT |
|||
#define WORKSPACE_OFFSET(AXIS) position_shift[AXIS] |
|||
#else |
|||
#define WORKSPACE_OFFSET(AXIS) 0 |
|||
#endif |
|||
|
|||
#define LOGICAL_POSITION(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS)) |
|||
#define RAW_POSITION(POS, AXIS) ((POS) - WORKSPACE_OFFSET(AXIS)) |
|||
|
|||
#if HAS_POSITION_SHIFT || DISABLED(DELTA) |
|||
#define LOGICAL_X_POSITION(POS) LOGICAL_POSITION(POS, X_AXIS) |
|||
#define LOGICAL_Y_POSITION(POS) LOGICAL_POSITION(POS, Y_AXIS) |
|||
#define RAW_X_POSITION(POS) RAW_POSITION(POS, X_AXIS) |
|||
#define RAW_Y_POSITION(POS) RAW_POSITION(POS, Y_AXIS) |
|||
#else |
|||
#define LOGICAL_X_POSITION(POS) (POS) |
|||
#define LOGICAL_Y_POSITION(POS) (POS) |
|||
#define RAW_X_POSITION(POS) (POS) |
|||
#define RAW_Y_POSITION(POS) (POS) |
|||
#endif |
|||
|
|||
#define LOGICAL_Z_POSITION(POS) LOGICAL_POSITION(POS, Z_AXIS) |
|||
#define RAW_Z_POSITION(POS) RAW_POSITION(POS, Z_AXIS) |
|||
#define RAW_CURRENT_POSITION(A) RAW_##A##_POSITION(current_position[A##_AXIS]) |
|||
|
|||
/**
|
|||
* position_is_reachable family of functions |
|||
*/ |
|||
|
|||
#if IS_KINEMATIC // (DELTA or SCARA)
|
|||
|
|||
#if IS_SCARA |
|||
extern const float L1, L2; |
|||
#endif |
|||
|
|||
inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) { |
|||
#if ENABLED(DELTA) |
|||
return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS); |
|||
#elif IS_SCARA |
|||
#if MIDDLE_DEAD_ZONE_R > 0 |
|||
const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y); |
|||
return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2); |
|||
#else |
|||
return HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y) <= sq(L1 + L2); |
|||
#endif |
|||
#else // CARTESIAN
|
|||
// To be migrated from MakerArm branch in future
|
|||
#endif |
|||
} |
|||
|
|||
inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) { |
|||
|
|||
// Both the nozzle and the probe must be able to reach the point.
|
|||
// This won't work on SCARA since the probe offset rotates with the arm.
|
|||
|
|||
return position_is_reachable_raw_xy(rx, ry) |
|||
&& position_is_reachable_raw_xy(rx - X_PROBE_OFFSET_FROM_EXTRUDER, ry - Y_PROBE_OFFSET_FROM_EXTRUDER); |
|||
} |
|||
|
|||
#else // CARTESIAN
|
|||
|
|||
inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) { |
|||
// Add 0.001 margin to deal with float imprecision
|
|||
return WITHIN(rx, X_MIN_POS - 0.001, X_MAX_POS + 0.001) |
|||
&& WITHIN(ry, Y_MIN_POS - 0.001, Y_MAX_POS + 0.001); |
|||
} |
|||
|
|||
inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) { |
|||
// Add 0.001 margin to deal with float imprecision
|
|||
return WITHIN(rx, MIN_PROBE_X - 0.001, MAX_PROBE_X + 0.001) |
|||
&& WITHIN(ry, MIN_PROBE_Y - 0.001, MAX_PROBE_Y + 0.001); |
|||
} |
|||
|
|||
#endif // CARTESIAN
|
|||
|
|||
FORCE_INLINE bool position_is_reachable_by_probe_xy(const float &lx, const float &ly) { |
|||
return position_is_reachable_by_probe_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly)); |
|||
} |
|||
|
|||
FORCE_INLINE bool position_is_reachable_xy(const float &lx, const float &ly) { |
|||
return position_is_reachable_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly)); |
|||
} |
|||
|
|||
#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE) |
|||
extern bool extruder_duplication_enabled; // Used in Dual X mode 2
|
|||
#endif |
|||
|
|||
#if ENABLED(DUAL_X_CARRIAGE) |
|||
|
|||
extern DualXMode dual_x_carriage_mode; |
|||
extern float inactive_extruder_x_pos, // used in mode 0 & 1
|
|||
raised_parked_position[XYZE], // used in mode 1
|
|||
duplicate_extruder_x_offset; // used in mode 2
|
|||
extern bool active_extruder_parked; // used in mode 1 & 2
|
|||
extern millis_t delayed_move_time; // used in mode 1
|
|||
extern int16_t duplicate_extruder_temp_offset; // used in mode 2
|
|||
|
|||
float x_home_pos(const int extruder); |
|||
|
|||
FORCE_INLINE int x_home_dir(const uint8_t extruder) { return extruder ? X2_HOME_DIR : X_HOME_DIR; } |
|||
|
|||
#endif // DUAL_X_CARRIAGE
|
|||
|
|||
#endif // MOTION_H
|
|||
Loading…
Reference in new issue