Add gcode.cpp, motion.*, queue.* - Apply to some G-codes.
Scott Lahteine
7 years ago
parent
4231faf779
commit
722786966a
File diff suppressed because it is too large
Load Diff
@ -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