diff options
76 files changed, 3500 insertions, 1874 deletions
diff --git a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin Binary files differdeleted file mode 100644 index 32671020..00000000 --- a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20alpha7.bin +++ /dev/null diff --git a/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin Binary files differnew file mode 100644 index 00000000..73a3aceb --- /dev/null +++ b/Release/Duet-Ethernet/Edge/DuetEthernetFirmware-1.20beta2.bin diff --git a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin Binary files differdeleted file mode 100644 index c46746be..00000000 --- a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20alpha7.bin +++ /dev/null diff --git a/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin Binary files differnew file mode 100644 index 00000000..b0086459 --- /dev/null +++ b/Release/Duet-WiFi/Edge/DuetWiFiFirmware-1.20beta2.bin diff --git a/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin b/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin Binary files differnew file mode 100644 index 00000000..47bb4ec1 --- /dev/null +++ b/Release/Duet-WiFi/Edge/DuetWiFiServer-1.20beta2.bin diff --git a/src/Alligator/Pins_Alligator.h b/src/Alligator/Pins_Alligator.h index d8eb673b..1167e492 100644 --- a/src/Alligator/Pins_Alligator.h +++ b/src/Alligator/Pins_Alligator.h @@ -7,6 +7,9 @@ #define HAS_LWIP_NETWORKING 1 #define HAS_CPU_TEMP_SENSOR 1 #define HAS_HIGH_SPEED_SD 0 +#define HAS_SMART_DRIVERS 0 +#define HAS_VOLTAGE_MONITOR 0 +#define ACTIVE_LOW_HEAT_ON 0 const size_t NumFirmwareUpdateModules = 1; #define IAP_UPDATE_FILE "iapalligator.bin" @@ -40,7 +43,7 @@ const size_t NUM_SERIAL_CHANNELS = 3; // The number of serial IO channels (USB #define SERIAL_AUX_DEVICE Serial #define SERIAL_AUX2_DEVICE Serial1 -// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to -1 to flag unavailability. +// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to NoPin to flag unavailability. // DRIVES const Pin ENABLE_PINS[DRIVES] = { 24, NoPin, NoPin, NoPin, NoPin, NoPin, NoPin }; const Pin STEP_PINS[DRIVES] = { X16, X14, X1, 5, 28, 11, X9 }; @@ -65,7 +68,6 @@ const size_t MaxSpiDac = 2; const Pin SPI_DAC_CS[MaxSpiDac] = { 53, 6 }; // HEATERS -const bool HEAT_ON = true; // Not inverted heater for Alligator const Pin TEMP_SENSE_PINS[Heaters] = HEATERS_(1, 0, 2, 3, 4, f, g, h); // Analogue pin numbers // h1,h2,h3,h4: X2,8,9,X8 is hardware PWM diff --git a/src/Configuration.h b/src/Configuration.h index 5d641841..5f659f4b 100644 --- a/src/Configuration.h +++ b/src/Configuration.h @@ -131,16 +131,18 @@ constexpr unsigned int DefaultPinWritePwmFreq = 500; // default PWM frequency fo // Using single-precision maths and up to 9-factor calibration: (9 + 5) * 4 bytes per point // Using double-precision maths and up to 9-factor calibration: (9 + 5) * 8 bytes per point // So 32 points using double precision arithmetic need 3584 bytes of stack space. -#ifdef DUET_NG +#if SAM4E || SAM4S constexpr size_t MaxGridProbePoints = 441; // 441 allows us to probe e.g. 400x400 at 20mm intervals constexpr size_t MaxXGridPoints = 41; // Maximum number of grid points in one X row constexpr size_t MaxProbePoints = 32; // Maximum number of G30 probe points constexpr size_t MaxDeltaCalibrationPoints = 32; // Should a power of 2 for speed -#else +#elif SAM3XA constexpr size_t MaxGridProbePoints = 121; // 121 allows us to probe 200x200 at 20mm intervals constexpr size_t MaxXGridPoints = 21; // Maximum number of grid points in one X row constexpr size_t MaxProbePoints = 32; // Maximum number of G30 probe points constexpr size_t MaxDeltaCalibrationPoints = 32; // Should a power of 2 for speed +#else +# error #endif const float DefaultGridSpacing = 20.0; // Default bed probing grid spacing in mm @@ -171,14 +173,16 @@ constexpr size_t FILENAME_LENGTH = 100; constexpr size_t MaxHeaterNameLength = 20; // Maximum number of characters in a heater name // Output buffer lengths -#ifdef DUET_NG +#if SAM4E || SAM4S constexpr uint16_t OUTPUT_BUFFER_SIZE = 256; // How many bytes does each OutputBuffer hold? constexpr size_t OUTPUT_BUFFER_COUNT = 32; // How many OutputBuffer instances do we have? constexpr size_t RESERVED_OUTPUT_BUFFERS = 1; // Number of reserved output buffers after long responses. Must be enough for an HTTP header -#else +#elif SAM3XA constexpr uint16_t OUTPUT_BUFFER_SIZE = 128; // How many bytes does each OutputBuffer hold? constexpr size_t OUTPUT_BUFFER_COUNT = 32; // How many OutputBuffer instances do we have? constexpr size_t RESERVED_OUTPUT_BUFFERS = 2; // Number of reserved output buffers after long responses. Must be enough for an HTTP header +#else +# error #endif // Move system diff --git a/src/Duet/Pins_Duet.h b/src/Duet/Pins_Duet.h index 768f686e..76623c34 100644 --- a/src/Duet/Pins_Duet.h +++ b/src/Duet/Pins_Duet.h @@ -7,6 +7,9 @@ #define HAS_LWIP_NETWORKING 1 #define HAS_CPU_TEMP_SENSOR 1 #define HAS_HIGH_SPEED_SD 1 +#define HAS_SMART_DRIVERS 0 +#define HAS_VOLTAGE_MONITOR 0 +#define ACTIVE_LOW_HEAT_ON 1 const size_t NumFirmwareUpdateModules = 1; #define IAP_UPDATE_FILE "iap.bin" @@ -40,9 +43,9 @@ const size_t NUM_SERIAL_CHANNELS = 3; // The number of serial IO channels (USB #define SERIAL_AUX_DEVICE Serial #define SERIAL_AUX2_DEVICE Serial1 -// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to -1 to flag unavailability. +// The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to NoPin to flag unavailability. -// DRIVES +// Drives const Pin ENABLE_PINS[DRIVES] = { 29, 27, X1, X0, 37, X8, 50, 47, X13 }; const Pin STEP_PINS[DRIVES] = { 14, 25, 5, X2, 41, 39, X4, 49, X10 }; @@ -61,9 +64,6 @@ const float STEPPER_DAC_VOLTAGE_RANGE = 2.02; // Stepper motor current ref const float STEPPER_DAC_VOLTAGE_OFFSET = -0.025; // Stepper motor current offset voltage for E1 if using a DAC // HEATERS - -const bool HEAT_ON = false; // false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4) - const Pin TEMP_SENSE_PINS[Heaters] = { 5, 4, 0, 7, 8, 9, 11 }; // Analogue pin numbers const Pin HEAT_ON_PINS[Heaters] = { 6, X5, X7, 7, 8, 9, X17 }; // Heater Channel 7 (pin X17) is shared with Fan1 diff --git a/src/Duet/Webserver.cpp b/src/Duet/Webserver.cpp index 27392548..bbb431a3 100644 --- a/src/Duet/Webserver.cpp +++ b/src/Duet/Webserver.cpp @@ -831,10 +831,14 @@ void Webserver::HttpInterpreter::SendJsonResponse(const char* command) return; } - if (StringEquals(command, "download") && StringEquals(qualifiers[0].key, "name")) + if (StringEquals(command, "download")) { - SendFile(qualifiers[0].value, false); - return; + const char* const filename = GetKeyValue("name"); + if (filename != nullptr) + { + SendFile(filename, false); + return; + } } } @@ -905,11 +909,12 @@ void Webserver::HttpInterpreter::GetJsonResponse(const char* request, OutputBuff if (Authenticate()) { // See if we can update the current RTC date and time - if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time") && !platform->IsDateTimeSet()) + const char* const timeString = GetKeyValue("time"); + if (timeString != nullptr && !platform->IsDateTimeSet()) { struct tm timeInfo; memset(&timeInfo, 0, sizeof(timeInfo)); - if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) + if (strptime(timeString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) { time_t newTime = mktime(&timeInfo); platform->SetDateTime(newTime); @@ -1527,7 +1532,8 @@ bool Webserver::HttpInterpreter::ProcessMessage() || (commandWords[1][0] == '/' && StringEquals(commandWords[1] + 1, KO_START "upload")); if (isUploadRequest) { - if (numQualKeys > 0 && StringEquals(qualifiers[0].key, "name")) + const char* const filename = GetKeyValue("name"); + if (filename != nullptr) { // We cannot upload more than one file at once if (IsUploading()) @@ -1537,7 +1543,7 @@ bool Webserver::HttpInterpreter::ProcessMessage() // See how many bytes we expect to read bool contentLengthFound = false; - for(size_t i=0; i<numHeaderKeys; i++) + for (size_t i = 0; i < numHeaderKeys; i++) { if (StringEquals(headers[i].key, "Content-Length")) { @@ -1554,18 +1560,19 @@ bool Webserver::HttpInterpreter::ProcessMessage() } // Start a new file upload - FileStore *file = platform->GetFileStore(FS_PREFIX, qualifiers[0].value, OpenMode::write); - if (!StartUpload(file, qualifiers[0].value)) + FileStore *file = platform->GetFileStore(FS_PREFIX, filename, OpenMode::write); + if (!StartUpload(file, filename)) { return RejectMessage("could not start file upload"); } // Try to get the last modified file date and time - if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time")) + const char* const lastModifiedString = GetKeyValue("time"); + if (lastModifiedString != nullptr) { struct tm timeInfo; memset(&timeInfo, 0, sizeof(timeInfo)); - if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) + if (strptime(lastModifiedString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) { fileLastModified = mktime(&timeInfo); } @@ -1581,7 +1588,7 @@ bool Webserver::HttpInterpreter::ProcessMessage() if (reprap.Debug(moduleWebserver)) { - platform->MessageF(UsbMessage, "Start uploading file %s length %lu\n", qualifiers[0].value, postFileLength); + platform->MessageF(UsbMessage, "Start uploading file %s length %lu\n", filename, postFileLength); } uploadedBytes = 0; diff --git a/src/DuetNG/DuetWiFi/Network.cpp b/src/DuetNG/DuetWiFi/Network.cpp index 3bb96387..b72e992d 100644 --- a/src/DuetNG/DuetWiFi/Network.cpp +++ b/src/DuetNG/DuetWiFi/Network.cpp @@ -376,6 +376,7 @@ void Network::Start() // The ESP takes about 300ms before it starts talking to us, so don't wait for it here, do that in Spin() spiTxUnderruns = spiRxOverruns = 0; reconnectCount = 0; + transferAlreadyPendingCount = readyTimeoutCount = responseTimeoutCount = 0; lastTickMillis = millis(); state = NetworkState::starting1; @@ -475,6 +476,7 @@ void Network::Spin(bool full) { if (espStatusChanged && digitalRead(EspTransferRequestPin)) { + Platform::softwareResetDebugInfo = 1; if (reprap.Debug(moduleNetwork)) { debugPrintf("ESP reported status change\n"); @@ -487,6 +489,7 @@ void Network::Spin(bool full) && currentMode != WiFiState::autoReconnecting ) { + Platform::softwareResetDebugInfo = 2; // Tell the wifi module to change mode int32_t rslt = ResponseUnknownError; if (currentMode != WiFiState::idle) @@ -515,6 +518,7 @@ void Network::Spin(bool full) } else if (currentMode == WiFiState::connected || currentMode == WiFiState::runningAsAccessPoint) { + Platform::softwareResetDebugInfo = 3; // Find the next socket to poll const size_t startingSocket = currentSocket; do @@ -549,6 +553,7 @@ void Network::Spin(bool full) { nr = responders; // 'responders' can't be null at this point } + Platform::softwareResetDebugInfo = 4; doneSomething = nr->Spin(); nr = nr->GetNext(); } while (!doneSomething && nr != nextResponderToPoll); @@ -558,6 +563,7 @@ void Network::Spin(bool full) } else if (currentMode == requestedMode && (currentMode == WiFiState::connected || currentMode == WiFiState::runningAsAccessPoint)) { + Platform::softwareResetDebugInfo = 5; sockets[currentSocket].Poll(full); ++currentSocket; if (currentSocket == NumTcpSockets) @@ -608,6 +614,7 @@ void Network::Spin(bool full) break; } + Platform::softwareResetDebugInfo = 0; if (full) { platform.ClassReport(longWait); @@ -651,10 +658,13 @@ void Network::Diagnostics(MessageType mtype) { platform.MessageF(mtype, "Network state is %s\n", TranslateNetworkState()); platform.MessageF(mtype, "WiFi module is %s\n", TranslateWiFiState(currentMode)); + platform.MessageF(mtype, "Failed messages: pending %u, notready %u, noresp %u\n", transferAlreadyPendingCount, readyTimeoutCount, responseTimeoutCount); + #if 0 // The underrun/overrun counters don't work at present platform.MessageF(mtype, "SPI underruns %u, overruns %u\n", spiTxUnderruns, spiRxOverruns); #endif + if (state != NetworkState::disabled && state != NetworkState::starting1 && state != NetworkState::starting2) { Receiver<NetworkStatusResponse> status; @@ -675,7 +685,8 @@ void Network::Diagnostics(MessageType mtype) if (currentMode == WiFiState::connected) { - platform.MessageF(mtype, "WiFi signal strength %ddBm\nReconnections %u\n", (int)r.rssi, reconnectCount); + const char* const sleepMode = (r.sleepMode == 1) ? "none" : (r.sleepMode == 2) ? "light" : (r.sleepMode == 3) ? "modem" : "unknown"; + platform.MessageF(mtype, "WiFi signal strength %ddBm, reconnections %u, sleep mode %s\n", (int)r.rssi, reconnectCount, sleepMode); } else if (currentMode == WiFiState::runningAsAccessPoint) { @@ -1192,6 +1203,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t { debugPrintf("ResponseBusy\n"); } + ++transferAlreadyPendingCount; return ResponseBusy; } @@ -1206,6 +1218,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t { debugPrintf("ResponseBusy\n"); } + ++readyTimeoutCount; return ResponseBusy; } } @@ -1255,6 +1268,7 @@ int32_t Network::SendCommand(NetworkCommand cmd, SocketNumber socketNum, uint8_t } transferPending = false; spi_dma_disable(); + ++responseTimeoutCount; return ResponseTimeout; } } diff --git a/src/DuetNG/DuetWiFi/Network.h b/src/DuetNG/DuetWiFi/Network.h index 4c97f7e8..fa89cb0b 100644 --- a/src/DuetNG/DuetWiFi/Network.h +++ b/src/DuetNG/DuetWiFi/Network.h @@ -158,6 +158,9 @@ private: unsigned int spiTxUnderruns; unsigned int spiRxOverruns; unsigned int reconnectCount; + unsigned int transferAlreadyPendingCount; + unsigned int readyTimeoutCount; + unsigned int responseTimeoutCount; char wiFiServerVersion[16]; }; diff --git a/src/DuetNG/HttpResponder.cpp b/src/DuetNG/HttpResponder.cpp index 484b0d50..cb25d9d8 100644 --- a/src/DuetNG/HttpResponder.cpp +++ b/src/DuetNG/HttpResponder.cpp @@ -463,11 +463,12 @@ bool HttpResponder::GetJsonResponse(const char* request, OutputBuffer *&response } // See if we can update the current RTC date and time - if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time") && !GetPlatform().IsDateTimeSet()) + const char* const timeString = GetKeyValue("time"); + if (timeString != nullptr && !GetPlatform().IsDateTimeSet()) { struct tm timeInfo; memset(&timeInfo, 0, sizeof(timeInfo)); - if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) + if (strptime(timeString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) { time_t newTime = mktime(&timeInfo); GetPlatform().SetDateTime(newTime); @@ -876,10 +877,14 @@ void HttpResponder::SendJsonResponse(const char* command) return; } - if (StringEquals(command, "download") && StringEquals(qualifiers[0].key, "name")) + if (StringEquals(command, "download")) { - SendFile(qualifiers[0].value, false); - return; + const char* const filename = GetKeyValue("name"); + if (filename != nullptr) + { + SendFile(filename, false); + return; + } } } @@ -996,7 +1001,8 @@ void HttpResponder::ProcessMessage() || (commandWords[1][0] == '/' && StringEquals(commandWords[1] + 1, KO_START "upload")); if (isUploadRequest) { - if (numQualKeys > 0 && StringEquals(qualifiers[0].key, "name")) + const char* const filename = GetKeyValue("name"); + if (filename != nullptr) { // See how many bytes we expect to read bool contentLengthFound = false; @@ -1018,21 +1024,22 @@ void HttpResponder::ProcessMessage() } // Start a new file upload - FileStore *file = GetPlatform().GetFileStore(FS_PREFIX, qualifiers[0].value, OpenMode::write); + FileStore *file = GetPlatform().GetFileStore(FS_PREFIX, filename, OpenMode::write); if (file == nullptr) { RejectMessage("could not create file"); return; } - StartUpload(file, qualifiers[0].value); + StartUpload(file, filename); // Try to get the last modified file date and time - if (numQualKeys > 1 && StringEquals(qualifiers[1].key, "time")) + const char* const lastModifiedString = GetKeyValue("time"); + if (lastModifiedString != nullptr) { struct tm timeInfo; memset(&timeInfo, 0, sizeof(timeInfo)); - if (strptime(qualifiers[1].value, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) + if (strptime(lastModifiedString, "%Y-%m-%dT%H:%M:%S", &timeInfo) != nullptr) { fileLastModified = mktime(&timeInfo); } @@ -1048,7 +1055,7 @@ void HttpResponder::ProcessMessage() if (reprap.Debug(moduleWebserver)) { - GetPlatform().MessageF(UsbMessage, "Start uploading file %s length %lu\n", qualifiers[0].value, postFileLength); + GetPlatform().MessageF(UsbMessage, "Start uploading file %s length %lu\n", filename, postFileLength); } uploadedBytes = 0; diff --git a/src/DuetNG/Pins_DuetNG.h b/src/DuetNG/Pins_DuetNG.h index 85b02396..047dfc18 100644 --- a/src/DuetNG/Pins_DuetNG.h +++ b/src/DuetNG/Pins_DuetNG.h @@ -25,6 +25,9 @@ const size_t NumFirmwareUpdateModules = 1; // 1 module #define HAS_LWIP_NETWORKING 0 #define HAS_CPU_TEMP_SENSOR 1 #define HAS_HIGH_SPEED_SD 1 +#define HAS_SMART_DRIVERS 1 +#define HAS_VOLTAGE_MONITOR 1 +#define ACTIVE_LOW_HEAT_ON 1 #define IAP_UPDATE_FILE "iap4e.bin" // using the same IAP file for both Duet WiFi and Duet Ethernet @@ -73,7 +76,6 @@ constexpr Pin DueX_INT = 17; // DueX interrupt pin = PA17 (was E6_STOP) constexpr Pin END_STOP_PINS[DRIVES] = { 46, 02, 93, 74, 48, 200, 203, 202, 201, 213, 39, 8 }; // HEATERS -constexpr bool HEAT_ON = false; // false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4) constexpr Pin TEMP_SENSE_PINS[Heaters] = { 45, 47, 44, 61, 62, 63, 59, 18 }; // Thermistor pin numbers constexpr Pin HEAT_ON_PINS[Heaters] = { 19, 20, 16, 35, 37, 40, 43, 15 }; // Heater pin numbers (heater 7 pin TBC) diff --git a/src/DuetNG/TMC2660.cpp b/src/DuetNG/TMC2660.cpp index fb0c9477..12f2f4fb 100644 --- a/src/DuetNG/TMC2660.cpp +++ b/src/DuetNG/TMC2660.cpp @@ -85,6 +85,7 @@ const uint32_t TMC_DRVCTRL_INTPOL = 1 << 9; const uint32_t TMC_SGCSCONF_CS_MASK = 31; #define TMC_SGCSCONF_CS(n) ((((uint32_t)n) & 31) << 0) const uint32_t TMC_SGCSCONF_SGT_MASK = 127 << 8; +const uint32_t TMC_SGCSCONF_SGT_SHIFT = 8; #define TMC_SGCSCONF_SGT(n) ((((uint32_t)n) & 127) << 8) const uint32_t TMC_SGCSCONF_SGT_SFILT = 1 << 16; @@ -126,6 +127,7 @@ const uint32_t defaultSgscConfReg = const uint32_t defaultDrvConfReg = TMC_REG_DRVCONF | TMC_DRVCONF_VSENSE // use high sensitivity range + | TMC_DRVCONF_TS2G_0P8 // fast short-to-ground detection | 0; // Driver control register @@ -137,7 +139,7 @@ const uint32_t defaultDrvCtrlReg = // coolStep control register const uint32_t defaultSmartEnReg = TMC_REG_SMARTEN - | 0; // disable coolStep, we already do this in the main firmware + | 0; // disable coolStep, it needs to be tuned ot the motor to work properly //---------------------------------------------------------------------------------------------------------------------------------- // Private types and methods @@ -161,6 +163,10 @@ struct TmcDriverState void Enable(bool en); void SpiSendWord(uint32_t dataOut); uint32_t ReadStatus(); + void SetStallThreshold(int sgThreshold); + void SetStallFilter(bool sgFilter); + void SetCoolStep(uint16_t coolStepConfig); + void AppendStallConfig(StringRef& reply); }; // Initialise the state of the driver. @@ -270,11 +276,48 @@ uint32_t TmcDriverState::ReadStatus() return lastReadValue & (TMC_RR_SG | TMC_RR_OT | TMC_RR_OTPW | TMC_RR_S2G | TMC_RR_OLA | TMC_RR_OLB | TMC_RR_STST); } +void TmcDriverState::SetStallThreshold(int sgThreshold) +{ + const uint32_t sgVal = ((uint32_t)constrain<int>(sgThreshold, -64, 63)) & 127; + sgcsConfReg = (sgcsConfReg & ~TMC_SGCSCONF_SGT_MASK) | (sgVal << TMC_SGCSCONF_SGT_SHIFT); + SpiSendWord(sgcsConfReg); +} + +void TmcDriverState::SetStallFilter(bool sgFilter) +{ + if (sgFilter) + { + sgcsConfReg |= TMC_SGCSCONF_SGT_SFILT; + } + else + { + sgcsConfReg &= ~TMC_SGCSCONF_SGT_SFILT; + } + SpiSendWord(sgcsConfReg); +} + +void TmcDriverState::SetCoolStep(uint16_t coolStepConfig) +{ + smartEnReg = TMC_REG_SMARTEN | coolStepConfig; + SpiSendWord(smartEnReg); +} + +void TmcDriverState::AppendStallConfig(StringRef& reply) +{ + const bool filtered = ((sgcsConfReg & TMC_SGCSCONF_SGT_SFILT) != 0); + int threshold = (int)((sgcsConfReg & TMC_SGCSCONF_SGT_MASK) >> TMC_SGCSCONF_SGT_SHIFT); + if (threshold >= 64) + { + threshold -= 128; + } + reply.catf("stall threshold %d, filter %s, coolstep %" PRIx32, threshold, ((filtered) ? "on" : "off"), smartEnReg & 0xFFFF); +} + static TmcDriverState driverStates[DRIVES]; //--------------------------- Public interface --------------------------------- -namespace TMC2660 +namespace SmartDrivers { // Initialise the driver interface and the drivers, leaving each drive disabled. // It is assumed that the drivers are not powered, so driversPowered(true) must be called after calling this before the motors can be moved. @@ -434,6 +477,38 @@ namespace TMC2660 } } + void SetStallThreshold(size_t drive, int sgThreshold) + { + if (drive < numTmc2660Drivers) + { + driverStates[drive].SetStallThreshold(sgThreshold); + } + } + + void SetStallFilter(size_t drive, bool sgFilter) + { + if (drive < numTmc2660Drivers) + { + driverStates[drive].SetStallFilter(sgFilter); + } + } + + void SetCoolStep(size_t drive, uint16_t coolStepConfig) + { + if (drive < numTmc2660Drivers) + { + driverStates[drive].SetCoolStep(coolStepConfig); + } + } + + void AppendStallConfig(size_t drive, StringRef& reply) + { + if (drive < numTmc2660Drivers) + { + driverStates[drive].AppendStallConfig(reply); + } + } + }; // end namespace // End diff --git a/src/DuetNG/TMC2660.h b/src/DuetNG/TMC2660.h index 5284789b..91f4b40c 100644 --- a/src/DuetNG/TMC2660.h +++ b/src/DuetNG/TMC2660.h @@ -9,6 +9,7 @@ #define TMC2660_H_ #include "Pins.h" +#include "Libraries/General/StringRef.h" // TMC2660 Read response. The microstep counter can also be read, but we don't include that here. const uint32_t TMC_RR_SG = 1 << 0; // stall detected @@ -19,7 +20,7 @@ const uint32_t TMC_RR_OLA = 1 << 5; // open load A const uint32_t TMC_RR_OLB = 1 << 6; // open load B const uint32_t TMC_RR_STST = 1 << 7; // standstill detected -namespace TMC2660 +namespace SmartDrivers { void Init(const Pin[DRIVES], size_t numTmcDrivers) pre(numTmcDrivers <= DRIVES); @@ -29,6 +30,10 @@ namespace TMC2660 bool SetMicrostepping(size_t drive, int microsteps, int mode); unsigned int GetMicrostepping(size_t drive, int mode, bool& interpolation); void SetDriversPowered(bool powered); + void SetStallThreshold(size_t drive, int sgThreshold); + void SetStallFilter(size_t drive, bool sgFilter); + void SetCoolStep(size_t drive, uint16_t coolStepConfig); + void AppendStallConfig(size_t drive, StringRef& reply); }; #endif /* TMC2660_H_ */ diff --git a/src/Fan.cpp b/src/Fan.cpp index 155ad99e..3b792b4a 100644 --- a/src/Fan.cpp +++ b/src/Fan.cpp @@ -204,7 +204,7 @@ void Fan::SetHeatersMonitored(uint16_t h) void Fan::Refresh() { float reqVal; -#ifdef DUET_NG +#if HAS_SMART_DRIVERS uint32_t driverChannelsMonitored = 0; #endif @@ -247,7 +247,7 @@ void Fan::Refresh() // If the fan is on, add a hysteresis before turning it off reqVal = max<float>(reqVal, (bangBangMode) ? max<float>(0.5, val) : minVal); } -#ifdef DUET_NG +#if HAS_SMART_DRIVERS const unsigned int channel = reprap.GetHeat().GetHeaterChannel(heaterHumber); if (channel >= FirstTmcDriversSenseChannel && channel < FirstTmcDriversSenseChannel + NumTmcDriversSenseChannels) { @@ -269,7 +269,7 @@ void Fan::Refresh() if (lastVal == 0.0) { // We are turning this fan on -#ifdef DUET_NG +#if HAS_SMART_DRIVERS if (driverChannelsMonitored != 0) { reprap.GetPlatform().DriverCoolingFansOn(driverChannelsMonitored); // tell Platform that we have started a fan that cools drivers diff --git a/src/GCodes/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer.cpp index bea16d10..5c444a9d 100644 --- a/src/GCodes/GCodeBuffer.cpp +++ b/src/GCodes/GCodeBuffer.cpp @@ -27,18 +27,19 @@ void GCodeBuffer::Reset() void GCodeBuffer::Init() { - gcodePointer = 0; + gcodeLineEnd = 0; commandLength = 0; readPointer = -1; - inQuotes = inComment = timerRunning = false; - bufferState = GCodeBufferState::idle; + hadLineNumber = hadChecksum = timerRunning = false; + computedChecksum = 0; + bufferState = GCodeBufferState::parseNotStarted; } void GCodeBuffer::Diagnostics(MessageType mtype) { switch (bufferState) { - case GCodeBufferState::idle: + case GCodeBufferState::parseNotStarted: scratchString.printf("%s is idle", identity); break; @@ -48,6 +49,10 @@ void GCodeBuffer::Diagnostics(MessageType mtype) case GCodeBufferState::executing: scratchString.printf("%s is doing \"%s\"", identity, Buffer()); + break; + + default: + scratchString.printf("%s is assembling a command", identity); } scratchString.cat(" in state(s)"); @@ -62,14 +67,18 @@ void GCodeBuffer::Diagnostics(MessageType mtype) reprap.GetPlatform().Message(mtype, scratchString.Pointer()); } -int GCodeBuffer::CheckSum() const +inline void GCodeBuffer::AddToChecksum(char c) +{ + computedChecksum ^= (uint8_t)c; +} + +inline void GCodeBuffer::StoreAndAddToChecksum(char c) { - uint8_t cs = 0; - for (size_t i = 0; gcodeBuffer[i] != '*' && gcodeBuffer[i] != 0; i++) + computedChecksum ^= (uint8_t)c; + if (gcodeLineEnd < ARRAY_SIZE(gcodeBuffer)) { - cs = cs ^ (uint8_t)gcodeBuffer[i]; + gcodeBuffer[gcodeLineEnd++] = c; } - return (int)cs; } // Add a byte to the code being assembled. If false is returned, the code is @@ -81,88 +90,262 @@ bool GCodeBuffer::Put(char c) ++commandLength; } - if (!inQuotes && ((c == ';') || (gcodePointer == 0 && c == '('))) + if (c == 0 || c == '\n' || c == '\r') { - inComment = true; + return LineFinished(); } - else if (c == '\n' || c == '\r' || c == 0) + + // Process the incoming character in a state machine + bool again; + do { - gcodeBuffer[gcodePointer] = 0; - if (reprap.Debug(moduleGcodes) && gcodeBuffer[0] != 0 && !writingFileDirectory) // don't bother echoing blank/comment lines + again = false; + switch (bufferState) { - reprap.GetPlatform().MessageF(DebugMessage, "%s: %s\n", identity, gcodeBuffer); - } + case GCodeBufferState::parseNotStarted: // we haven't started parsing yet + switch (c) + { + case 'N': + case 'n': + hadLineNumber = true; + AddToChecksum(c); + bufferState = GCodeBufferState::parsingLineNumber; + lineNumber = 0; + break; - // Deal with line numbers and checksums - if (Seen('*')) - { - const int csSent = GetIValue(); - const int csHere = CheckSum(); - if (csSent != csHere) + case ' ': + case '\t': + AddToChecksum(c); + break; + + default: + bufferState = GCodeBufferState::parsingGCode; + commandStart = 0; + again = true; + break; + } + break; + + case GCodeBufferState::parsingLineNumber: // we saw N at the start and we are parsing the line number + if (isDigit(c)) { - if (Seen('N')) - { - snprintf(gcodeBuffer, GCODE_LENGTH, "M998 P%ld", GetIValue()); - } - Init(); - return true; + AddToChecksum(c); + lineNumber = (10 * lineNumber) + (c - '0'); + break; + } + else + { + bufferState = GCodeBufferState::parsingWhitespace; + again = true; + } + break; + + case GCodeBufferState::parsingWhitespace: + switch (c) + { + case ' ': + case '\t': + AddToChecksum(c); + break; + + default: + bufferState = GCodeBufferState::parsingGCode; + commandStart = 0; + again = true; + break; + } + break; + + case GCodeBufferState::parsingGCode: // parsing GCode words + switch (c) + { + case '*': + declaredChecksum = 0; + hadChecksum = true; + bufferState = GCodeBufferState::parsingChecksum; + break; + + case ';': + bufferState = GCodeBufferState::discarding; + break; + + case '(': + AddToChecksum(c); + bufferState = GCodeBufferState::parsingBracketedComment; + break; + + case '"': + StoreAndAddToChecksum(c); + bufferState = GCodeBufferState::parsingQuotedString; + break; + + default: + StoreAndAddToChecksum(c); } + break; - // Strip out the line number and checksum - gcodePointer = 0; - while (gcodeBuffer[gcodePointer] != ' ' && gcodeBuffer[gcodePointer] != 0) + case GCodeBufferState::parsingBracketedComment: // inside a (...) comment + AddToChecksum(c); + if (c == ')') { - gcodePointer++; + bufferState = GCodeBufferState::parsingGCode; } + break; - // Anything there? - if (gcodeBuffer[gcodePointer] == 0) + case GCodeBufferState::parsingQuotedString: // inside a double-quoted string + StoreAndAddToChecksum(c); + if (c == '"') { - // No... - gcodeBuffer[0] = 0; - Init(); - return false; + bufferState = GCodeBufferState::parsingGCode; } + break; - // Yes... - gcodePointer++; - int gp2 = 0; - while (gcodeBuffer[gcodePointer] != '*' && gcodeBuffer[gcodePointer] != 0) + case GCodeBufferState::parsingChecksum: // parsing the checksum after '*' + if (isDigit(c)) + { + declaredChecksum = (10 * declaredChecksum) + (c - '0'); + } + else { - gcodeBuffer[gp2] = gcodeBuffer[gcodePointer++]; - gp2++; + bufferState = GCodeBufferState::discarding; + again = true; } - gcodeBuffer[gp2] = 0; + break; + + case GCodeBufferState::discarding: // discarding characters after the checksum or an end-of-line comment + default: + // throw the character away + break; + } + } while (again); + + return false; +} + +// This is called when we are fed a null, CR or LF character. +// Return true if there is a completed command ready to be executed. +bool GCodeBuffer::LineFinished() +{ + if (gcodeLineEnd == 0) + { + // Empty line + Init(); + return false; + } + + if (gcodeLineEnd == ARRAY_SIZE(gcodeBuffer)) + { + reprap.GetPlatform().MessageF(ErrorMessage, "G-Code buffer '%s' length overflow\n", identity); + Init(); + return false; + } + + gcodeBuffer[gcodeLineEnd] = 0; + const bool badChecksum = (hadChecksum && computedChecksum != declaredChecksum); + const bool missingChecksum = (checksumRequired && !hadChecksum && machineState->previous == nullptr); + if (reprap.Debug(moduleGcodes) && !writingFileDirectory) + { + reprap.GetPlatform().MessageF(DebugMessage, "%s%s: %s\n", identity, ((badChecksum) ? "(bad-csum)" : (missingChecksum) ? "(no-csum)" : ""), gcodeBuffer); + } + + if (badChecksum) + { + if (hadLineNumber) + { + snprintf(gcodeBuffer, ARRAY_SIZE(gcodeBuffer), "M998 P%u", lineNumber); // request resend } - else if ((checksumRequired && machineState->previous == nullptr) || IsEmpty()) + else { - // Checksum not found or buffer empty - cannot do anything - gcodeBuffer[0] = 0; Init(); return false; } - readPointer = -1;; - bufferState = GCodeBufferState::ready; - return true; } - else if (!inComment || writingFileDirectory) + else if (missingChecksum) + { + // Checksum required but none was provided + Init(); + return false; + } + + commandStart = 0; + DecodeCommand(); + return true; +} + +// Decode this command command and find the start of the next one on the same line. +// On entry, 'commandStart' has already been set to the address the start of where the command should be. +// On return, the state must be set to 'ready' to indicate that a command is available and we should stop adding characters. +void GCodeBuffer::DecodeCommand() +{ + // Check for a valid command letter at the start + commandLetter = toupper(gcodeBuffer[commandStart]); + hasCommandNumber = false; + commandNumber = -1; + commandFraction = -1; + if (commandLetter == 'G' || commandLetter == 'M' || commandLetter == 'T') { - if (gcodePointer >= (int)GCODE_LENGTH) + parameterStart = commandStart + 1; + const bool negative = (gcodeBuffer[parameterStart] == '-'); + if (negative) { - reprap.GetPlatform().MessageF(ErrorMessage, "G-Code buffer '%s' length overflow\n", identity); - Init(); + ++parameterStart; } - else + if (isdigit(gcodeBuffer[parameterStart])) + { + hasCommandNumber = true; + // Read the number after the command letter + commandNumber = 0; + do + { + commandNumber = (10 * commandNumber) + (gcodeBuffer[parameterStart] - '0'); + ++parameterStart; + } + while (isdigit(gcodeBuffer[parameterStart])); + if (negative) + { + commandNumber = -commandNumber; + } + + // Read the fractional digit, if any + if (gcodeBuffer[parameterStart] == '.') + { + ++parameterStart; + if (isdigit(gcodeBuffer[parameterStart])) + { + commandFraction = gcodeBuffer[parameterStart] - '0'; + ++parameterStart; + } + } + } + + // Find where the end of the command is. We assume that a G or M preceded by a space and not inside quotes is the start of a new command. + bool inQuotes = false; + bool primed = false; + for (commandEnd = parameterStart; commandEnd < gcodeLineEnd; ++commandEnd) { - gcodeBuffer[gcodePointer++] = c; - if (c == '"' && !inComment) + const char c = gcodeBuffer[commandEnd]; + char c2; + if (c == '"') { inQuotes = !inQuotes; + primed = false; + } + else if (!inQuotes) + { + if (primed && ((c2 = toupper(c)) == 'G' || c2 == 'M')) + { + break; + } + primed = (c == ' ' || c == '\t'); } } } - - return false; + else + { + parameterStart = commandStart; + commandEnd = gcodeLineEnd; + } + bufferState = GCodeBufferState::ready; } // Add an entire string, overwriting any existing content @@ -184,8 +367,16 @@ void GCodeBuffer::SetFinished(bool f) { if (f) { - bufferState = GCodeBufferState::idle; - Init(); + if (commandEnd < gcodeLineEnd) + { + // There is another command in the same line of gcode + commandStart = commandEnd; + DecodeCommand(); + } + else + { + Init(); + } } else { @@ -198,88 +389,32 @@ FilePosition GCodeBuffer::GetFilePosition(size_t bytesCached) const { if (machineState->fileState.IsLive()) { - return machineState->fileState.GetPosition() - bytesCached - commandLength; + return machineState->fileState.GetPosition() - bytesCached - commandLength + commandStart; } return noFilePosition; } -// Does this buffer contain any code? -bool GCodeBuffer::IsEmpty() const -{ - const char *buf = gcodeBuffer; - while (*buf != 0 && strchr(" \t\n\r", *buf) != nullptr) - { - buf++; - } - return *buf == 0; -} - // Is 'c' in the G Code string? // Leave the pointer there for a subsequent read. bool GCodeBuffer::Seen(char c) { - readPointer = 0; bool inQuotes = false; - for (;;) + for (readPointer = parameterStart; (unsigned int)readPointer < commandEnd; ++readPointer) { const char b = gcodeBuffer[readPointer]; - if (b == 0) - { - break; - } if (b == '"') { inQuotes = !inQuotes; } - else if (!inQuotes) + else if (!inQuotes && toupper(b) == c) { - if (b == ';') - { - break; - } - if (toupper(b) == c) - { - return true; - } + return true; } - ++readPointer; } readPointer = -1; return false; } -// Return the first G, M or T command letter. Needed so that we don't pick up a spurious command letter from inside a string parameter. -char GCodeBuffer::GetCommandLetter() -{ - readPointer = 0; - for (;;) - { - char b = gcodeBuffer[readPointer]; - if (b == 0 || b == ';' || b == '"' || b == '(') - { - break; - } - b = (char)toupper(b); - if (b == 'T') - { - return b; - } - if (b == 'G' || b == 'M') - { - // Check that the command letter is followed by a digit. This is mostly to avoid a malformed string in which the first letter is M being interpreted as M0. - const char b2 = gcodeBuffer[readPointer + 1]; - if (b2 >= '0' && b2 <= '9') - { - return b; - } - break; - } - ++readPointer; - } - readPointer = -1; - return 0; -} - // Get a float after a G Code letter found by a call to Seen() float GCodeBuffer::GetFValue() { @@ -290,11 +425,11 @@ float GCodeBuffer::GetFValue() return result; } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode float before a search.\n"); + INTERNAL_ERROR; return 0.0; } -// Get a :-separated list of floats after a key letter +// Get a colon-separated list of floats after a key letter const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool doPad) { if (readPointer >= 0) @@ -340,7 +475,7 @@ const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool do } else { - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode float array before a search.\n"); + INTERNAL_ERROR; returnedLength = 0; } } @@ -377,7 +512,8 @@ const void GCodeBuffer::GetLongArray(long l[], size_t& returnedLength) } else { - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode long array before a search.\n"); + INTERNAL_ERROR; + returnedLength = 0; } } @@ -388,12 +524,13 @@ const char* GCodeBuffer::GetString() { if (readPointer >= 0) { + commandEnd = gcodeLineEnd; // the string is the remainder of the line of gcode const char* const result = &gcodeBuffer[readPointer + 1]; readPointer = -1; return result; } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n"); + INTERNAL_ERROR; return ""; } @@ -440,7 +577,7 @@ bool GCodeBuffer::GetQuotedString(const StringRef& str) return false; } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n"); + INTERNAL_ERROR; return false; } @@ -454,6 +591,7 @@ bool GCodeBuffer::GetPossiblyQuotedString(const StringRef& str) return GetQuotedString(str); } + commandEnd = gcodeLineEnd; // the string is the remainder of the line of gcode str.Clear(); for (;;) { @@ -469,43 +607,33 @@ bool GCodeBuffer::GetPossiblyQuotedString(const StringRef& str) return !str.IsEmpty(); } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a possibly quoted GCode string before a search.\n"); + INTERNAL_ERROR; return false; } -// This returns a pointer to the end of the buffer where a -// string starts. It assumes that an M or G search has -// been done followed by a GetIValue(), so readPointer will -// be -1. It absorbs "M/Gnnn " (including the space) from the -// start and returns a pointer to the next location. - -// This is provided for legacy use, in particular in the M23 +// This returns a pointer to the end of the buffer where a string starts. +// It is provided for legacy use, in particular in the M23 // command that sets the name of a file to be printed. In // preference use GetString() which requires the string to have // been preceded by a tag letter. - // If no string was provided, it produces an error message if the string was not optional, and returns nullptr. const char* GCodeBuffer::GetUnprecedentedString(bool optional) { - readPointer = 0; - while (gcodeBuffer[readPointer] != 0 && gcodeBuffer[readPointer] != ' ') - { - readPointer++; - } + commandEnd = gcodeLineEnd; // the string is the remainder of the line + size_t i; + char c; + for (i = parameterStart; i < commandEnd && ((c = gcodeBuffer[i]) == ' ' || c == '\t'); ++i) { } - if (gcodeBuffer[readPointer] == 0) + if (i == commandEnd) { - readPointer = -1; if (!optional) { - reprap.GetPlatform().Message(ErrorMessage, "GCodes: String expected but not seen.\n"); + reprap.GetPlatform().MessageF(ErrorMessage, "%c%d: String expected but not seen.\n", commandLetter, commandNumber); } return nullptr; } - const char* const result = &gcodeBuffer[readPointer + 1]; - readPointer = -1; - return result; + return &gcodeBuffer[i]; } // Get an int32 after a G Code letter @@ -518,8 +646,7 @@ int32_t GCodeBuffer::GetIValue() return result; } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode int before a search.\n"); - readPointer = -1; + INTERNAL_ERROR; return 0; } @@ -533,8 +660,7 @@ uint32_t GCodeBuffer::GetUIValue() return result; } - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode unsigned int before a search.\n"); - readPointer = -1; + INTERNAL_ERROR; return 0; } @@ -619,9 +745,10 @@ bool GCodeBuffer::GetIPAddress(uint8_t ip[4]) { if (readPointer < 0) { - reprap.GetPlatform().Message(ErrorMessage, "GCodes: Attempt to read a GCode string before a search.\n"); + INTERNAL_ERROR; return false; } + const char* p = &gcodeBuffer[readPointer + 1]; unsigned int n = 0; for (;;) @@ -654,6 +781,12 @@ bool GCodeBuffer::GetIPAddress(uint8_t ip[4]) // Get an IP address quad after a key letter bool GCodeBuffer::GetIPAddress(uint32_t& ip) { + if (readPointer < 0) + { + INTERNAL_ERROR; + return false; + } + uint8_t ipa[4]; const bool ok = GetIPAddress(ipa); if (ok) @@ -697,6 +830,7 @@ bool GCodeBuffer::PushState() ms->axesRelative = machineState->axesRelative; ms->doingFileMacro = machineState->doingFileMacro; ms->waitWhileCooling = machineState->waitWhileCooling; + ms->runningM501 = machineState->runningM501; ms->runningM502 = machineState->runningM502; ms->volumetricExtrusion = false; ms->messageAcknowledged = false; diff --git a/src/GCodes/GCodeBuffer.h b/src/GCodes/GCodeBuffer.h index c31c6340..f917f1d4 100644 --- a/src/GCodes/GCodeBuffer.h +++ b/src/GCodes/GCodeBuffer.h @@ -22,10 +22,13 @@ public: void Diagnostics(MessageType mtype); // Write some debug info bool Put(char c) __attribute__((hot)); // Add a character to the end bool Put(const char *str, size_t len); // Add an entire string, overwriting any existing content - bool IsEmpty() const; // Does this buffer contain any code? bool Seen(char c) __attribute__((hot)); // Is a character present? - char GetCommandLetter() __attribute__((hot)); // Find the first G, M or T command + char GetCommandLetter() const { return commandLetter; } + bool HasCommandNumber() const { return hasCommandNumber; } + int GetCommandNumber() const { return commandNumber; } + int8_t GetCommandFraction() const { return commandFraction; } + float GetFValue() __attribute__((hot)); // Get a float after a key letter int32_t GetIValue() __attribute__((hot)); // Get an integer after a key letter uint32_t GetUIValue(); // Get an unsigned integer value @@ -55,7 +58,7 @@ public: int GetToolNumberAdjust() const { return toolNumberAdjust; } void SetToolNumberAdjust(int arg) { toolNumberAdjust = arg; } void SetCommsProperties(uint32_t arg) { checksumRequired = (arg & 1); } - bool StartingNewCode() const { return gcodePointer == 0; } + bool StartingNewCode() const { return gcodeLineEnd == 0; } MessageType GetResponseMessageType() const { return responseMessageType; } GCodeMachineState& MachineState() const { return *machineState; } GCodeMachineState& OriginalMachineState() const; @@ -79,28 +82,49 @@ public: private: - enum class GCodeBufferState + enum class GCodeBufferState : uint8_t { - idle, // we don't have a complete gcode ready - ready, // we have a complete gcode but haven't started executing it - executing // we have a complete gcode and have started executing it + parseNotStarted, // we haven't started parsing yet + parsingLineNumber, // we saw N at the start and we are parsing the line number + parsingWhitespace, // parsing whitespace after the line number + parsingGCode, // parsing GCode words + parsingBracketedComment, // inside a (...) comment + parsingQuotedString, // inside a double-quoted string + parsingChecksum, // parsing the checksum after '*' + discarding, // discarding characters after the checksum or an end-of-line comment + ready, // we have a complete gcode but haven't started executing it + executing // we have a complete gcode and have started executing it }; - int CheckSum() const; // Compute the checksum (if any) at the end of the G Code + void AddToChecksum(char c); + void StoreAndAddToChecksum(char c); + bool LineFinished(); // Deal with receiving end-of-line and return true if we have a command + void DecodeCommand(); GCodeMachineState *machineState; // Machine state for this gcode source char gcodeBuffer[GCODE_LENGTH]; // The G Code - const char* identity; // Where we are from (web, file, serial line etc) - int gcodePointer; // Index in the buffer + const char* const identity; // Where we are from (web, file, serial line etc) + unsigned int commandStart; // Index in the buffer of the command letter of this command + unsigned int parameterStart; + unsigned int commandEnd; // Index in the buffer of one past the last character of this command unsigned int commandLength; // Number of characters we read to build this command including the final \r or \n + unsigned int gcodeLineEnd; // Number of characters in the entire line of gcode int readPointer; // Where in the buffer to read next - bool inQuotes; // Are we inside double quotation marks? - bool inComment; // Are we after a ';' character? bool checksumRequired; // True if we only accept commands with a valid checksum GCodeBufferState bufferState; // Idle, executing or paused const char* writingFileDirectory; // If the G Code is going into a file, where that is int toolNumberAdjust; // The adjustment to tool numbers in commands we receive const MessageType responseMessageType; // The message type we use for responses to commands coming from this channel + unsigned int lineNumber; + unsigned int declaredChecksum; + uint8_t computedChecksum; + bool hadLineNumber; + bool hadChecksum; + bool hasCommandNumber; + char commandLetter; + int commandNumber; + int8_t commandFraction; + bool queueCodes; // Can we queue certain G-codes from this source? bool binaryWriting; // Executing gcode or writing binary file? uint32_t crc32; // crc32 of the binary file @@ -133,7 +157,7 @@ inline const char* GCodeBuffer::Buffer() const inline bool GCodeBuffer::IsIdle() const { - return bufferState == GCodeBufferState::idle; + return bufferState != GCodeBufferState::ready && bufferState != GCodeBufferState::executing; } inline bool GCodeBuffer::IsReady() const diff --git a/src/GCodes/GCodeInput.cpp b/src/GCodes/GCodeInput.cpp index 3a6f6227..e4787fbd 100644 --- a/src/GCodes/GCodeInput.cpp +++ b/src/GCodes/GCodeInput.cpp @@ -13,7 +13,7 @@ bool GCodeInput::FillBuffer(GCodeBuffer *gb) { - size_t bytesToPass = min<size_t>(BytesCached(), GCODE_LENGTH); + const size_t bytesToPass = min<size_t>(BytesCached(), GCODE_LENGTH); for (size_t i = 0; i < bytesToPass; i++) { const char c = ReadByte(); diff --git a/src/GCodes/GCodeMachineState.cpp b/src/GCodes/GCodeMachineState.cpp index 5b21a612..d4a600eb 100644 --- a/src/GCodes/GCodeMachineState.cpp +++ b/src/GCodes/GCodeMachineState.cpp @@ -13,7 +13,7 @@ unsigned int GCodeMachineState::numAllocated = 0; // Create a default initialised GCodeMachineState GCodeMachineState::GCodeMachineState() : previous(nullptr), feedrate(DefaultFeedrate * SecondsToMinutes), fileState(), lockedResources(0), state(GCodeState::normal), - drivesRelative(false), axesRelative(false), doingFileMacro(false), runningM502(false), volumetricExtrusion(false), waitingForAcknowledgement(false), messageAcknowledged(false) + drivesRelative(false), axesRelative(false), doingFileMacro(false), runningM501(false), runningM502(false), volumetricExtrusion(false), waitingForAcknowledgement(false), messageAcknowledged(false) { } diff --git a/src/GCodes/GCodeMachineState.h b/src/GCodes/GCodeMachineState.h index ce5694cd..0848f369 100644 --- a/src/GCodes/GCodeMachineState.h +++ b/src/GCodes/GCodeMachineState.h @@ -16,7 +16,9 @@ enum class GCodeState : uint8_t { normal, // not doing anything and ready to process a new GCode - waitingForSpecialMoveToComplete, // doing a special move, so we must wait for it to finish before processing another GCode + waitingForSpecialMoveToComplete, // doing a special move, so we must wait for it to finish before processing another GCode + + probingToolOffset, homing1, homing2, @@ -35,11 +37,14 @@ enum class GCodeState : uint8_t pausing1, pausing2, + resuming1, resuming2, resuming3, + flashing1, flashing2, + stopping, sleeping, @@ -64,7 +69,11 @@ enum class GCodeState : uint8_t doingFirmwareRetraction, doingFirmwareUnRetraction, loadingFilament, - unloadingFilament + unloadingFilament, + +#if HAS_VOLTAGE_MONITOR + powerFailPausing1 +#endif }; // Class to hold the state of gcode execution for some input source @@ -79,11 +88,14 @@ public: FileData fileState; ResourceBitmap lockedResources; GCodeState state; + uint8_t toolChangeParam; + int16_t newToolNumber; unsigned int drivesRelative : 1, axesRelative : 1, doingFileMacro : 1, waitWhileCooling : 1, + runningM501 : 1, runningM502 : 1, volumetricExtrusion : 1, // Caution: these next 3 will be modified out-of-process when we use RTOS, so they will need to be individual bool variables diff --git a/src/GCodes/GCodeQueue.cpp b/src/GCodes/GCodeQueue.cpp index 69b35d87..0e701acf 100644 --- a/src/GCodes/GCodeQueue.cpp +++ b/src/GCodes/GCodeQueue.cpp @@ -49,21 +49,19 @@ bool GCodeQueue::QueueCode(GCodeBuffer &gb, uint32_t segmentsLeft) { case 'G': { - const int code = gb.GetIValue(); + const int code = gb.GetCommandNumber(); + queueCode = (code == 10 && gb.Seen('P')); // Set active/standby temperatures - // Set active/standby temperatures - queueCode = (code == 10 && gb.Seen('P')); } break; case 'M': { - const int code = gb.GetIValue(); - switch(code) + switch (gb.GetCommandNumber()) { case 3: // spindle control - case 4: - case 5: + case 4: // spindle control + case 5: // spindle control case 42: // set IO pin case 106: // fan control case 107: // fan off diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index fb2c42b5..6acf9fb7 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -44,8 +44,11 @@ const char GCodes::axisLetters[MaxAxes] = AXES_('X', 'Y', 'Z', 'U', 'V', 'W', 'A const size_t gcodeReplyLength = 2048; // long enough to pass back a reasonable number of files in response to M20 GCodes::GCodes(Platform& p) : - platform(p), machineType(MachineType::fff), active(false), isFlashing(false), - fileBeingHashed(nullptr), lastWarningMillis(0) + platform(p), machineType(MachineType::fff), active(false), +#if HAS_VOLTAGE_MONITOR + powerFailScript(nullptr), +#endif + isFlashing(false), fileBeingHashed(nullptr), lastWarningMillis(0) { httpInput = new RegularGCodeInput(true); telnetInput = new RegularGCodeInput(true); @@ -60,9 +63,7 @@ GCodes::GCodes(Platform& p) : auxGCode = new GCodeBuffer("aux", LcdMessage, false); daemonGCode = new GCodeBuffer("daemon", GenericMessage, false); queuedGCode = new GCodeBuffer("queue", GenericMessage, false); -#ifdef DUET_NG autoPauseGCode = new GCodeBuffer("autopause", GenericMessage, false); -#endif codeQueue = new GCodeQueue(); } @@ -89,7 +90,6 @@ void GCodes::Init() eofStringLength = strlen(eofString); runningConfigFile = false; doingToolChange = false; - toolChangeParam = DefaultToolChangeParam; active = true; fileSize = 0; longWait = millis(); @@ -128,9 +128,7 @@ void GCodes::Reset() auxGCode->SetCommsProperties(1); // by default, we require a checksum on the aux port daemonGCode->Reset(); queuedGCode->Reset(); -#ifdef DUET_NG autoPauseGCode->Reset(); -#endif nextGcodeSource = 0; @@ -178,8 +176,8 @@ void GCodes::Reset() exitSimulationWhenFileComplete = false; simulationTime = 0.0; isPaused = false; -#ifdef DUET_NG - isAutoPaused = false; +#if HAS_VOLTAGE_MONITOR + isPowerFailPaused = false; #endif doingToolChange = false; doingManualBedProbe = false; @@ -259,9 +257,20 @@ void GCodes::Spin() } CheckTriggers(); + CheckFilament(); - // Get the GCodeBuffer that we want to work from - GCodeBuffer& gb = *(gcodeSources[nextGcodeSource]); + // Get the GCodeBuffer that we want to process a command from. Give priority to auto-pause. + GCodeBuffer *gbp = autoPauseGCode; + if (gbp->GetState() == GCodeState::normal && gbp->IsIdle()) + { + gbp = gcodeSources[nextGcodeSource]; + ++nextGcodeSource; // move on to the next gcode source ready for next time + if (nextGcodeSource == ARRAY_SIZE(gcodeSources) - 1) // the last one is autoPauseGCode, so don't do it again + { + nextGcodeSource = 0; + } + } + GCodeBuffer& gb = *gbp; // Set up a buffer for the reply char replyBuffer[gcodeReplyLength]; @@ -294,888 +303,921 @@ void GCodes::Spin() } else { - // Perform the next operation of the state machine for this gcode source - bool error = false; + RunStateMachine(gb, reply); // Execute the state machine + } - switch (gb.GetState()) + // Check if we need to display a warning + const uint32_t now = millis(); + if (now - lastWarningMillis >= MinimumWarningInterval) + { + if (displayNoToolWarning) { - case GCodeState::waitingForSpecialMoveToComplete: - if (LockMovementAndWaitForStandstill(gb)) // movement should already be locked, but we need to wait for standstill and fetch the current position + platform.Message(ErrorMessage, "Attempting to extrude with no tool selected.\n"); + displayNoToolWarning = false; + lastWarningMillis = now; + } + if (displayDeltaNotHomedWarning) + { + platform.Message(ErrorMessage, "Attempt to move the head of a Delta or SCARA printer before homing it\n"); + displayDeltaNotHomedWarning = false; + lastWarningMillis = now; + } + } + platform.ClassReport(longWait); +} + +// Execute a step of the state machine +void GCodes::RunStateMachine(GCodeBuffer& gb, StringRef& reply) +{ + // Perform the next operation of the state machine for this gcode source + bool error = false; + + switch (gb.GetState()) + { + case GCodeState::waitingForSpecialMoveToComplete: + if (LockMovementAndWaitForStandstill(gb)) // movement should already be locked, but we need to wait for standstill and fetch the current position + { + // Check whether we made any G1 S3 moves and need to set the axis limits + for (size_t axis = 0; axis < numVisibleAxes; ++axis) { - // Check whether we made any G1 S3 moves and need to set the axis limits - for (size_t axis = 0; axis < numVisibleAxes; ++axis) + if (IsBitSet<AxesBitmap>(axesToSenseLength, axis)) { - if (IsBitSet<AxesBitmap>(axesToSenseLength, axis)) + EndStopType stopType; + bool dummy; + platform.GetEndStopConfiguration(axis, stopType, dummy); + if (stopType == EndStopType::highEndStop) { - EndStopType stopType; - bool dummy; - platform.GetEndStopConfiguration(axis, stopType, dummy); - if (stopType == EndStopType::highEndStop) - { - platform.SetAxisMaximum(axis, moveBuffer.coords[axis]); - } - else if (stopType == EndStopType::lowEndStop) - { - platform.SetAxisMinimum(axis, moveBuffer.coords[axis]); - } + platform.SetAxisMaximum(axis, moveBuffer.coords[axis], true); + } + else if (stopType == EndStopType::lowEndStop) + { + platform.SetAxisMinimum(axis, moveBuffer.coords[axis], true); } } - gb.SetState(GCodeState::normal); } - break; + gb.SetState(GCodeState::normal); + } + break; - case GCodeState::homing1: - if (toBeHomed == 0) - { - gb.SetState(GCodeState::normal); - } - else + case GCodeState::probingToolOffset: + if (LockMovementAndWaitForStandstill(gb)) + { + Tool * const currentTool = reprap.GetCurrentTool(); + if (currentTool != nullptr) { - AxesBitmap mustHomeFirst; - const char *nextHomingFile = reprap.GetMove().GetKinematics().GetHomingFileName(toBeHomed, axesHomed, numVisibleAxes, mustHomeFirst); - if (nextHomingFile == nullptr) + for (size_t axis = 0; axis < numTotalAxes; ++axis) { - // Error, can't home this axes - reply.copy("Must home these axes:"); - AppendAxes(reply, mustHomeFirst); - reply.cat(" before homing these:"); - AppendAxes(reply, toBeHomed); - error = true; - toBeHomed = 0; - gb.SetState(GCodeState::normal); - } - else - { - gb.SetState(GCodeState::homing2); - if (!DoFileMacro(gb, nextHomingFile, false)) + if (gb.Seen(axisLetters[axis])) { - reply.printf("Homing file %s not found", nextHomingFile); - error = true; - gb.SetState(GCodeState::normal); + // We get here when the tool probe has been activated. In this case we know how far we + // went (i.e. the difference between our start and end positions) and if we need to + // incorporate any correction factors. That's why we only need to set the final tool + // offset to this value in order to finish the tool probing. + currentTool->SetOffset(axis, (currentUserPosition[axis] - toolChangeRestorePoint.moveCoords[axis]) + gb.GetFValue(), true); + break; } } } - break; + gb.SetState(GCodeState::normal); + } + break; - case GCodeState::homing2: - if (LockMovementAndWaitForStandstill(gb)) // movement should already be locked, but we need to wait for the previous homing move to complete + case GCodeState::homing1: + if (toBeHomed == 0) + { + gb.SetState(GCodeState::normal); + } + else + { + AxesBitmap mustHomeFirst; + const char *nextHomingFile = reprap.GetMove().GetKinematics().GetHomingFileName(toBeHomed, axesHomed, numVisibleAxes, mustHomeFirst); + if (nextHomingFile == nullptr) { - // Test whether the previous homing move homed any axes - if ((toBeHomed & axesHomed) == 0) + // Error, can't home this axes + reply.copy("Must home these axes:"); + AppendAxes(reply, mustHomeFirst); + reply.cat(" before homing these:"); + AppendAxes(reply, toBeHomed); + error = true; + toBeHomed = 0; + gb.SetState(GCodeState::normal); + } + else + { + gb.SetState(GCodeState::homing2); + if (!DoFileMacro(gb, nextHomingFile, false)) { - reply.copy("Homing failed"); + reply.printf("Homing file %s not found", nextHomingFile); error = true; gb.SetState(GCodeState::normal); } - else - { - toBeHomed &= ~axesHomed; - gb.SetState((toBeHomed == 0) ? GCodeState::normal : GCodeState::homing1); - } } - break; + } + break; - case GCodeState::toolChange0: // Run tfree for the old tool (if any) - case GCodeState::m109ToolChange0: // Run tfree for the old tool (if any) - doingToolChange = true; - SaveFanSpeeds(); - memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, MaxAxes * sizeof(currentUserPosition[0])); - toolChangeRestorePoint.feedRate = gb.MachineState().feedrate; - gb.AdvanceState(); - if ((toolChangeParam & TFreeBit) != 0) + case GCodeState::homing2: + if (LockMovementAndWaitForStandstill(gb)) // movement should already be locked, but we need to wait for the previous homing move to complete + { + // Test whether the previous homing move homed any axes + if ((toBeHomed & axesHomed) == 0) { - const Tool * const oldTool = reprap.GetCurrentTool(); - if (oldTool != nullptr && AllAxesAreHomed()) - { - scratchString.printf("tfree%d.g", oldTool->Number()); - DoFileMacro(gb, scratchString.Pointer(), false); - } + reply.copy("Homing failed"); + error = true; + gb.SetState(GCodeState::normal); } - break; - - case GCodeState::toolChange1: // Release the old tool (if any), then run tpre for the new tool - case GCodeState::m109ToolChange1: // Release the old tool (if any), then run tpre for the new tool + else { - const Tool * const oldTool = reprap.GetCurrentTool(); - if (oldTool != nullptr) - { - reprap.StandbyTool(oldTool->Number()); - } + toBeHomed &= ~axesHomed; + gb.SetState((toBeHomed == 0) ? GCodeState::normal : GCodeState::homing1); } - gb.AdvanceState(); - if (reprap.GetTool(newToolNumber) != nullptr && AllAxesAreHomed() && (toolChangeParam & TPreBit) != 0) + } + break; + + case GCodeState::toolChange0: // Run tfree for the old tool (if any) + case GCodeState::m109ToolChange0: // Run tfree for the old tool (if any) + doingToolChange = true; + SaveFanSpeeds(); + memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, MaxAxes * sizeof(currentUserPosition[0])); + toolChangeRestorePoint.feedRate = gb.MachineState().feedrate; + gb.AdvanceState(); + if ((gb.MachineState().toolChangeParam & TFreeBit) != 0) + { + const Tool * const oldTool = reprap.GetCurrentTool(); + if (oldTool != nullptr && AllAxesAreHomed()) { - scratchString.printf("tpre%d.g", newToolNumber); + scratchString.printf("tfree%d.g", oldTool->Number()); DoFileMacro(gb, scratchString.Pointer(), false); } - break; + } + break; - case GCodeState::toolChange2: // Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost - case GCodeState::m109ToolChange2: // Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost - reprap.SelectTool(newToolNumber, simulationMode != 0); - GetCurrentUserPosition(); // get the actual position of the new tool + case GCodeState::toolChange1: // Release the old tool (if any), then run tpre for the new tool + case GCodeState::m109ToolChange1: // Release the old tool (if any), then run tpre for the new tool + { + const Tool * const oldTool = reprap.GetCurrentTool(); + if (oldTool != nullptr) + { + reprap.StandbyTool(oldTool->Number()); + } + } + gb.AdvanceState(); + if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && AllAxesAreHomed() && (gb.MachineState().toolChangeParam & TPreBit) != 0) + { + scratchString.printf("tpre%d.g", gb.MachineState().newToolNumber); + DoFileMacro(gb, scratchString.Pointer(), false); + } + break; - gb.AdvanceState(); - if (AllAxesAreHomed()) + case GCodeState::toolChange2: // Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost + case GCodeState::m109ToolChange2: // Select the new tool (even if it doesn't exist - that just deselects all tools) and run tpost + reprap.SelectTool(gb.MachineState().newToolNumber, simulationMode != 0); + GetCurrentUserPosition(); // get the actual position of the new tool + + gb.AdvanceState(); + if (AllAxesAreHomed()) + { + if (reprap.GetTool(gb.MachineState().newToolNumber) != nullptr && (gb.MachineState().toolChangeParam & TPostBit) != 0) { - if (reprap.GetTool(newToolNumber) != nullptr && (toolChangeParam & TPostBit) != 0) - { - scratchString.printf("tpost%d.g", newToolNumber); - DoFileMacro(gb, scratchString.Pointer(), false); - } + scratchString.printf("tpost%d.g", gb.MachineState().newToolNumber); + DoFileMacro(gb, scratchString.Pointer(), false); } - break; + } + break; - case GCodeState::toolChangeComplete: - case GCodeState::m109ToolChangeComplete: - // Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate - currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS]; - gb.MachineState().feedrate = toolChangeRestorePoint.feedRate; - // We don't restore the default fan speed in case the user wants to use a different one for the new tool - doingToolChange = false; + case GCodeState::toolChangeComplete: + case GCodeState::m109ToolChangeComplete: + // Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate + currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS]; + gb.MachineState().feedrate = toolChangeRestorePoint.feedRate; + // We don't restore the default fan speed in case the user wants to use a different one for the new tool + doingToolChange = false; - if (gb.GetState() == GCodeState::toolChangeComplete) + if (gb.GetState() == GCodeState::toolChangeComplete) + { + gb.SetState(GCodeState::normal); + } + else + { + UnlockAll(gb); // allow movement again + if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling)) { + cancelWait = isWaiting = false; gb.SetState(GCodeState::normal); } else { - UnlockAll(gb); // allow movement again - if (cancelWait || ToolHeatersAtSetTemperatures(reprap.GetCurrentTool(), gb.MachineState().waitWhileCooling)) - { - cancelWait = isWaiting = false; - gb.SetState(GCodeState::normal); - } - else - { - CheckReportDue(gb, reply); - isWaiting = true; - } + CheckReportDue(gb, reply); + isWaiting = true; } - break; + } + break; - case GCodeState::pausing1: - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::pausing1: + if (LockMovementAndWaitForStandstill(gb)) + { + gb.SetState(GCodeState::pausing2); + if (AllAxesAreHomed()) { - gb.SetState(GCodeState::pausing2); - if (AllAxesAreHomed()) - { -#ifdef DUET_NG - DoFileMacro(gb, (isAutoPaused) ? POWER_FAIL_G : PAUSE_G, true); -#else - DoFileMacro(gb, PAUSE_G, true); -#endif - } + DoFileMacro(gb, PAUSE_G, true); } - break; + } + break; - case GCodeState::pausing2: - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::pausing2: + if (LockMovementAndWaitForStandstill(gb)) + { + reply.copy("Printing paused"); + platform.Message(LogMessage, "Printing paused\n"); + gb.SetState(GCodeState::normal); + } + break; + + case GCodeState::resuming1: + case GCodeState::resuming2: + // Here when we have just finished running the resume macro file. + // Move the head back to the paused location + if (LockMovementAndWaitForStandstill(gb)) + { + float currentZ = moveBuffer.coords[Z_AXIS]; + for (size_t drive = 0; drive < numVisibleAxes; ++drive) { -#ifdef DUET_NG - if (isAutoPaused) - { - platform.Message(LoggedGenericMessage, "Print auto-paused due to low voltage\n"); - reprap.GetHeat().SuspendHeaters(false); // resume the heaters, we may have turned them off while we executed the pause macro - } - else -#endif - { - reply.copy("Printing paused"); - platform.Message(LogMessage, "Printing paused\n"); - } - gb.SetState(GCodeState::normal); + currentUserPosition[drive] = pauseRestorePoint.moveCoords[drive]; } - break; - - case GCodeState::resuming1: - case GCodeState::resuming2: - // Here when we have just finished running the resume macro file. - // Move the head back to the paused location - if (LockMovementAndWaitForStandstill(gb)) + ToolOffsetTransform(currentUserPosition, moveBuffer.coords); + for (size_t drive = numTotalAxes; drive < DRIVES; ++drive) { - float currentZ = moveBuffer.coords[Z_AXIS]; - for (size_t drive = 0; drive < numVisibleAxes; ++drive) - { - currentUserPosition[drive] = pauseRestorePoint.moveCoords[drive]; - } - ToolOffsetTransform(currentUserPosition, moveBuffer.coords); - for (size_t drive = numTotalAxes; drive < DRIVES; ++drive) - { - moveBuffer.coords[drive] = 0.0; - } - moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes; // ask for a good feed rate, we may have paused during a slow move - moveBuffer.moveType = 0; - moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = 0; - moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = noFilePosition; - if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS]) - { - // First move the head to the correct XY point, then move it down in a separate move - moveBuffer.coords[Z_AXIS] = currentZ; - gb.SetState(GCodeState::resuming2); - } - else - { - // Just move to the saved position in one go - gb.SetState(GCodeState::resuming3); - } - segmentsLeft = 1; + moveBuffer.coords[drive] = 0.0; } - break; - - case GCodeState::resuming3: - if (LockMovementAndWaitForStandstill(gb)) + moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes; // ask for a good feed rate, we may have paused during a slow move + moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; + moveBuffer.endStopsToCheck = 0; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = noFilePosition; + if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS]) { - for (size_t i = 0; i < NUM_FANS; ++i) - { - platform.SetFanValue(i, pausedFanSpeeds[i]); - } - virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition; // reset the extruder position in case we are receiving absolute extruder moves - moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition; - fileGCode->MachineState().feedrate = pauseRestorePoint.feedRate; - isPaused = false; - reply.copy("Printing resumed"); - platform.Message(LogMessage, "Printing resumed\n"); - gb.SetState(GCodeState::normal); + // First move the head to the correct XY point, then move it down in a separate move + moveBuffer.coords[Z_AXIS] = currentZ; + gb.SetState(GCodeState::resuming2); } - break; - - case GCodeState::flashing1: -#ifdef DUET_NG - // Update additional modules before the main firmware - if (FirmwareUpdater::IsReady()) + else { - bool updating = false; - for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module) - { - if ((firmwareUpdateModuleMap & (1u << module)) != 0) - { - firmwareUpdateModuleMap &= ~(1u << module); - FirmwareUpdater::UpdateModule(module); - updating = true; - break; - } - } - if (!updating) - { - gb.SetState(GCodeState::flashing2); - } + // Just move to the saved position in one go + gb.SetState(GCodeState::resuming3); } -#else - gb.SetState(GCodeState::flashing2); -#endif - break; + totalSegments = 1; + segmentsLeft = 1; + } + break; - case GCodeState::flashing2: - if ((firmwareUpdateModuleMap & 1) != 0) + case GCodeState::resuming3: + if (LockMovementAndWaitForStandstill(gb)) + { + for (size_t i = 0; i < NUM_FANS; ++i) { - // Update main firmware - firmwareUpdateModuleMap = 0; - platform.UpdateFirmware(); - // The above call does not return unless an error occurred + platform.SetFanValue(i, pausedFanSpeeds[i]); } - isFlashing = false; + virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition; // reset the extruder position in case we are receiving absolute extruder moves + moveBuffer.virtualExtruderPosition = pauseRestorePoint.virtualExtruderPosition; + fileGCode->MachineState().feedrate = pauseRestorePoint.feedRate; + isPaused = false; + reply.copy("Printing resumed"); + platform.Message(LogMessage, "Printing resumed\n"); gb.SetState(GCodeState::normal); - break; + } + break; - case GCodeState::stopping: // MO after executing stop.g if present - case GCodeState::sleeping: // M1 after executing sleep.g if present - // Deselect the active tool and turn off all heaters, unless parameter Hn was used with n > 0 - if (!gb.Seen('H') || gb.GetIValue() <= 0) + case GCodeState::flashing1: +#ifdef DUET_NG + // Update additional modules before the main firmware + if (FirmwareUpdater::IsReady()) + { + bool updating = false; + for (unsigned int module = 1; module < NumFirmwareUpdateModules; ++module) { - Tool* tool = reprap.GetCurrentTool(); - if (tool != nullptr) + if ((firmwareUpdateModuleMap & (1u << module)) != 0) { - reprap.StandbyTool(tool->Number()); + firmwareUpdateModuleMap &= ~(1u << module); + FirmwareUpdater::UpdateModule(module); + updating = true; + break; } - reprap.GetHeat().SwitchOffAll(); } - - // chrishamm 2014-18-10: Although RRP says M0 is supposed to turn off all drives and heaters, - // I think M1 is sufficient for this purpose. Leave M0 for a normal reset. - if (gb.GetState() == GCodeState::sleeping) + if (!updating) { - DisableDrives(); + gb.SetState(GCodeState::flashing2); } - else - { - platform.SetDriversIdle(); - } - gb.SetState(GCodeState::normal); - break; + } +#else + gb.SetState(GCodeState::flashing2); +#endif + break; - // States used for grid probing - case GCodeState::gridProbing1: // ready to move to next grid probe point - { - // Move to the current probe point - Move& move = reprap.GetMove(); - const GridDefinition& grid = move.AccessHeightMap().GetGrid(); - const float x = grid.GetXCoordinate(gridXindex); - const float y = grid.GetYCoordinate(gridYindex); - if (grid.IsInRadius(x, y)) - { - if (move.IsAccessibleProbePoint(x, y)) - { - moveBuffer.moveType = 0; - moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = 0; - moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = noFilePosition; - moveBuffer.coords[X_AXIS] = x - platform.GetCurrentZProbeParameters().xOffset; - moveBuffer.coords[Y_AXIS] = y - platform.GetCurrentZProbeParameters().yOffset; - moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); - moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); - moveBuffer.xAxes = DefaultXAxisMapping; - moveBuffer.yAxes = DefaultYAxisMapping; - segmentsLeft = 1; - gb.AdvanceState(); - } - else - { - platform.MessageF(WarningMessage, "Skipping grid point (%.1f, %.1f) because Z probe cannot reach it\n", (double)x, (double)y); - gb.SetState(GCodeState::gridProbing5); - } - } - else - { - gb.SetState(GCodeState::gridProbing5); - } - } - break; + case GCodeState::flashing2: + if ((firmwareUpdateModuleMap & 1) != 0) + { + // Update main firmware + firmwareUpdateModuleMap = 0; + platform.UpdateFirmware(); + // The above call does not return unless an error occurred + } + isFlashing = false; + gb.SetState(GCodeState::normal); + break; - case GCodeState::gridProbing2: // ready to probe the current grid probe point - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::stopping: // MO after executing stop.g if present + case GCodeState::sleeping: // M1 after executing sleep.g if present + // Deselect the active tool and turn off all heaters, unless parameter Hn was used with n > 0 + if (!gb.Seen('H') || gb.GetIValue() <= 0) + { + Tool* tool = reprap.GetCurrentTool(); + if (tool != nullptr) { - lastProbedTime = millis(); - gb.AdvanceState(); + reprap.StandbyTool(tool->Number()); } - break; + reprap.GetHeat().SwitchOffAll(); + } + + // chrishamm 2014-18-10: Although RRP says M0 is supposed to turn off all drives and heaters, + // I think M1 is sufficient for this purpose. Leave M0 for a normal reset. + if (gb.GetState() == GCodeState::sleeping) + { + DisableDrives(); + } + else + { + platform.SetDriversIdle(); + } + gb.SetState(GCodeState::normal); + break; - case GCodeState::gridProbing3: // ready to probe the current grid probe point - if (millis() - lastProbedTime >= (uint32_t)(reprap.GetPlatform().GetCurrentZProbeParameters().recoveryTime * SecondsToMillis)) + // States used for grid probing + case GCodeState::gridProbing1: // ready to move to next grid probe point + { + // Move to the current probe point + Move& move = reprap.GetMove(); + const GridDefinition& grid = move.AccessHeightMap().GetGrid(); + const float x = grid.GetXCoordinate(gridXindex); + const float y = grid.GetYCoordinate(gridYindex); + if (grid.IsInRadius(x, y)) { - // Probe the bed at the current XY coordinates - // Check for probe already triggered at start - if (platform.GetZProbeType() == 0) - { - // No Z probe, so do manual mesh levelling instead - UnlockAll(gb); // release the movement lock to allow manual Z moves - gb.AdvanceState(); // resume at next state when user has finished adjusting the height - doingManualBedProbe = true; // suspend the Z movement limit - DoManualProbe(gb); - } - else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) - { - platform.Message(ErrorMessage, "Z probe already triggered before probing move started"); - gb.SetState(GCodeState::normal); - if (platform.GetZProbeType() != 0 && !probeIsDeployed) - { - DoFileMacro(gb, RETRACTPROBE_G, false); - } - break; - } - else + if (move.IsAccessibleProbePoint(x, y)) { - zProbeTriggered = false; - platform.SetProbing(true); moveBuffer.moveType = 0; moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = ZProbeActive; + moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; - moveBuffer.coords[Z_AXIS] = -platform.GetZProbeDiveHeight(); - moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed; + moveBuffer.coords[X_AXIS] = x - platform.GetCurrentZProbeParameters().xOffset; + moveBuffer.coords[Y_AXIS] = y - platform.GetCurrentZProbeParameters().yOffset; + moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); + moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); moveBuffer.xAxes = DefaultXAxisMapping; moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; segmentsLeft = 1; gb.AdvanceState(); } + else + { + platform.MessageF(WarningMessage, "Skipping grid point (%.1f, %.1f) because Z probe cannot reach it\n", (double)x, (double)y); + gb.SetState(GCodeState::gridProbing5); + } } - break; + else + { + gb.SetState(GCodeState::gridProbing5); + } + } + break; + + case GCodeState::gridProbing2: // ready to probe the current grid probe point + if (LockMovementAndWaitForStandstill(gb)) + { + lastProbedTime = millis(); + gb.AdvanceState(); + } + break; - case GCodeState::gridProbing4: // ready to lift the probe after probing the current grid probe point - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::gridProbing3: // ready to probe the current grid probe point + if (millis() - lastProbedTime >= (uint32_t)(reprap.GetPlatform().GetCurrentZProbeParameters().recoveryTime * SecondsToMillis)) + { + // Probe the bed at the current XY coordinates + // Check for probe already triggered at start + if (platform.GetZProbeType() == 0) { - doingManualBedProbe = false; - float heightError; - if (platform.GetZProbeType() == 0) - { - // No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error. - heightError = moveBuffer.coords[Z_AXIS]; - } - else + // No Z probe, so do manual mesh levelling instead + UnlockAll(gb); // release the movement lock to allow manual Z moves + gb.AdvanceState(); // resume at next state when user has finished adjusting the height + doingManualBedProbe = true; // suspend the Z movement limit + DoManualProbe(gb); + } + else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) + { + platform.Message(ErrorMessage, "Z probe already triggered before probing move started"); + gb.SetState(GCodeState::normal); + if (platform.GetZProbeType() != 0 && !probeIsDeployed) { - platform.SetProbing(false); - if (!zProbeTriggered) - { - platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n"); - gb.SetState(GCodeState::normal); - if (platform.GetZProbeType() != 0 && !probeIsDeployed) - { - DoFileMacro(gb, RETRACTPROBE_G, false); - } - break; - } - - heightError = moveBuffer.coords[Z_AXIS] - platform.ZProbeStopHeight(); + DoFileMacro(gb, RETRACTPROBE_G, false); } - reprap.GetMove().AccessHeightMap().SetGridHeight(gridXindex, gridYindex, heightError); - - // Move back up to the dive height + break; + } + else + { + zProbeTriggered = false; + platform.SetProbing(true); moveBuffer.moveType = 0; moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = 0; + moveBuffer.endStopsToCheck = ZProbeActive; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; - moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); - moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); + moveBuffer.coords[Z_AXIS] = -platform.GetZProbeDiveHeight(); + moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed; moveBuffer.xAxes = DefaultXAxisMapping; moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; segmentsLeft = 1; - gb.SetState(GCodeState::gridProbing5); + gb.AdvanceState(); } - break; + } + break; - case GCodeState::gridProbing5: // ready to compute the next probe point - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::gridProbing4: // ready to lift the probe after probing the current grid probe point + if (LockMovementAndWaitForStandstill(gb)) + { + doingManualBedProbe = false; + float heightError; + if (platform.GetZProbeType() == 0) { - const GridDefinition& grid = reprap.GetMove().AccessHeightMap().GetGrid(); - if (gridYindex & 1) - { - // Odd row, so decreasing X - if (gridXindex == 0) - { - ++gridYindex; - } - else - { - --gridXindex; - } - } - else - { - // Even row, so increasing X - if (gridXindex + 1 == grid.NumXpoints()) - { - ++gridYindex; - } - else - { - ++gridXindex; - } - } - if (gridYindex == grid.NumYpoints()) + // No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error. + heightError = moveBuffer.coords[Z_AXIS]; + } + else + { + platform.SetProbing(false); + if (!zProbeTriggered) { - // Done all the points - gb.AdvanceState(); + platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n"); + gb.SetState(GCodeState::normal); if (platform.GetZProbeType() != 0 && !probeIsDeployed) { DoFileMacro(gb, RETRACTPROBE_G, false); } + break; + } + + heightError = moveBuffer.coords[Z_AXIS] - platform.ZProbeStopHeight(); + } + reprap.GetMove().AccessHeightMap().SetGridHeight(gridXindex, gridYindex, heightError); + + // Move back up to the dive height + moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; + moveBuffer.endStopsToCheck = 0; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = noFilePosition; + moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); + moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); + moveBuffer.xAxes = DefaultXAxisMapping; + moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; + segmentsLeft = 1; + gb.SetState(GCodeState::gridProbing5); + } + break; + + case GCodeState::gridProbing5: // ready to compute the next probe point + if (LockMovementAndWaitForStandstill(gb)) + { + const GridDefinition& grid = reprap.GetMove().AccessHeightMap().GetGrid(); + if (gridYindex & 1) + { + // Odd row, so decreasing X + if (gridXindex == 0) + { + ++gridYindex; } else { - gb.SetState(GCodeState::gridProbing1); + --gridXindex; } } - break; - - case GCodeState::gridprobing6: - // Finished probing the grid, and retracted the probe if necessary + else { - float mean, deviation; - const uint32_t numPointsProbed = reprap.GetMove().AccessHeightMap().GetStatistics(mean, deviation); - if (numPointsProbed >= 4) + // Even row, so increasing X + if (gridXindex + 1 == grid.NumXpoints()) { - reply.printf("%" PRIu32 " points probed, mean error %.3f, deviation %.3f\n", numPointsProbed, (double)mean, (double)deviation); - error = SaveHeightMap(gb, reply); - reprap.GetMove().AccessHeightMap().ExtrapolateMissing(); - reprap.GetMove().UseMesh(true); + ++gridYindex; } else { - reply.copy("Too few points probed"); - error = true; + ++gridXindex; } } - gb.SetState(GCodeState::normal); - break; + if (gridYindex == grid.NumYpoints()) + { + // Done all the points + gb.AdvanceState(); + if (platform.GetZProbeType() != 0 && !probeIsDeployed) + { + DoFileMacro(gb, RETRACTPROBE_G, false); + } + } + else + { + gb.SetState(GCodeState::gridProbing1); + } + } + break; + + case GCodeState::gridprobing6: + // Finished probing the grid, and retracted the probe if necessary + { + float mean, deviation; + const uint32_t numPointsProbed = reprap.GetMove().AccessHeightMap().GetStatistics(mean, deviation); + if (numPointsProbed >= 4) + { + reply.printf("%" PRIu32 " points probed, mean error %.3f, deviation %.3f\n", numPointsProbed, (double)mean, (double)deviation); + error = SaveHeightMap(gb, reply); + reprap.GetMove().AccessHeightMap().ExtrapolateMissing(); + reprap.GetMove().UseMesh(true); + } + else + { + reply.copy("Too few points probed"); + error = true; + } + } + gb.SetState(GCodeState::normal); + break; - // States used for G30 probing - case GCodeState::probingAtPoint0: - // Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position. + // States used for G30 probing + case GCodeState::probingAtPoint0: + // Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position. + moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; + moveBuffer.endStopsToCheck = 0; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = noFilePosition; + moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); + moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); + moveBuffer.xAxes = DefaultXAxisMapping; + moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; + segmentsLeft = 1; + gb.AdvanceState(); + break; + + case GCodeState::probingAtPoint1: + // The move to raise/lower the head to the correct dive height has been commanded. + if (LockMovementAndWaitForStandstill(gb)) + { + // Head is at the dive height but needs to be moved to the correct XY position. + // The XY coordinates have already been stored. moveBuffer.moveType = 0; moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; + (void)reprap.GetMove().GetProbeCoordinates(g30ProbePointIndex, moveBuffer.coords[X_AXIS], moveBuffer.coords[Y_AXIS], true); moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); moveBuffer.xAxes = DefaultXAxisMapping; moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; segmentsLeft = 1; + gb.AdvanceState(); - break; + } + break; - case GCodeState::probingAtPoint1: - // The move to raise/lower the head to the correct dive height has been commanded. - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::probingAtPoint2: + // Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been commanded. + // OR initial state when executing G30 with no P parameter + if (LockMovementAndWaitForStandstill(gb)) + { + // Head has finished moving to the correct XY position + lastProbedTime = millis(); // start the probe recovery timer + gb.AdvanceState(); + } + break; + + case GCodeState::probingAtPoint3: + // Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been completed and the recovery timer started. + // OR executing G30 without a P parameter, and the recovery timer has been started. + if (millis() - lastProbedTime >= (uint32_t)(platform.GetCurrentZProbeParameters().recoveryTime * SecondsToMillis)) + { + // The probe recovery time has elapsed, so we can start the probing move + if (platform.GetZProbeType() == 0) + { + // No Z probe, so we are doing manual 'probing' + UnlockAll(gb); // release the movement lock to allow manual Z moves + gb.AdvanceState(); // resume at the next state when the user has finished + doingManualBedProbe = true; // suspend the Z movement limit + DoManualProbe(gb); + } + else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) // check for probe already triggered at start + { + // Z probe is already triggered at the start of the move, so abandon the probe and record an error + platform.Message(ErrorMessage, "Z probe already triggered at start of probing move\n"); + if (g30ProbePointIndex >= 0) + { + reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, platform.GetZProbeDiveHeight(), true, true); + } + if (platform.GetZProbeType() != 0 && !probeIsDeployed) + { + DoFileMacro(gb, RETRACTPROBE_G, false); + } + gb.SetState(GCodeState::normal); // no point in doing anything else + } + else { - // Head is at the dive height but needs to be moved to the correct XY position. - // The XY coordinates have already been stored. + zProbeTriggered = false; + platform.SetProbing(true); moveBuffer.moveType = 0; moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = 0; + moveBuffer.endStopsToCheck = ZProbeActive; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; - (void)reprap.GetMove().GetProbeCoordinates(g30ProbePointIndex, moveBuffer.coords[X_AXIS], moveBuffer.coords[Y_AXIS], true); - moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); - moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); + moveBuffer.coords[Z_AXIS] = (GetAxisIsHomed(Z_AXIS)) + ? -platform.GetZProbeDiveHeight() // Z axis has been homed, so no point in going very far + : -1.1 * platform.AxisTotalLength(Z_AXIS); // Z axis not homed yet, so treat this as a homing move + moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed; moveBuffer.xAxes = DefaultXAxisMapping; moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; segmentsLeft = 1; - gb.AdvanceState(); } - break; + } + break; - case GCodeState::probingAtPoint2: - // Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been commanded. - // OR initial state when executing G30 with no P parameter - if (LockMovementAndWaitForStandstill(gb)) + case GCodeState::probingAtPoint4: + // Executing G30. The probe wasn't triggered at the start of the move, and the probing move has been commanded. + if (LockMovementAndWaitForStandstill(gb)) + { + // Probing move has stopped + doingManualBedProbe = false; + bool probingError = false; + if (platform.GetZProbeType() == 0) { - // Head has finished moving to the correct XY position - lastProbedTime = millis(); // start the probe recovery timer - gb.AdvanceState(); + // No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error. + g30zHeightError = moveBuffer.coords[Z_AXIS]; } - break; - - case GCodeState::probingAtPoint3: - // Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been completed and the recovery timer started. - // OR executing G30 without a P parameter, and the recovery timer has been started. - if (millis() - lastProbedTime >= (uint32_t)(platform.GetCurrentZProbeParameters().recoveryTime * SecondsToMillis)) + else { - // The probe recovery time has elapsed, so we can start the probing move - if (platform.GetZProbeType() == 0) + platform.SetProbing(false); + if (!zProbeTriggered) { - // No Z probe, so we are doing manual 'probing' - UnlockAll(gb); // release the movement lock to allow manual Z moves - gb.AdvanceState(); // resume at the next state when the user has finished - doingManualBedProbe = true; // suspend the Z movement limit - DoManualProbe(gb); - } - else if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) // check for probe already triggered at start - { - // Z probe is already triggered at the start of the move, so abandon the probe and record an error - platform.Message(ErrorMessage, "Z probe already triggered at start of probing move\n"); - if (g30ProbePointIndex >= 0) - { - reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, platform.GetZProbeDiveHeight(), true, true); - } - if (platform.GetZProbeType() != 0 && !probeIsDeployed) - { - DoFileMacro(gb, RETRACTPROBE_G, false); - } - gb.SetState(GCodeState::normal); // no point in doing anything else + platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n"); + g30zHeightError = 0.0; + probingError = true; } else { - zProbeTriggered = false; - platform.SetProbing(true); - moveBuffer.moveType = 0; - moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = ZProbeActive; - moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = noFilePosition; - moveBuffer.coords[Z_AXIS] = (GetAxisIsHomed(Z_AXIS)) - ? -platform.GetZProbeDiveHeight() // Z axis has been homed, so no point in going very far - : -1.1 * platform.AxisTotalLength(Z_AXIS); // Z axis not homed yet, so treat this as a homing move - moveBuffer.feedRate = platform.GetCurrentZProbeParameters().probeSpeed; - moveBuffer.xAxes = DefaultXAxisMapping; - moveBuffer.yAxes = DefaultYAxisMapping; - segmentsLeft = 1; - gb.AdvanceState(); + // Successful probing + float heightAdjust = 0.0; + bool dummy; + gb.TryGetFValue('H', heightAdjust, dummy); + float m[MaxAxes]; + reprap.GetMove().GetCurrentMachinePosition(m, false); // get height without bed compensation + g30zStoppedHeight = m[Z_AXIS] - heightAdjust; // save for later + g30zHeightError = g30zStoppedHeight - platform.ZProbeStopHeight(); } } - break; - case GCodeState::probingAtPoint4: - // Executing G30. The probe wasn't triggered at the start of the move, and the probing move has been commanded. - if (LockMovementAndWaitForStandstill(gb)) + if (g30ProbePointIndex < 0) { - // Probing move has stopped - doingManualBedProbe = false; - bool probingError = false; - if (platform.GetZProbeType() == 0) + // Simple G30 probing move + gb.SetState(GCodeState::normal); // usually nothing more to do except perhaps retract the probe + if (!probingError) { - // No Z probe, so we are doing manual mesh levelling. Take the current Z height as the height error. - g30zHeightError = moveBuffer.coords[Z_AXIS]; - } - else - { - platform.SetProbing(false); - if (!zProbeTriggered) + if (g30SValue == -1 || g30SValue == -2) { - platform.Message(ErrorMessage, "Z probe was not triggered during probing move\n"); - g30zHeightError = 0.0; - probingError = true; + // G30 S-1 or S-2 command + gb.SetState(GCodeState::probingAtPoint7); // special state for reporting the stopped height at the end } else { - // Successful probing - float heightAdjust = 0.0; - bool dummy; - gb.TryGetFValue('H', heightAdjust, dummy); - float m[MaxAxes]; - reprap.GetMove().GetCurrentMachinePosition(m, false); // get height without bed compensation - g30zStoppedHeight = m[Z_AXIS] - heightAdjust; // save for later - g30zHeightError = g30zStoppedHeight - platform.ZProbeStopHeight(); - } - } - - if (g30ProbePointIndex < 0) - { - // Simple G30 probing move - gb.SetState(GCodeState::normal); // usually nothing more to do except perhaps retract the probe - if (!probingError) - { - if (g30SValue == -1 || g30SValue == -2) - { - // G30 S-1 or S-2 command - gb.SetState(GCodeState::probingAtPoint7); // special state for reporting the stopped height at the end - } - else - { - // Reset the Z axis origin according to the height error - moveBuffer.coords[Z_AXIS] -= g30zHeightError; - reprap.GetMove().SetNewPosition(moveBuffer.coords, false); - ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); - SetAxisIsHomed(Z_AXIS); //TODO this is only correct if the Z axis is Cartesian-like! - } - } - - if (platform.GetZProbeType() != 0 && !probeIsDeployed) - { - DoFileMacro(gb, RETRACTPROBE_G, false); // retract the probe before moving to the new state - } - } - else - { - // Probing with a probe point index number - if (!GetAxisIsHomed(Z_AXIS) && !probingError) - { - // The Z axis has not yet been homed, so treat this probe as a homing move. - moveBuffer.coords[Z_AXIS] -= g30zHeightError; // reset the Z origin + // Reset the Z axis origin according to the height error + moveBuffer.coords[Z_AXIS] -= g30zHeightError; reprap.GetMove().SetNewPosition(moveBuffer.coords, false); ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); - SetAxisIsHomed(Z_AXIS); - g30zHeightError = 0.0; + SetAxisIsHomed(Z_AXIS); //TODO this is only correct if the Z axis is Cartesian-like! } - reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, g30zHeightError, true, probingError); - - // Move back up to the dive height before we change anything, in particular before we adjust leadscrews - moveBuffer.moveType = 0; - moveBuffer.isCoordinated = false; - moveBuffer.endStopsToCheck = 0; - moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = noFilePosition; - moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); - moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); - moveBuffer.xAxes = DefaultXAxisMapping; - moveBuffer.yAxes = DefaultYAxisMapping; - segmentsLeft = 1; - gb.AdvanceState(); } - } - break; - case GCodeState::probingAtPoint5: - // Here when we were probing at a numbered point and we have moved the head back up to the dive height - if (LockMovementAndWaitForStandstill(gb)) - { - gb.AdvanceState(); if (platform.GetZProbeType() != 0 && !probeIsDeployed) { - DoFileMacro(gb, RETRACTPROBE_G, false); + DoFileMacro(gb, RETRACTPROBE_G, false); // retract the probe before moving to the new state } } - break; - - case GCodeState::probingAtPoint6: - // Here when we have finished probing with a P parameter and have retracted the probe if necessary - if (LockMovementAndWaitForStandstill(gb)) // retracting the Z probe + else { - if (g30SValue == 1) + // Probing with a probe point index number + if (!GetAxisIsHomed(Z_AXIS) && !probingError) { - // G30 with a silly Z value and S=1 is equivalent to G30 with no parameters in that it sets the current Z height - // This is useful because it adjusts the XY position to account for the probe offset. - moveBuffer.coords[Z_AXIS] -= g30zHeightError; + // The Z axis has not yet been homed, so treat this probe as a homing move. + moveBuffer.coords[Z_AXIS] -= g30zHeightError; // reset the Z origin reprap.GetMove().SetNewPosition(moveBuffer.coords, false); ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); + SetAxisIsHomed(Z_AXIS); + g30zHeightError = 0.0; } - else if (g30SValue >= -1) - { - error = reprap.GetMove().FinishedBedProbing(g30SValue, reply); - } + reprap.GetMove().SetZBedProbePoint(g30ProbePointIndex, g30zHeightError, true, probingError); + + // Move back up to the dive height before we change anything, in particular before we adjust leadscrews + moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; + moveBuffer.endStopsToCheck = 0; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = noFilePosition; + moveBuffer.coords[Z_AXIS] = platform.GetZProbeStartingHeight(); + moveBuffer.feedRate = platform.GetZProbeTravelSpeed(); + moveBuffer.xAxes = DefaultXAxisMapping; + moveBuffer.yAxes = DefaultYAxisMapping; + totalSegments = 1; + segmentsLeft = 1; + gb.AdvanceState(); } + } + break; - gb.SetState(GCodeState::normal); - break; + case GCodeState::probingAtPoint5: + // Here when we were probing at a numbered point and we have moved the head back up to the dive height + if (LockMovementAndWaitForStandstill(gb)) + { + gb.AdvanceState(); + if (platform.GetZProbeType() != 0 && !probeIsDeployed) + { + DoFileMacro(gb, RETRACTPROBE_G, false); + } + } + break; - case GCodeState::probingAtPoint7: - // Here when we have finished executing G30 S-1 or S-2 including retracting the probe if necessary - if (g30SValue == -2) + case GCodeState::probingAtPoint6: + // Here when we have finished probing with a P parameter and have retracted the probe if necessary + if (LockMovementAndWaitForStandstill(gb)) // retracting the Z probe + { + if (g30SValue == 1) { - // Adjust the Z offset of the current tool to account for the height error - Tool *const tool = reprap.GetCurrentTool(); - if (tool == nullptr) - { - platform.Message(ErrorMessage, "G30 S-2 commanded with no tool selected"); - error = true; - } - else - { - const float *offsets = tool->GetOffsets(); - tool->SetOffset(Z_AXIS, offsets[Z_AXIS] + g30zHeightError); - } + // G30 with a silly Z value and S=1 is equivalent to G30 with no parameters in that it sets the current Z height + // This is useful because it adjusts the XY position to account for the probe offset. + moveBuffer.coords[Z_AXIS] -= g30zHeightError; + reprap.GetMove().SetNewPosition(moveBuffer.coords, false); + ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); + } + else if (g30SValue >= -1) + { + error = reprap.GetMove().FinishedBedProbing(g30SValue, reply); + } + } + + gb.SetState(GCodeState::normal); + break; + + case GCodeState::probingAtPoint7: + // Here when we have finished executing G30 S-1 or S-2 including retracting the probe if necessary + if (g30SValue == -2) + { + // Adjust the Z offset of the current tool to account for the height error + Tool *const tool = reprap.GetCurrentTool(); + if (tool == nullptr) + { + platform.Message(ErrorMessage, "G30 S-2 commanded with no tool selected"); + error = true; } else { - // Just print the stop height - reply.printf("Stopped at height %.3f mm", (double)g30zStoppedHeight); + const float *offsets = tool->GetOffsets(); + tool->SetOffset(Z_AXIS, offsets[Z_AXIS] + g30zHeightError, true); + } + } + else + { + // Just print the stop height + reply.printf("Stopped at height %.3f mm", (double)g30zStoppedHeight); + } + gb.SetState(GCodeState::normal); + break; + + // Firmware retraction/un-retraction states + case GCodeState::doingFirmwareRetraction: + // We just did the retraction part of a firmware retraction, now we need to do the Z hop + if (segmentsLeft == 0) + { + const AxesBitmap xAxes = reprap.GetCurrentXAxes(); + const AxesBitmap yAxes = reprap.GetCurrentYAxes(); + reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes); + for (size_t i = numTotalAxes; i < DRIVES; ++i) + { + moveBuffer.coords[i] = 0.0; } + moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS); + moveBuffer.coords[Z_AXIS] += retractHop; + currentZHop = retractHop; + moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; + moveBuffer.isFirmwareRetraction = true; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; + moveBuffer.canPauseAfter = false; // don't pause after a retraction because that could cause too much retraction + moveBuffer.xAxes = xAxes; + moveBuffer.yAxes = yAxes; + totalSegments = 1; + segmentsLeft = 1; gb.SetState(GCodeState::normal); - break; + } + break; - // Firmware retraction/un-retraction states - case GCodeState::doingFirmwareRetraction: - // We just did the retraction part of a firmware retraction, now we need to do the Z hop - if (segmentsLeft == 0) + case GCodeState::doingFirmwareUnRetraction: + // We just undid the Z-hop part of a firmware un-retraction, now we need to do the un-retract + if (segmentsLeft == 0) + { + const Tool * const tool = reprap.GetCurrentTool(); + if (tool != nullptr) { - const AxesBitmap xAxes = reprap.GetCurrentXAxes(); - const AxesBitmap yAxes = reprap.GetCurrentYAxes(); + const uint32_t xAxes = reprap.GetCurrentXAxes(); + const uint32_t yAxes = reprap.GetCurrentYAxes(); reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes); for (size_t i = numTotalAxes; i < DRIVES; ++i) { moveBuffer.coords[i] = 0.0; } - moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS); - moveBuffer.coords[Z_AXIS] += retractHop; - currentZHop = retractHop; + for (size_t i = 0; i < tool->DriveCount(); ++i) + { + moveBuffer.coords[numTotalAxes + tool->Drive(i)] = retractLength + retractExtra; + } + moveBuffer.feedRate = unRetractSpeed; moveBuffer.moveType = 0; moveBuffer.isCoordinated = false; moveBuffer.isFirmwareRetraction = true; moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; - moveBuffer.canPauseAfter = false; // don't pause after a retraction because that could cause too much retraction + moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition; + moveBuffer.canPauseAfter = true; moveBuffer.xAxes = xAxes; moveBuffer.yAxes = yAxes; + totalSegments = 1; segmentsLeft = 1; - gb.SetState(GCodeState::normal); } - break; - - case GCodeState::doingFirmwareUnRetraction: - // We just undid the Z-hop part of a firmware un-retraction, now we need to do the un-retract - if (segmentsLeft == 0) - { - const Tool * const tool = reprap.GetCurrentTool(); - if (tool != nullptr) - { - const uint32_t xAxes = reprap.GetCurrentXAxes(); - const uint32_t yAxes = reprap.GetCurrentYAxes(); - reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes); - for (size_t i = numTotalAxes; i < DRIVES; ++i) - { - moveBuffer.coords[i] = 0.0; - } - for (size_t i = 0; i < tool->DriveCount(); ++i) - { - moveBuffer.coords[numTotalAxes + tool->Drive(i)] = retractLength + retractExtra; - } - moveBuffer.feedRate = unRetractSpeed; - moveBuffer.moveType = 0; - moveBuffer.isCoordinated = false; - moveBuffer.isFirmwareRetraction = true; - moveBuffer.usePressureAdvance = false; - moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition; - moveBuffer.canPauseAfter = true; - moveBuffer.xAxes = xAxes; - moveBuffer.yAxes = yAxes; - segmentsLeft = 1; - } - gb.SetState(GCodeState::normal); - } - break; + gb.SetState(GCodeState::normal); + } + break; - case GCodeState::loadingFilament: - // We just returned from the filament load macro - if (reprap.GetCurrentTool() != nullptr) + case GCodeState::loadingFilament: + // We just returned from the filament load macro + if (reprap.GetCurrentTool() != nullptr) + { + reprap.GetCurrentTool()->GetFilament()->Load(filamentToLoad); + if (reprap.Debug(moduleGcodes)) { - reprap.GetCurrentTool()->GetFilament()->Load(filamentToLoad); - if (reprap.Debug(moduleGcodes)) - { - platform.MessageF(LoggedGenericMessage, "Filament %s loaded", filamentToLoad); - } + platform.MessageF(LoggedGenericMessage, "Filament %s loaded", filamentToLoad); } - gb.SetState(GCodeState::normal); - break; + } + gb.SetState(GCodeState::normal); + break; - case GCodeState::unloadingFilament: - // We just returned from the filament unload macro - if (reprap.GetCurrentTool() != nullptr) + case GCodeState::unloadingFilament: + // We just returned from the filament unload macro + if (reprap.GetCurrentTool() != nullptr) + { + if (reprap.Debug(moduleGcodes)) { - if (reprap.Debug(moduleGcodes)) - { - platform.MessageF(LoggedGenericMessage, "Filament %s unloaded", reprap.GetCurrentTool()->GetFilament()->GetName()); - } - reprap.GetCurrentTool()->GetFilament()->Unload(); + platform.MessageF(LoggedGenericMessage, "Filament %s unloaded", reprap.GetCurrentTool()->GetFilament()->GetName()); } - gb.SetState(GCodeState::normal); - break; - - default: // should not happen - platform.Message(ErrorMessage, "Undefined GCodeState\n"); - gb.SetState(GCodeState::normal); - break; + reprap.GetCurrentTool()->GetFilament()->Unload(); } + gb.SetState(GCodeState::normal); + break; - if (gb.GetState() == GCodeState::normal) +#if HAS_VOLTAGE_MONITOR + case GCodeState::powerFailPausing1: + if (gb.IsReady() || gb.IsExecuting()) { - // We completed a command, so unlock resources and tell the host about it - gb.timerRunning = false; - UnlockAll(gb); - HandleReply(gb, error, reply.Pointer()); + gb.SetFinished(ActOnCode(gb, reply)); // execute the pause script } - } + else + { + SaveResumeInfo(true); // create the resume file so that we can resume after power down + platform.Message(LoggedGenericMessage, "Print auto-paused due to low voltage\n"); + gb.SetState(GCodeState::normal); + } + break; +#endif - // Move on to the next gcode source ready for next time - ++nextGcodeSource; - if (nextGcodeSource == ARRAY_SIZE(gcodeSources)) - { - nextGcodeSource = 0; + default: // should not happen + platform.Message(ErrorMessage, "Undefined GCodeState\n"); + gb.SetState(GCodeState::normal); + break; } - // Check if we need to display a warning - const uint32_t now = millis(); - if (now - lastWarningMillis >= MinimumWarningInterval) + if (gb.GetState() == GCodeState::normal) { - if (displayNoToolWarning) - { - platform.Message(ErrorMessage, "Attempting to extrude with no tool selected.\n"); - displayNoToolWarning = false; - lastWarningMillis = now; - } - if (displayDeltaNotHomedWarning) - { - platform.Message(ErrorMessage, "Attempt to move the head of a Delta or SCARA printer before homing it\n"); - displayDeltaNotHomedWarning = false; - lastWarningMillis = now; - } + // We completed a command, so unlock resources and tell the host about it + gb.timerRunning = false; + UnlockAll(gb); + HandleReply(gb, error, reply.Pointer()); } - platform.ClassReport(longWait); } // Start a new gcode, or continue to execute one that has already been started: @@ -1350,7 +1392,7 @@ void GCodes::CheckTriggers() else if (LockMovement(*daemonGCode)) // need to lock movement before executing the pause macro { ClearBit(triggersPending, lowestTriggerPending); // clear the trigger - DoPause(*daemonGCode, false); + DoPause(*daemonGCode, PauseReason::trigger, nullptr); } } else @@ -1362,15 +1404,18 @@ void GCodes::CheckTriggers() DoFileMacro(*daemonGCode, filename.Pointer(), true); } } - else if ( lastFilamentError != FilamentSensorStatus::ok // check for a filament error - && reprap.GetPrintMonitor().IsPrinting() - && !isPaused - && LockMovement(*daemonGCode) // need to lock movement before executing the pause macro - ) +} + +// Check for and respond to filament errors +void GCodes::CheckFilament() +{ + if ( lastFilamentError != FilamentSensorStatus::ok // check for a filament error + && reprap.GetPrintMonitor().IsPrinting() + && !isPaused + && LockMovement(*autoPauseGCode) // need to lock movement before executing the pause macro + ) { - scratchString.printf("Extruder %u reports %s", lastFilamentErrorExtruder, FilamentSensor::GetErrorMessage(lastFilamentError)); - platform.SendAlert(GenericMessage, scratchString.Pointer(), "Filament error", 1, 0.0, 0); - DoPause(*daemonGCode, false); + DoPause(*autoPauseGCode, PauseReason::filament, "Extruder %u reports %s", lastFilamentErrorExtruder, FilamentSensor::GetErrorMessage(lastFilamentError)); lastFilamentError = FilamentSensorStatus::ok; } } @@ -1394,7 +1439,7 @@ void GCodes::DoEmergencyStop() } // Pause the print. Before calling this, check that we are doing a file print that isn't already paused and get the movement lock. -void GCodes::DoPause(GCodeBuffer& gb, bool isAuto) +void GCodes::DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg, ...) { if (&gb == fileGCode) { @@ -1403,18 +1448,14 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto) } else { - // Pausing a file print via another input source + // Pausing a file print via another input source or for some other reason const bool movesSkipped = reprap.GetMove().PausePrint(pauseRestorePoint); // tell Move we wish to pause the current print if (movesSkipped) { // The PausePrint call has filled in the restore point with machine coordinates ToolOffsetInverseTransform(pauseRestorePoint.moveCoords, currentUserPosition); // transform the returned coordinates to user coordinates - - if (segmentsLeft != 0) - { - ClearMove(); - } + ClearMove(); } else if (segmentsLeft != 0 && moveBuffer.canPauseBefore) { @@ -1442,14 +1483,12 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto) #endif } - // Replace the pauses machine coordinates by user coordinates, which we updated earlier + // Replace the paused machine coordinates by user coordinates, which we updated earlier for (size_t axis = 0; axis < numVisibleAxes; ++axis) { pauseRestorePoint.moveCoords[axis] = currentUserPosition[axis]; } - pauseRestorePoint.virtualExtruderPosition = virtualExtruderPosition; - // If we skipped any moves, reset the file pointer to the start of the first move we need to replay // The following could be delayed until we resume the print FileData& fdata = fileGCode->MachineState().fileState; @@ -1470,15 +1509,20 @@ void GCodes::DoPause(GCodeBuffer& gb, bool isAuto) SaveFanSpeeds(); if (simulationMode == 0) { - SaveResumeInfo(); // create the resume file so that we can resume after power down + SaveResumeInfo(false); // create the resume file so that we can resume after power down } gb.SetState(GCodeState::pausing1); isPaused = true; -#ifdef DUET_NG - isAutoPaused = isAuto; -#endif + if (msg != nullptr) + { + va_list vargs; + va_start(vargs, msg); + scratchString.vprintf(msg, vargs); + va_end(vargs); + platform.SendAlert(GenericMessage, scratchString.Pointer(), "Printing paused", 1, 0.0, 0); + } } bool GCodes::IsPaused() const @@ -1488,8 +1532,19 @@ bool GCodes::IsPaused() const bool GCodes::IsPausing() const { - const GCodeState topState = fileGCode->OriginalMachineState().state; - return topState == GCodeState::pausing1 || topState == GCodeState::pausing2; + GCodeState topState = fileGCode->OriginalMachineState().state; + if (topState == GCodeState::pausing1 || topState == GCodeState::pausing2) + { + return true; + } +#if HAS_VOLTAGE_MONITOR + topState = autoPauseGCode->OriginalMachineState().state; + if (topState == GCodeState::powerFailPausing1) + { + return true; + } +#endif + return false; } bool GCodes::IsResuming() const @@ -1503,54 +1558,114 @@ bool GCodes::IsRunning() const return !IsPaused() && !IsPausing() && !IsResuming(); } -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Try to pause the current SD card print, returning true if successful, false if needs to be called again -bool GCodes::AutoPause() +bool GCodes::LowVoltagePause() { - if (IsPausing() || IsResuming()) + if (simulationMode != 0) { + return true; // ignore the low voltage indication + } + + reprap.GetHeat().SuspendHeaters(true); // turn heaters off to conserve power for the motors to execute the pause + if (IsResuming()) + { + // This is an unlucky situation, because the resume macro is probably being run, which will probably lower the head back on to the print. + // It may well be that the power loss will prevent the resume macro being completed. If not, try again when the print has been resumed. return false; } - if (!isPaused && reprap.GetPrintMonitor().IsPrinting()) + if (IsPausing()) { - if (!LockMovement(*autoPauseGCode)) - { - return false; - } - reprap.GetHeat().SuspendHeaters(true); // turn heaters off to conserve power for the motors to execute the pause - DoPause(*autoPauseGCode, true); + // We are in the process of pausing already, so the resume info has already been saved. + // With luck the retraction and lifting of the head in the pause.g file has been done already. + return true; } - return true; -} -// Suspend the printer, only ever called after it has been auto paused -bool GCodes::AutoShutdown() -{ - if (isPaused && isAutoPaused) + if (IsPaused()) + { + // Resume info has already been saved, and resuming will be prevented while the power is low + return true; + } + + if (reprap.GetPrintMonitor().IsPrinting()) { - if (!LockMovementAndWaitForStandstill(*autoPauseGCode)) + if (!autoPauseGCode->IsIdle() || autoPauseGCode->OriginalMachineState().state != GCodeState::normal) { - return false; + return false; // we can't pause if the auto pause thread is busy already } - CancelPrint(false, false); - platform.Message(WarningMessage, "Print cancelled due to low voltage\n"); - return true; + + // Save the resume info, stop movement immediately and run the low voltage pause script to lift the nozzle etc. + GrabMovement(*autoPauseGCode); + + const bool movesSkipped = reprap.GetMove().LowPowerPause(pauseRestorePoint); + if (movesSkipped) + { + // The PausePrint call has filled in the restore point with machine coordinates + ToolOffsetInverseTransform(pauseRestorePoint.moveCoords, currentUserPosition); // transform the returned coordinates to user coordinates + ClearMove(); + } + else if (segmentsLeft != 0 && moveBuffer.filePos != noFilePosition) + { + // We were not able to skip any moves, however we can skip the remaining segments of this current move + ToolOffsetInverseTransform(moveBuffer.initialCoords, currentUserPosition); + pauseRestorePoint.feedRate = moveBuffer.feedRate; + pauseRestorePoint.virtualExtruderPosition = moveBuffer.virtualExtruderPosition; + pauseRestorePoint.filePos = moveBuffer.filePos; + pauseRestorePoint.proportionDone = (float)(totalSegments - segmentsLeft)/(float)totalSegments; +#if SUPPORT_IOBITS + pauseRestorePoint.ioBits = moveBuffer.ioBits; +#endif + ClearMove(); + } + else + { + // We were not able to skip any moves, and if there is a move waiting then we can't skip that one either + pauseRestorePoint.feedRate = fileGCode->MachineState().feedrate; + pauseRestorePoint.virtualExtruderPosition = virtualExtruderPosition; + + // TODO: when we use RTOS there is a possible race condition in the following, + // because we might try to pause when a waiting move has just been added but before the gcode buffer has been re-initialised ready for the next command + pauseRestorePoint.filePos = fileGCode->GetFilePosition(fileInput->BytesCached()); + pauseRestorePoint.proportionDone = 0.0; +#if SUPPORT_IOBITS + pauseRestorePoint.ioBits = moveBuffer.ioBits; +#endif + } + + // Replace the paused machine coordinates by user coordinates, which we updated earlier + for (size_t axis = 0; axis < numVisibleAxes; ++axis) + { + pauseRestorePoint.moveCoords[axis] = currentUserPosition[axis]; + } + + SaveFanSpeeds(); + + // Run the auto-pause script + if (powerFailScript != nullptr) + { + autoPauseGCode->Put(powerFailScript, strlen(powerFailScript) + 1); + } + autoPauseGCode->SetState(GCodeState::powerFailPausing1); + isPaused = true; + isPowerFailPaused = true; + + // Don't do any more here, we want the auto pause thread to run as soon as possible } + return true; } -// Resume printing, normally only ever called after it has been auto paused -bool GCodes::AutoResume() +// Resume printing, normally only ever called after it has been paused because if low voltage. +// If the pause was short enough, resume automatically. +bool GCodes::LowVoltageResume() { - if (isPaused && isAutoPaused) + reprap.GetHeat().SuspendHeaters(false); // turn heaters off to conserve power for the motors to execute the pause + if (isPaused && isPowerFailPaused) { - autoPauseGCode->SetState(GCodeState::resuming1); - if (AllAxesAreHomed()) - { - DoFileMacro(*autoPauseGCode, POWER_RESTORE_G, true); - } + // Run resurrect.g automatically + //TODO qq; platform.Message(LoggedGenericMessage, "Print auto-resumed\n"); } return true; @@ -1564,7 +1679,7 @@ bool GCodes::AutoResumeAfterShutdown() } #endif -void GCodes::SaveResumeInfo() +void GCodes::SaveResumeInfo(bool wasPowerFailure) { const char* const printingFilename = reprap.GetPrintMonitor().GetPrintingFilename(); if (printingFilename != nullptr) @@ -1580,7 +1695,7 @@ void GCodes::SaveResumeInfo() StringRef buf(bufferSpace, ARRAY_SIZE(bufferSpace)); // Write the header comment - buf.printf("; File \"%s\" resume print after auto-pause", printingFilename); + buf.printf("; File \"%s\" resume print after %s", printingFilename, (wasPowerFailure) ? "power failure" : "print paused"); if (reprap.GetPlatform().IsDateTimeSet()) { time_t timeNow = reprap.GetPlatform().GetDateTime(); @@ -1606,8 +1721,8 @@ void GCodes::SaveResumeInfo() } if (ok) { - buf.printf("M116\nM290 S%.3f\nM23 %s\nM26 S%" PRIu32 "\n", (double)currentBabyStepZOffset, printingFilename, pauseRestorePoint.filePos); - ok = f->Write(buf.Pointer()); // write baby stepping offset, filename and file position + buf.printf("M116\nM290 S%.3f\n", (double)currentBabyStepZOffset); + ok = f->Write(buf.Pointer()); // write baby stepping offset } if (ok && fileGCode->OriginalMachineState().volumetricExtrusion) { @@ -1628,9 +1743,14 @@ void GCodes::SaveResumeInfo() } if (ok) { + buf.printf("M23 %s\nM26 S%" PRIu32 " P%.3f\n", printingFilename, pauseRestorePoint.filePos, (double)pauseRestorePoint.proportionDone); + ok = f->Write(buf.Pointer()); // write filename and file position + } + if (ok) + { // Build the commands to restore the head position. These assume that we are working in mm. // Start with a vertical move to 2mm above the final Z position - buf.printf("G0 F1200 Z%.3f\n", (double)(pauseRestorePoint.moveCoords[Z_AXIS] + 2.0)); + buf.printf("G0 F6000 Z%.3f\n", (double)(pauseRestorePoint.moveCoords[Z_AXIS] + 2.0)); // Now set all the other axes buf.cat("G0 F6000"); @@ -1643,7 +1763,7 @@ void GCodes::SaveResumeInfo() } // Now move down to the correct Z height - buf.catf("\nG0 F1200 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]); + buf.catf("\nG0 F6000 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]); // Set the feed rate buf.catf("G1 F%.1f", (double)(pauseRestorePoint.feedRate * MinutesToSeconds)); @@ -1999,12 +2119,12 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate if (moveBuffer.moveType != 0) { // It's a raw motor move, so do it in a single segment and wait for it to complete - segmentsLeft = 1; + totalSegments = 1; gb.SetState(GCodeState::waitingForSpecialMoveToComplete); } else if (axesMentioned == 0) { - segmentsLeft = 1; // extruder-only movement + totalSegments = 1; } else { @@ -2014,7 +2134,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate { ClearBit(effectiveAxesHomed, Z_AXIS); // if doing a manual Z probe, don't limit the Z movement } - if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, effectiveAxesHomed)) + if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, effectiveAxesHomed, moveBuffer.isCoordinated)) { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -2025,7 +2145,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate { AxesBitmap axesMentionedExceptZ = axesMentioned; ClearBit(axesMentionedExceptZ, Z_AXIS); - moveBuffer.usePressureAdvance = (axesMentionedExceptZ != 0); + moveBuffer.usePressureAdvance = moveBuffer.hasExtrusion && (axesMentionedExceptZ != 0); } // Apply segmentation if necessary. To speed up simulation on SCARA printers, we don't apply kinematics segmentation when simulating. @@ -2037,19 +2157,20 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinate // We assume that the segments will be smaller than the mesh spacing. const float xyLength = sqrtf(fsquare(currentUserPosition[X_AXIS] - initialX) + fsquare(currentUserPosition[Y_AXIS] - initialY)); const float moveTime = xyLength/moveBuffer.feedRate; // this is a best-case time, often the move will take longer - segmentsLeft = (unsigned int)max<int>(1, min<int>(rintf(xyLength/kin.GetMinSegmentLength()), rintf(moveTime * kin.GetSegmentsPerSecond()))); + totalSegments = (unsigned int)max<int>(1, min<int>(rintf(xyLength/kin.GetMinSegmentLength()), rintf(moveTime * kin.GetSegmentsPerSecond()))); } else if (reprap.GetMove().IsUsingMesh()) { const HeightMap& heightMap = reprap.GetMove().AccessHeightMap(); - segmentsLeft = max<unsigned int>(1, heightMap.GetMinimumSegments(currentUserPosition[X_AXIS] - initialX, currentUserPosition[Y_AXIS] - initialY)); + totalSegments = max<unsigned int>(1, heightMap.GetMinimumSegments(currentUserPosition[X_AXIS] - initialX, currentUserPosition[Y_AXIS] - initialY)); } else { - segmentsLeft = 1; + totalSegments = 1; } } + segmentsLeft = totalSegments; return false; } @@ -2102,7 +2223,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) } ToolOffsetTransform(currentUserPosition, moveBuffer.coords); // set the final position - if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed)) + if (limitAxes && reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, true)) { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -2118,7 +2239,6 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) moveBuffer.yAxes = reprap.GetCurrentYAxes(); moveBuffer.virtualExtruderPosition = virtualExtruderPosition; moveBuffer.endStopsToCheck = 0; - moveBuffer.usePressureAdvance = true; moveBuffer.isCoordinated = true; moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; @@ -2138,6 +2258,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) } LoadExtrusionAndFeedrateFromGCode(gb, moveBuffer.moveType); + moveBuffer.usePressureAdvance = moveBuffer.hasExtrusion; arcRadius = sqrtf(iParam * iParam + jParam * jParam); arcCurrentAngle = atan2(-jParam, -iParam); @@ -2150,15 +2271,15 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) } // Compute how many segments we need to move, but don't store it yet - const unsigned int segsLeft = max<unsigned int>((unsigned int)((arcRadius * totalArc)/arcSegmentLength + 0.8), 1u); - arcAngleIncrement = totalArc/segsLeft; + totalSegments = max<unsigned int>((unsigned int)((arcRadius * totalArc)/arcSegmentLength + 0.8), 1u); + arcAngleIncrement = totalArc/totalSegments; if (clockwise) { arcAngleIncrement = -arcAngleIncrement; } doingArcMove = true; - segmentsLeft = segsLeft; // must do this last for RTOS + segmentsLeft = totalSegments; // must do this last for RTOS // debugPrintf("Radius %.2f, initial angle %.1f, increment %.1f, segments %u\n", // arcRadius, arcCurrentAngle * RadiansToDegrees, arcAngleIncrement * RadiansToDegrees, segmentsLeft); return false; @@ -2178,12 +2299,14 @@ bool GCodes::ReadMove(RawMove& m) { // If there is just 1 segment left, it doesn't matter if it is an arc move or not, just move to the end position ClearMove(); + m.proportionRemaining = 0; } else { // This move needs to be divided into 2 or more segments - m.canPauseAfter = false; // we can only pause after the final segment - moveBuffer.canPauseBefore = false; // we can't pause before any of the remaining segments + m.canPauseAfter = false; // we can only do a controlled pause pause after the final segment + moveBuffer.canPauseBefore = false; // we can't do a controlled pause before any of the remaining segments + m.proportionRemaining = (uint8_t)min<unsigned int>((((totalSegments - segmentsLeft - 1) * 256) + (totalSegments/2))/totalSegments, 255); // Do the axes if (doingArcMove) @@ -2223,6 +2346,11 @@ bool GCodes::ReadMove(RawMove& m) } --segmentsLeft; + if ((unsigned int)moveFractionToSkip + (unsigned int)m.proportionRemaining > 256) + { + // We are resuming a print part way through a move and we printed this segment already + return false; + } } return true; @@ -2230,16 +2358,17 @@ bool GCodes::ReadMove(RawMove& m) void GCodes::ClearMove() { + segmentsLeft = 0; doingArcMove = false; moveBuffer.endStopsToCheck = 0; moveBuffer.moveType = 0; moveBuffer.isFirmwareRetraction = false; - segmentsLeft = 0; // do this last + moveFractionToSkip = 0; } // Run a file macro. Prior to calling this, 'state' must be set to the state we want to enter when the macro has been completed. // Return true if the file was found or it wasn't and we were asked to report that fact. -bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, bool runningM502) +bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning) { FileStore * const f = platform.GetFileStore(platform.GetSysDir(), fileName, OpenMode::read); if (f == nullptr) @@ -2259,7 +2388,8 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi } gb.MachineState().fileState.Set(f); gb.MachineState().doingFileMacro = true; - gb.MachineState().runningM502 = runningM502; + gb.MachineState().runningM501 = (codeRunning == 501); + gb.MachineState().runningM502 = (codeRunning == 502); gb.SetState(GCodeState::normal); gb.Init(); return true; @@ -2297,7 +2427,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb) } } SetBit(axesIncluded, axis); - currentUserPosition[axis] = axisValue; + currentUserPosition[axis] = axisValue * distanceScale; } } @@ -2310,7 +2440,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb) if (axesIncluded != 0) { ToolOffsetTransform(currentUserPosition, moveBuffer.coords); - if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, LowestNBits<AxesBitmap>(numVisibleAxes))) // pretend that all axes are homed + if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, LowestNBits<AxesBitmap>(numVisibleAxes), false)) // pretend that all axes are homed { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -2635,13 +2765,13 @@ GCodeResult GCodes::ProbeGrid(GCodeBuffer& gb, StringRef& reply) { if (!defaultGrid.IsValid()) { - reply.copy("No valid grid defined for G29 bed probing"); + reply.copy("No valid grid defined for bed probing"); return GCodeResult::error; } if (!AllAxesAreHomed()) { - reply.copy("Must home printer before G29 bed probing"); + reply.copy("Must home printer before bed probing"); return GCodeResult::error; } @@ -2750,11 +2880,12 @@ void GCodes::GetCurrentCoordinates(StringRef& s) const s.Clear(); for (size_t axis = 0; axis < numVisibleAxes; ++axis) { - s.catf("%c: %.3f ", axisLetters[axis], (double)liveCoordinates[axis]); + // Don't put a space after the colon in the response, it confuses Pronterface + s.catf("%c:%.3f ", axisLetters[axis], (double)liveCoordinates[axis]); } for (size_t i = numTotalAxes; i < DRIVES; i++) { - s.catf("E%u: %.1f ", i - numTotalAxes, (double)liveCoordinates[i]); + s.catf("E%u:%.1f ", i - numTotalAxes, (double)liveCoordinates[i]); } // Print the axis stepper motor positions as Marlin does, as an aid to debugging. @@ -2907,6 +3038,7 @@ bool GCodes::QueueFileToPrint(const char* fileName, StringRef& reply) fileToPrint.Set(f); fileOffsetToPrint = 0; + moveFractionToStartAt = 0; return true; } @@ -3338,7 +3470,7 @@ void GCodes::HandleReply(GCodeBuffer& gb, bool error, const char* reply) const Compatibility c = (&gb == serialGCode || &gb == telnetGCode) ? platform.Emulating() : me; const MessageType type = gb.GetResponseMessageType(); - const char* const response = (gb.GetCommandLetter() == 'M' && gb.GetIValue() == 998) ? "rs " : "ok"; + const char* const response = (gb.GetCommandLetter() == 'M' && gb.GetCommandNumber() == 998) ? "rs " : "ok"; const char* emulationType = nullptr; switch (c) @@ -4123,10 +4255,17 @@ bool GCodes::WriteConfigOverrideFile(StringRef& reply, const char *fileName) con { ok = reprap.GetHeat().WriteModelParameters(f); } + + if (ok) + { + ok = platform.WritePlatformParameters(f); + } + if (ok) { - ok = platform.WriteZProbeParameters(f); + ok = reprap.WriteToolParameters(f); } + if (!f->Close()) { ok = false; @@ -4255,6 +4394,26 @@ bool GCodes::LockResource(const GCodeBuffer& gb, Resource r) return false; } +// Grab the movement lock even if another GCode source has it +// CAUTION: this will be unsafe when we move to RTOS +void GCodes::GrabResource(const GCodeBuffer& gb, Resource r) +{ + if (resourceOwners[r] != &gb) + { + if (resourceOwners[r] != nullptr) + { + GCodeMachineState *m = &(resourceOwners[r]->MachineState()); + do + { + ClearBit(m->lockedResources, r); + m = m->previous; + } + while (m != nullptr); + } + resourceOwners[r] = &gb; + } +} + bool GCodes::LockHeater(const GCodeBuffer& gb, int heater) { if (heater >= 0 && heater < (int)Heaters) @@ -4285,6 +4444,11 @@ bool GCodes::LockMovement(const GCodeBuffer& gb) return LockResource(gb, MoveResource); } +void GCodes::GrabMovement(const GCodeBuffer& gb) +{ + GrabResource(gb, MoveResource); +} + // Release all locks, except those that were owned when the current macro was started void GCodes::UnlockAll(const GCodeBuffer& gb) { @@ -4328,4 +4492,13 @@ const char* GCodes::GetMachineModeString() const } } +// Respond to a heater fault. The heater has already been turned off and its status set to 'fault' when this is called. +void GCodes::HandleHeaterFault(int heater) +{ + if (reprap.GetPrintMonitor().IsPrinting() && !isPaused) + { + DoPause(*autoPauseGCode, PauseReason::heaterFault, "Fault on heater %d", heater); + } +} + // End diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h index aa06db0a..bbe13e46 100644 --- a/src/GCodes/GCodes.h +++ b/src/GCodes/GCodes.h @@ -41,6 +41,7 @@ typedef AxesBitmap EndstopChecks; // must be large enough to hold a bitmap const EndstopChecks ZProbeActive = 1 << 31; // must be distinct from 1 << (any drive number) const EndstopChecks HomeAxes = 1 << 30; // must be distinct from 1 << (any drive number) const EndstopChecks LogProbeChanges = 1 << 29; // must be distinct from 1 << (any drive number) +const EndstopChecks UseSpecialEndstop = 1 << 28; // must be distinct from 1 << (any drive number) typedef uint32_t TriggerInputsBitmap; // Bitmap of input pins that a single trigger number responds to typedef uint32_t TriggerNumbersBitmap; // Bitmap of trigger numbers @@ -64,10 +65,10 @@ struct Trigger }; // Bits for T-code P-parameter to specify which macros are supposed to be run -const int TFreeBit = 1 << 0; -const int TPreBit = 1 << 1; -const int TPostBit = 1 << 2; -const int DefaultToolChangeParam = TFreeBit | TPreBit | TPostBit; +constexpr uint8_t TFreeBit = 1 << 0; +constexpr uint8_t TPreBit = 1 << 1; +constexpr uint8_t TPostBit = 1 << 2; +constexpr uint8_t DefaultToolChangeParam = TFreeBit | TPreBit | TPostBit; // Machine type enumeration. The numeric values must be in the same order as the corresponding M451..M453 commands. enum class MachineType : uint8_t @@ -77,6 +78,21 @@ enum class MachineType : uint8_t cnc = 2 }; +enum class PauseReason +{ + user, // M25 command received + gcode, // M25 or M226 command encountered in the file being printed + trigger, // external switch + heaterFault, // heater fault detected + filament, // filament monitor +#if HAS_SMART_DRIVERS + stall, // motor stall detected +#endif +#if HAS_VOLTAGE_MONITOR + lowVoltage // VIN voltage dropped below configured minimum +#endif +}; + //**************************************************************************************************** // The GCode interpreter @@ -98,6 +114,8 @@ public: IoBits_t ioBits; // I/O bits to set/clear at the start of this move #endif uint8_t moveType; // the S parameter from the G0 or G1 command, 0 for a normal move + uint8_t proportionRemaining; // what proportion of the entire move remains after this segment + uint8_t isFirmwareRetraction : 1; // true if this is a firmware retraction/un-retraction move uint8_t usePressureAdvance : 1; // true if we want to us extruder pressure advance, if there is any extrusion uint8_t canPauseBefore : 1; // true if we can pause before this move @@ -161,11 +179,11 @@ public: size_t GetNumExtruders() const { return numExtruders; } void FilamentError(size_t extruder, FilamentSensorStatus fstat); + void HandleHeaterFault(int heater); // Respond to a heater fault -#ifdef DUET_NG - bool AutoPause(); - bool AutoShutdown(); - bool AutoResume(); +#if HAS_VOLTAGE_MONITOR + bool LowVoltagePause(); + bool LowVoltageResume(); bool AutoResumeAfterShutdown(); #endif @@ -193,11 +211,14 @@ private: bool LockFileSystem(const GCodeBuffer& gb); // Lock the unshareable parts of the file system bool LockMovement(const GCodeBuffer& gb); // Lock movement bool LockMovementAndWaitForStandstill(const GCodeBuffer& gb); // Lock movement and wait for pending moves to finish + void GrabResource(const GCodeBuffer& gb, Resource r); // Grab a resource even if it is already owned + void GrabMovement(const GCodeBuffer& gb); // Grab the movement lock even if it is already owned void UnlockAll(const GCodeBuffer& gb); // Release all locks void StartNextGCode(GCodeBuffer& gb, StringRef& reply); // Fetch a new or old GCode and process it + void RunStateMachine(GCodeBuffer& gb, StringRef& reply); // Execute a step of the state machine void DoFilePrint(GCodeBuffer& gb, StringRef& reply); // Get G Codes from a file and print them - bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, bool runningM502 = false); + bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing, int codeRunning = 0); // Run a GCode macro file, optionally report error if not found void FileMacroCyclesReturn(GCodeBuffer& gb); // End a macro @@ -256,9 +277,10 @@ private: bool ChangeMicrostepping(size_t drive, int microsteps, int mode) const; // Change microstepping on the specified drive void ListTriggers(StringRef reply, TriggerInputsBitmap mask); // Append a list of trigger inputs to a message void CheckTriggers(); // Check for and execute triggers + void CheckFilament(); // Check for and respond to filament errors void DoEmergencyStop(); // Execute an emergency stop - void DoPause(GCodeBuffer& gb, bool isAuto) // Pause the print + void DoPause(GCodeBuffer& gb, PauseReason reason, const char *msg, ...) __attribute__ ((format (printf, 4, 5))); // Pause the print pre(resourceOwners[movementResource] = &gb); void SetMappedFanSpeed(); // Set the speeds of fans mapped for the current tool @@ -282,7 +304,7 @@ private: void EndSimulation(GCodeBuffer *gb); // Restore positions etc. when exiting simulation mode bool IsCodeQueueIdle() const; // Return true if the code queue is idle - void SaveResumeInfo(); + void SaveResumeInfo(bool wasPowerFailure); const char* GetMachineModeString() const; // Get the name of the current machine mode @@ -294,12 +316,7 @@ private: StreamGCodeInput* serialInput; // ... StreamGCodeInput* auxInput; // ...for the GCodeBuffers below -#ifdef DUET_NG GCodeBuffer* gcodeSources[8]; // The various sources of gcodes - GCodeBuffer*& autoPauseGCode = gcodeSources[7]; // GCode state machine used to run pause.g and resume.g -#else - GCodeBuffer* gcodeSources[7]; // The various sources of gcodes -#endif GCodeBuffer*& httpGCode = gcodeSources[0]; GCodeBuffer*& telnetGCode = gcodeSources[1]; @@ -308,6 +325,8 @@ private: GCodeBuffer*& auxGCode = gcodeSources[4]; // This one is for the LCD display on the async serial interface GCodeBuffer*& daemonGCode = gcodeSources[5]; // Used for executing config.g and trigger macro files GCodeBuffer*& queuedGCode = gcodeSources[6]; + GCodeBuffer*& autoPauseGCode = gcodeSources[7]; // ***THIS ONE MUST BE LAST*** GCode state machine used to run macros on power fail, heater faults and filament out + size_t nextGcodeSource; // The one to check next const GCodeBuffer* resourceOwners[NumResources]; // Which gcode buffer owns each resource @@ -316,18 +335,24 @@ private: bool active; // Live and running? bool isPaused; // true if the print has been paused manually or automatically bool pausePending; // true if we have been asked to pause but we are running a macro -#ifdef DUET_NG - bool isAutoPaused; // true if the print was paused automatically -#endif bool runningConfigFile; // We are running config.g during the startup process bool doingToolChange; // We are running tool change macros + uint8_t moveFractionToStartAt; // how much of the next move was printed before the power failure + uint8_t moveFractionToSkip; + + #if HAS_VOLTAGE_MONITOR + bool isPowerFailPaused; // true if the print was paused automatically because of a power failure + char *powerFailScript; // the commands run when there is a power failure +#endif + float currentUserPosition[MaxAxes]; // The current position of the axes as commanded by the input gcode, before accounting for tool offset and Z hop float currentZHop; // The amount of Z hop that is currently applied // The following contain the details of moves that the Move module fetches RawMove moveBuffer; // Move details to pass to Move class unsigned int segmentsLeft; // The number of segments left to do in the current move, or 0 if no move available + unsigned int totalSegments; // The total number of segments left in the complete move float arcCentre[MaxAxes]; float arcRadius; float arcCurrentAngle; @@ -355,9 +380,6 @@ private: FileStore* fileBeingWritten; // A file to write G Codes (or sometimes HTML) to FilePosition fileSize; // Size of the file being written - int oldToolNumber, newToolNumber; // Tools being changed - int toolChangeParam; // Bitmap of all the macros to be run during a tool change - const char* eofString; // What's at the end of an HTML file? uint8_t eofStringCounter; // Check the... uint8_t eofStringLength; // ... EoF string as we read. @@ -443,20 +465,14 @@ private: static constexpr const char* CONFIG_OVERRIDE_G = "config-override.g"; static constexpr const char* DEPLOYPROBE_G = "deployprobe.g"; static constexpr const char* RETRACTPROBE_G = "retractprobe.g"; - static constexpr const char* RESUME_PROLOGUE_G = "resurrect-prologue.g"; static constexpr const char* PAUSE_G = "pause.g"; static constexpr const char* HOME_ALL_G = "homeall.g"; static constexpr const char* HOME_DELTA_G = "homedelta.g"; static constexpr const char* DefaultHeightMapFile = "heightmap.csv"; static constexpr const char* LOAD_FILAMENT_G = "load.g"; static constexpr const char* UNLOAD_FILAMENT_G = "unload.g"; - -#ifdef DUET_NG - static constexpr const char* POWER_FAIL_G = "powerfail.g"; - static constexpr const char* POWER_RESTORE_G = "powerrestore.g"; -#endif - static constexpr const char* RESUME_AFTER_POWER_FAIL_G = "resurrect.g"; + static constexpr const char* RESUME_PROLOGUE_G = "resurrect-prologue.g"; static constexpr const float MinServoPulseWidth = 544.0, MaxServoPulseWidth = 2400.0; static const uint16_t ServoRefreshFrequency = 50; diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp index f30cc1bb..8767b4a1 100644 --- a/src/GCodes/GCodes2.cpp +++ b/src/GCodes/GCodes2.cpp @@ -29,17 +29,13 @@ # include "FirmwareUpdater.h" #endif +#include <algorithm> // for std::swap + // If the code to act on is completed, this returns true, // otherwise false. It is called repeatedly for a given // code until it returns true for that code. bool GCodes::ActOnCode(GCodeBuffer& gb, StringRef& reply) { - // Discard empty buffers right away - if (gb.IsEmpty()) - { - return true; - } - // Can we queue this code? if (gb.CanQueueCodes() && codeQueue->QueueCode(gb, segmentsLeft)) { @@ -52,22 +48,35 @@ bool GCodes::ActOnCode(GCodeBuffer& gb, StringRef& reply) switch (gb.GetCommandLetter()) { case 'G': - return HandleGcode(gb, reply); + if (gb.HasCommandNumber()) + { + return HandleGcode(gb, reply); + } + break; + case 'M': - return HandleMcode(gb, reply); + if (gb.HasCommandNumber()) + { + return HandleMcode(gb, reply); + } + break; + case 'T': return HandleTcode(gb, reply); + default: - // An invalid command gets discarded - HandleReply(gb, false, ""); - return true; + break; } + + reply.printf("Bad command: %s", gb.Buffer()); + HandleReply(gb, true, reply.Pointer()); + return true; } bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply) { GCodeResult result = GCodeResult::ok; - const int code = gb.GetIValue(); + const int code = gb.GetCommandNumber(); if (simulationMode != 0 && code > 4 && code != 10 && code != 11 && code != 20 && code != 21 && code != 90 && code != 91 && code != 92) { return true; // we only simulate some gcodes @@ -108,7 +117,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply) } if (DoArcMove(gb, code == 2)) { - reply.copy("Invalid G2 or G3 command"); + reply.copy("Invalid arc parameters"); result = GCodeResult::error; } break; @@ -229,7 +238,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply) break; default: - reply.printf("invalid G Code: %s", gb.Buffer()); + reply.copy("Unsupported command"); result = GCodeResult::error; } @@ -246,6 +255,11 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply) if (gb.GetState() == GCodeState::normal) { UnlockAll(gb); + if (result == GCodeResult::error) + { + scratchString.printf("G%d: ", code); + reply.Prepend(scratchString.Pointer()); + } HandleReply(gb, result != GCodeResult::ok, reply.Pointer()); } return true; @@ -255,7 +269,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) { GCodeResult result = GCodeResult::ok; - const int code = gb.GetIValue(); + const int code = gb.GetCommandNumber(); if ( simulationMode != 0 && (code < 20 || code > 37) && code != 0 && code != 1 && code != 82 && code != 83 && code != 105 && code != 109 && code != 111 && code != 112 && code != 122 @@ -496,7 +510,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } { const size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0; - result = GetGCodeResultFromError(platform.GetMassStorage()->Mount(card, reply, true)) ; + result = platform.GetMassStorage()->Mount(card, reply, true); } break; @@ -507,7 +521,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } { const size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0; - result = GetGCodeResultFromError(platform.GetMassStorage()->Unmount(card, reply)); + result = platform.GetMassStorage()->Unmount(card, reply); } break; @@ -556,35 +570,63 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) break; case 24: // Print/resume-printing the selected file - if (!LockMovementAndWaitForStandstill(gb)) + if (IsPausing() || IsResuming()) { - return false; + // ignore the resume request } - - if (isPaused) + else { - gb.SetState(GCodeState::resuming1); - if (AllAxesAreHomed()) + if (!LockMovementAndWaitForStandstill(gb)) { - DoFileMacro(gb, RESUME_G, true); + return false; } - } - else if (!fileToPrint.IsLive()) - { - reply.copy("Cannot print, because no file is selected!"); - result = GCodeResult::error; - } - else - { - if (fileOffsetToPrint != 0) + + if (IsPaused()) + { +#if HAS_VOLTAGE_MONITOR + if (!platform.IsPowerOk()) + { + reply.copy("Cannot resume while power voltage is low"); + result = GCodeResult::error; + } + else +#endif + { + gb.SetState(GCodeState::resuming1); + if (AllAxesAreHomed()) + { + DoFileMacro(gb, RESUME_G, true); + } + } + } + else if (!fileToPrint.IsLive()) { - // We executed M23 to set the file offset, which normally means that we are executing resurrect.g. - // We need to copy the absolute/relative and volumetric extrusion flags over - fileGCode->OriginalMachineState().drivesRelative = gb.MachineState().drivesRelative; - fileGCode->OriginalMachineState().volumetricExtrusion = gb.MachineState().volumetricExtrusion; - fileToPrint.Seek(fileOffsetToPrint); + reply.copy("Cannot print, because no file is selected!"); + result = GCodeResult::error; + } + else + { +#if HAS_VOLTAGE_MONITOR + if (!platform.IsPowerOk()) + { + reply.copy("Cannot start a print while power voltage is low"); + result = GCodeResult::error; + } + else +#endif + { + if (fileOffsetToPrint != 0) + { + // We executed M23 to set the file offset, which normally means that we are executing resurrect.g. + // We need to copy the absolute/relative and volumetric extrusion flags over + fileGCode->OriginalMachineState().drivesRelative = gb.MachineState().drivesRelative; + fileGCode->OriginalMachineState().volumetricExtrusion = gb.MachineState().volumetricExtrusion; + fileToPrint.Seek(fileOffsetToPrint); + moveFractionToSkip = moveFractionToStartAt; + } + StartPrinting(); + } } - StartPrinting(); } break; @@ -601,7 +643,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) { return false; } - DoPause(gb, false); + DoPause(gb, PauseReason::gcode, nullptr); } } break; @@ -627,7 +669,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) { return false; } - DoPause(gb, false); + DoPause(gb, PauseReason::user, nullptr); } break; @@ -635,8 +677,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) // This is used between executing M23 to set up the file to print, and M25 to print it if (gb.Seen('S')) { - // Ideally we would get an unsigned value here in case of the file offset being >2Gb - fileOffsetToPrint = (FilePosition)gb.GetIValue(); + fileOffsetToPrint = (FilePosition)gb.GetUIValue(); + if (gb.Seen('P')) + { + moveFractionToStartAt = (uint8_t)constrain<unsigned long>(lrintf(gb.GetFValue()), 0, 255); + } } break; @@ -939,16 +984,17 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) FileMacroCyclesReturn(gb); break; - case 101: // Un-retract + case 101: // Un-retract, generated by S3D if "Include M101/101/103" is enabled result = RetractFilament(gb, false); break; case 102: - // S3D generates this command once at the start of he print job if firmware retraction is enabled. + // S3D generates this command just before each explicit retraction command if both explicit retraction and "Include M101/101/103" are enabled. + // Old versions of S3D also generate it once at the start of each prnit file if "Include M101/101/103" is enabled. // It's not documented, so we just ignore it rather than generate an error message. break; - case 103: // Retract + case 103: // Retract, generated by S3D if "Include M101/101/103" is enabled result = RetractFilament(gb, true); break; @@ -1077,8 +1123,8 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) return false; } - newToolNumber = tool->Number(); - toolChangeParam = (simulationMode == 0) ? 0 : DefaultToolChangeParam; + gb.MachineState().newToolNumber = tool->Number(); + gb.MachineState().toolChangeParam = (simulationMode == 0) ? 0 : DefaultToolChangeParam; gb.SetState(GCodeState::m109ToolChange0); } else @@ -1272,7 +1318,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else if (bedHeater >= (int)Heaters) { - reply.copy("Invalid heater number"); + reply.printf("Invalid heater number '%d'", bedHeater); result = GCodeResult::error; break; } @@ -1331,7 +1377,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else if (heater >= (int)Heaters) { - reply.copy("Bad heater number specified!"); + reply.printf("Bad heater number '%d'", heater); result = GCodeResult::error; break; } @@ -1362,7 +1408,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("No chamber heater has been set up yet!"); + reply.copy("No chamber heater has been configured"); result = GCodeResult::error; } } @@ -1376,7 +1422,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("No chamber heater has been configured yet."); + reply.copy("No chamber heater has been configured"); } } } @@ -1387,7 +1433,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const int heater = (gb.Seen('H')) ? gb.GetIValue() : 1; // default to extruder 1 if no heater number provided if (heater < 0 || heater >= (int)Heaters) { - reply.copy("Invalid heater number"); + reply.printf("Invalid heater number '%d'", heater); result = GCodeResult::error; } else if (gb.Seen('S')) @@ -1656,11 +1702,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const float value = gb.GetFValue() * distanceScale; if (setMin) { - platform.SetAxisMinimum(axis, value); + platform.SetAxisMinimum(axis, value, gb.MachineState().runningM501); } else { - platform.SetAxisMaximum(axis, value); + platform.SetAxisMaximum(axis, value, gb.MachineState().runningM501); } seen = true; } @@ -1709,7 +1755,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.printf("Invalid speed factor specified."); + reply.printf("Invalid speed factor"); result = GCodeResult::error; } } @@ -1921,7 +1967,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) : heater == reprap.GetHeat().GetBedHeater() ? 75.0 : heater == reprap.GetHeat().GetChamberHeater() ? 50.0 : 200.0; - const float maxPwm = (gb.Seen('P')) ? gb.GetFValue() : 1.0; + const float maxPwm = (gb.Seen('P')) ? gb.GetFValue() : reprap.GetHeat().GetHeaterModel(heater).GetMaxPwm(); if (heater < 0 || heater >= (int)Heaters) { reply.copy("Bad heater number in M303 command"); @@ -2198,7 +2244,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Bad M452 P parameter"); + reply.copy("Bad P parameter"); result = GCodeResult::error; } } @@ -2233,7 +2279,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Bad M453 P parameter"); + reply.copy("Bad P parameter"); result = GCodeResult::error; } } @@ -2252,14 +2298,14 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) break; case 501: // Load parameters from EEPROM - DoFileMacro(gb, "config-override.g", true); + DoFileMacro(gb, "config-override.g", true, code); break; case 502: // Revert to default "factory settings" reprap.GetHeat().ResetHeaterModels(); // in case some heaters have no M307 commands in config.g reprap.GetMove().GetKinematics().SetCalibrationDefaults(); // in case M665/M666/M667/M669 in config.g don't define all the parameters platform.SetZProbeDefaults(); - DoFileMacro(gb, "config.g", true, true); + DoFileMacro(gb, "config.g", true, code); break; case 503: // List variable settings @@ -2281,7 +2327,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) FileStore * const f = platform.GetFileStore(platform.GetSysDir(), platform.GetConfigFile(), OpenMode::read); if (f == nullptr) { - reply.copy("Configuration file not found!"); + reply.copy("Configuration file not found"); result = GCodeResult::error; } else @@ -2368,7 +2414,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) StringRef ssid(ssidBuffer, ARRAY_SIZE(ssidBuffer)); if (gb.Seen('P') && !gb.GetQuotedString(ssid)) { - reply.copy("Bad or missing SSID in M552 command"); + reply.copy("Bad or missing SSID"); result = GCodeResult::error; } else @@ -2635,7 +2681,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Invalid heater number.\n"); + reply.printf("Invalid heater number '%d'", heater); result = GCodeResult::error; } } @@ -2758,7 +2804,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) { return false; } - platform.SetEnableValue(drive, gb.GetIValue() != 0); + platform.SetEnableValue(drive, (int8_t)gb.GetIValue()); seen = true; } if (gb.Seen('T')) @@ -3011,7 +3057,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const size_t eDrive = eDrives[extruder]; if (eDrive >= MaxExtruders) { - reply.copy("Invalid extruder drive specified!"); + reply.printf("Invalid extruder drive '%u'", eDrive); result = GCodeResult::error; break; } @@ -3261,7 +3307,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) if (badDrive) { - reply.copy("Invalid drive number in M584 command"); + reply.copy("Invalid drive number"); result = GCodeResult::error; } else @@ -3276,7 +3322,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Invalid number of visible axes in M584 command"); + reply.copy("Invalid number of visible axes"); result = GCodeResult::error; } } @@ -3307,6 +3353,79 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } break; + case 585: // Probe Tool + if (reprap.GetCurrentTool() == nullptr) + { + reply.copy("No tool selected!"); + result = GCodeResult::error; + break; + } + + if (LockMovementAndWaitForStandstill(gb)) + { + for (size_t axis = 0; axis < numTotalAxes; axis++) + { + if (gb.Seen(axisLetters[axis])) + { + // Get parameters first and check them + const int endStopToUse = gb.Seen('E') ? gb.GetIValue() : 0; + if (endStopToUse < 0 || endStopToUse > (int)DRIVES) + { + reply.copy("Invalid endstop number"); + result = GCodeResult::error; + break; + } + + // Save the current axis coordinates + memcpy(toolChangeRestorePoint.moveCoords, currentUserPosition, ARRAY_SIZE(currentUserPosition) * sizeof(currentUserPosition[0])); + + // Prepare another move similar to G1 .. S3 + moveBuffer.moveType = 3; + if (endStopToUse == 0) + { + moveBuffer.endStopsToCheck = 0; + SetBit(moveBuffer.endStopsToCheck, axis); + } + else + { + moveBuffer.endStopsToCheck = UseSpecialEndstop; + SetBit(moveBuffer.endStopsToCheck, endStopToUse); + } + moveBuffer.xAxes = DefaultXAxisMapping; + moveBuffer.yAxes = DefaultYAxisMapping; + moveBuffer.usePressureAdvance = false; + moveBuffer.filePos = noFilePosition; + moveBuffer.canPauseAfter = false; + moveBuffer.canPauseBefore = true; + + // Decide which way and how far to go + const float axisLength = platform.AxisMaximum(axis) - platform.AxisMinimum(axis) + 5.0; + moveBuffer.coords[axis] = (gb.Seen('S') && gb.GetIValue() == 1) ? axisLength * -1.0 : axisLength; + + // Zero every extruder drive + for (size_t drive = numTotalAxes; drive < DRIVES; drive++) + { + moveBuffer.coords[drive] = 0.0; + } + moveBuffer.hasExtrusion = false; + + // Deal with feed rate + if (gb.Seen(feedrateLetter)) + { + const float rate = gb.GetFValue() * distanceScale; + gb.MachineState().feedrate = rate * SecondsToMinutes; // don't apply the speed factor to homing and other special moves + } + moveBuffer.feedRate = gb.MachineState().feedrate; + + // Kick off new movement + segmentsLeft = 1; + gb.SetState(GCodeState::probingToolOffset); + break; + } + } + } + break; + case 586: // Configure network protocols if (gb.Seen('P')) { @@ -3373,7 +3492,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Bad parameter in M587 command"); + reply.copy("Bad parameter"); result = GCodeResult::error; } } @@ -3462,7 +3581,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Bad parameter in M588 command"); + reply.copy("Bad parameter"); result = GCodeResult::error; } } @@ -3513,7 +3632,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Bad or missing parameter in M589 command"); + reply.copy("Bad or missing parameter"); result = GCodeResult::error; } } @@ -3603,7 +3722,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } if (changed || changedMode) { - if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed)) + if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false)) { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -3661,7 +3780,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) break; default: - reply.printf("Mode %d is not valid in M667 command\n", mode); + reply.printf("Mode %d is not valid", mode); result = GCodeResult::error; break; } @@ -3688,7 +3807,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) move.GetKinematics().GetAssumedInitialPosition(numVisibleAxes, moveBuffer.coords); ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); } - if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed)) + if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false)) { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -3714,7 +3833,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const int nk = gb.GetIValue(); if (nk < 0 || nk >= (int)KinematicsType::unknown || !move.SetKinematics(static_cast<KinematicsType>(nk))) { - reply.printf("Unknown kinematics type %d in M669 command", nk); + reply.printf("Unknown kinematics type %d", nk); result = GCodeResult::error; break; } @@ -3735,7 +3854,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) move.GetKinematics().GetAssumedInitialPosition(numVisibleAxes, moveBuffer.coords); ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); } - if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed)) + if (reprap.GetMove().GetKinematics().LimitPosition(moveBuffer.coords, numVisibleAxes, axesHomed, false)) { ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // make sure the limits are reflected in the user position } @@ -3795,7 +3914,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } else { - reply.copy("Invalid source for this M-code"); + reply.copy("Invalid source"); result = GCodeResult::error; } break; @@ -3946,7 +4065,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const char * const dateString = gb.GetString(); if (strptime(dateString, "%Y-%m-%d", &timeInfo) == nullptr) { - reply.copy("M905: Invalid date format"); + reply.copy("Invalid date format"); result = GCodeResult::error; break; } @@ -3960,7 +4079,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) const char * const timeString = gb.GetString(); if (strptime(timeString, "%H:%M:%S", &timeInfo) == nullptr) { - reply.copy("M905: Invalid time format"); + reply.copy("Invalid time format"); result = GCodeResult::error; break; } @@ -4052,12 +4171,57 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) } break; -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR case 911: // Enable auto save - result = GetGCodeResultFromError(platform.ConfigureAutoSave(gb, reply)); + if (gb.Seen('S')) + { + const float saveVoltage = gb.GetFValue(); + if (saveVoltage < 10.0) + { + platform.DisableAutoSave(); + } + else + { + float resumeVoltage = saveVoltage + 1.0; // set up default resume voltage + bool dummy; + gb.TryGetFValue('R', resumeVoltage, dummy); + + String<80> powerFailString; + bool seenCommandString = false; + gb.TryGetQuotedString('P', powerFailString.GetRef(), seenCommandString); + if (seenCommandString) + { + // Replace the power fail script atomically + char *newPowerFailScript = new char[powerFailString.strlen() + 1]; + strcpy(newPowerFailScript, powerFailString.c_str()); + std::swap(newPowerFailScript, powerFailScript); + delete[] newPowerFailScript; + } + else if (powerFailScript == nullptr) + { + reply.copy("No power fail script provided"); + result = GCodeResult::error; + break; + } + platform.EnableAutoSave(saveVoltage, resumeVoltage); + } + } + else + { + float saveVoltage, resumeVoltage; + if (platform.GetAutoSaveSettings(saveVoltage, resumeVoltage)) + { + reply.printf("Auto save voltage %.1fV, resume %.1fV, script \"%s\"", (double)saveVoltage, (double)resumeVoltage, (powerFailScript == nullptr) ? "" : powerFailScript); + } + else + { + reply.copy("Auto save is disabled"); + } + } break; #endif +#if HAS_CPU_TEMP_SENSOR case 912: // Set electronics temperature monitor adjustment // Currently we ignore the P parameter (i.e. temperature measurement channel) if (gb.Seen('S')) @@ -4069,6 +4233,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) reply.printf("MCU temperature calibration adjustment is %.1f" DEGREE_SYMBOL "C", (double)platform.GetMcuTemperatureAdjust()); } break; +#endif // For case 913, see 906 @@ -4098,6 +4263,31 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) break; #endif +#if HAS_SMART_DRIVERS + case 915: + result = GetGCodeResultFromError(platform.ConfigureStallDetection(gb, reply)); + break; +#endif + +#if HAS_VOLTAGE_MONITOR + case 916: + if (!platform.GetMassStorage()->FileExists(platform.GetSysDir(), RESUME_AFTER_POWER_FAIL_G)) + { + reply.copy("No resume file found"); + result = GCodeResult::error; + } + else if (!platform.GetMassStorage()->FileExists(platform.GetSysDir(), RESUME_PROLOGUE_G)) + { + reply.printf("Resume prologue file '%s' not found", RESUME_PROLOGUE_G); + result = GCodeResult::error; + } + else + { + DoFileMacro(gb, RESUME_AFTER_POWER_FAIL_G, true); + } + break; +#endif + case 929: // Start/stop event logging result = GetGCodeResultFromError(platform.ConfigureLogging(gb, reply)); break; @@ -4191,7 +4381,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) break; default: - reply.printf("Unsupported command: %s", gb.Buffer()); + reply.copy("Unsupported command"); result = GCodeResult::error; break; } @@ -4210,6 +4400,11 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply) { gb.timerRunning = false; UnlockAll(gb); + if (result == GCodeResult::error) + { + scratchString.printf("M%d: ", code); + reply.Prepend(scratchString.Pointer()); + } HandleReply(gb, result != GCodeResult::ok, reply.Pointer()); } return true; @@ -4222,24 +4417,25 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, StringRef& reply) return true; // when running M502 we don't execute T commands } - if (!LockMovementAndWaitForStandstill(gb)) + if (gb.HasCommandNumber()) { - return false; - } + if (!LockMovementAndWaitForStandstill(gb)) + { + return false; + } - if (strlen(gb.Buffer()) > 1) - { // See if the tool can be changed - newToolNumber = gb.GetIValue(); - newToolNumber += gb.GetToolNumberAdjust(); + int toolNum = gb.GetCommandNumber(); + toolNum += gb.GetToolNumberAdjust(); const Tool * const oldTool = reprap.GetCurrentTool(); // If old and new are the same we no longer follow the sequence. User can deselect and then reselect the tool if he wants the macros run. - if (oldTool == nullptr || oldTool->Number() != newToolNumber) + if (oldTool == nullptr || oldTool->Number() != toolNum) { - toolChangeParam = (simulationMode != 0) ? 0 - : gb.Seen('P') ? gb.GetIValue() - : DefaultToolChangeParam; + gb.MachineState().newToolNumber = toolNum; + gb.MachineState().toolChangeParam = (simulationMode != 0) ? 0 + : gb.Seen('P') ? gb.GetUIValue() + : DefaultToolChangeParam; gb.SetState(GCodeState::toolChange0); return true; // proceeding with state machine, so don't unlock or send a reply } @@ -4250,11 +4446,11 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, StringRef& reply) const Tool * const tool = reprap.GetCurrentTool(); if (tool == nullptr) { - reply.copy("No tool is selected."); + reply.copy("No tool is selected"); } else { - reply.printf("Tool %d is selected.", tool->Number()); + reply.printf("Tool %d is selected", tool->Number()); } } diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp index 011fec02..977730de 100644 --- a/src/GCodes/RestorePoint.cpp +++ b/src/GCodes/RestorePoint.cpp @@ -20,6 +20,7 @@ void RestorePoint::Init() } feedRate = DefaultFeedrate * SecondsToMinutes; virtualExtruderPosition = 0.0; + proportionDone = 0.0; filePos = noFilePosition; #if SUPPORT_IOBITS ioBits = 0; diff --git a/src/GCodes/RestorePoint.h b/src/GCodes/RestorePoint.h index fd9ca828..129d6100 100644 --- a/src/GCodes/RestorePoint.h +++ b/src/GCodes/RestorePoint.h @@ -16,13 +16,15 @@ struct RestorePoint { - float moveCoords[MaxAxes]; - float feedRate; - float virtualExtruderPosition; - FilePosition filePos; + float moveCoords[MaxAxes]; // The axis locations when we paused + float feedRate; // The feed rate for the current move + float virtualExtruderPosition; // The virtual extruder position at the start of this move + float proportionDone; // How much of this move we have already done (zero unless we interrupts a move) + FilePosition filePos; // The file position that this move was read from #if SUPPORT_IOBITS - IoBits_t ioBits; + IoBits_t ioBits; // The output port bits setting for this move #endif + RestorePoint(); void Init(); }; diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp index 841f79b4..b67a8abc 100644 --- a/src/Heating/Heat.cpp +++ b/src/Heating/Heat.cpp @@ -105,7 +105,7 @@ void Heat::Init() virtualHeaterSensors[0] = TemperatureSensor::Create(CpuTemperatureSenseChannel); virtualHeaterSensors[0]->SetHeaterName("MCU"); // name this virtual heater so that it appears in DWC #endif -#ifdef DUET_NG +#if HAS_SMART_DRIVERS virtualHeaterSensors[1] = TemperatureSensor::Create(FirstTmcDriversSenseChannel); virtualHeaterSensors[2] = TemperatureSensor::Create(FirstTmcDriversSenseChannel + 1); #endif @@ -488,7 +488,7 @@ float Heat::GetTemperature(size_t heater, TemperatureError& err) return t; } -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Suspend the heaters to conserve power void Heat::SuspendHeaters(bool sus) diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h index cfa81fc4..cdfc9c99 100644 --- a/src/Heating/Heat.h +++ b/src/Heating/Heat.h @@ -130,7 +130,7 @@ public: bool WriteBedAndChamberTempSettings(FileStore *f) const; // Save some resume information -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR void SuspendHeaters(bool sus); // Suspend the heaters to conserve power #endif diff --git a/src/Heating/Pid.cpp b/src/Heating/Pid.cpp index 0fef6e59..236981bb 100644 --- a/src/Heating/Pid.cpp +++ b/src/Heating/Pid.cpp @@ -74,7 +74,7 @@ void PID::Reset() averagePWM = lastPwm = 0.0; heatingFaultCount = 0; temperature = BAD_ERROR_TEMPERATURE; -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR suspended = false; #endif } @@ -188,16 +188,16 @@ void PID::Spin() { if (model.IsEnabled()) { -#ifdef DUET_NG + // Read the temperature even if the heater is suspended + const TemperatureError err = ReadTemperature(); + +#if HAS_VOLTAGE_MONITOR if (suspended) { SetHeater(0.0); return; } #endif - // Read the temperature - const TemperatureError err = ReadTemperature(); - // Handle any temperature reading error and calculate the temperature rate of change, if possible if (err != TemperatureError::success) { @@ -267,7 +267,7 @@ void PID::Spin() { SetHeater(0.0); // do this here just to be sure mode = HeaterMode::fault; - reprap.GetGCodes().CancelPrint(false, false); + reprap.GetGCodes().HandleHeaterFault(heater); platform.MessageF(ErrorMessage, "Heating fault on heater %d, temperature rising much more slowly than the expected %.1f" DEGREE_SYMBOL "C/sec\n", heater, (double)expectedRate); @@ -294,7 +294,7 @@ void PID::Spin() { SetHeater(0.0); // do this here just to be sure mode = HeaterMode::fault; - reprap.GetGCodes().CancelPrint(false, false); + reprap.GetGCodes().HandleHeaterFault(heater); platform.MessageF(ErrorMessage, "Heating fault on heater %d, temperature excursion exceeded %.1f" DEGREE_SYMBOL "C\n", heater, (double)maxTempExcursion); } @@ -498,7 +498,7 @@ void PID::StartAutoTune(float targetTemp, float maxPwm, StringRef& reply) tuningTempReadings = new float[MaxTuningTempReadings]; tuningTempReadings[0] = temperature; tuningReadingInterval = platform.HeatSampleInterval(); - tuningPwm = min<float>(maxPwm, model.GetMaxPwm()); + tuningPwm = maxPwm; tuningTargetTemp = targetTemp; reply.printf("Auto tuning heater %d using target temperature %.1f" DEGREE_SYMBOL "C and PWM %.2f - do not leave printer unattended", heater, (double)targetTemp, (double)maxPwm); } @@ -857,7 +857,7 @@ void PID::DisplayBuffer(const char *intro) } } -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Suspend the heater to conserve power, or resume it void PID::Suspend(bool sus) diff --git a/src/Heating/Pid.h b/src/Heating/Pid.h index 8907063a..1ca9b5d3 100644 --- a/src/Heating/Pid.h +++ b/src/Heating/Pid.h @@ -16,8 +16,6 @@ #include "FOPDT.h" #include "TemperatureError.h" -#define NEW_TUNING (1) - class PID { enum class HeaterMode : uint8_t @@ -32,12 +30,8 @@ class PID tuning0, tuning1, tuning2, -#ifdef NEW_TUNING tuning3, lastTuningMode = tuning3 -#else - lastTuningMode = tuning2 -#endif }; static const size_t NumPreviousTemperatures = 4; // How many samples we average the temperature derivative over @@ -86,7 +80,7 @@ public: void SetM301PidParameters(const M301PidParameters& params) { model.SetM301PidParameters(params); } -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR void Suspend(bool sus); // Suspend the heater to conserve power #endif @@ -98,14 +92,9 @@ private: void DoTuningStep(); // Called on each temperature sample when auto tuning static bool ReadingsStable(size_t numReadings, float maxDiff) pre(numReadings >= 2; numReadings <= MaxTuningTempReadings); -#ifdef NEW_TUNING static int GetPeakTempIndex(); // Auto tune helper function static int IdentifyPeak(size_t numToAverage); // Auto tune helper function void CalculateModel(); // Calculate G, td and tc from the accumulated readings -#else - static size_t GetMaxRateIndex(); // Auto tune helper function - void FitCurve(); // Calculate G, td and tc from the accumulated readings -#endif void DisplayBuffer(const char *intro); // Debug helper float GetExpectedHeatingRate() const; // Get the minimum heating rate we expect @@ -132,7 +121,7 @@ private: HeaterMode mode; // Current state of the heater bool active; // Are we active or standby? bool tuned; // True if tuning was successful -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR bool suspended; // True if suspended to save power #endif uint8_t badTemperatureCount; // Count of sequential dud readings @@ -150,16 +139,10 @@ private: static uint32_t tuningPhaseStartTime; // when we started the current tuning phase static uint32_t tuningReadingInterval; // how often we are sampling, in milliseconds static size_t tuningReadingsTaken; // how many temperature samples we have taken - -#ifdef NEW_TUNING static float tuningHeaterOffTemp; // the temperature when we turned the heater off static float tuningPeakTemperature; // the peak temperature reached, averaged over 3 readings (so slightly less than the true peak) static uint32_t tuningHeatingTime; // how long we had the heating on for static uint32_t tuningPeakDelay; // how many milliseconds the temperature continues to rise after turning the heater off -#else - static float tuningTimeOfFastestRate; // how long after turn-on the fastest temperature rise occurred - static float tuningFastestRate; // the fastest temperature rise -#endif }; diff --git a/src/Heating/Sensors/TemperatureSensor.cpp b/src/Heating/Sensors/TemperatureSensor.cpp index d6054d2f..852c9796 100644 --- a/src/Heating/Sensors/TemperatureSensor.cpp +++ b/src/Heating/Sensors/TemperatureSensor.cpp @@ -12,6 +12,9 @@ #ifdef DUET_NG #include "DhtSensor.h" +#endif + +#if HAS_SMART_DRIVERS #include "TmcDriverTemperatureSensor.h" #endif @@ -115,7 +118,7 @@ TemperatureSensor *TemperatureSensor::Create(unsigned int channel) ts = new CpuTemperatureSensor(channel); } #endif -#ifdef DUET_NG +#if HAS_SMART_DRIVERS else if (channel >= FirstTmcDriversSenseChannel && channel < FirstTmcDriversSenseChannel + 2) { ts = new TmcDriverTemperatureSensor(channel); diff --git a/src/Heating/Sensors/Thermistor.h b/src/Heating/Sensors/Thermistor.h index 1c002c34..74774f2b 100644 --- a/src/Heating/Sensors/Thermistor.h +++ b/src/Heating/Sensors/Thermistor.h @@ -41,7 +41,7 @@ private: void SetHighOffset(int8_t p_offset) { adcHighOffset = p_offset; } // For the theory behind ADC oversampling, see http://www.atmel.com/Images/doc8003.pdf - static const unsigned int AdcOversampleBits = 1 ; // we use 1-bit oversampling + static const unsigned int AdcOversampleBits = 2; // we use 2-bit oversampling void CalcDerivedParameters(); // calculate shA and shB diff --git a/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp b/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp index fe2ccbf9..ac1bd464 100644 --- a/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp +++ b/src/Heating/Sensors/TmcDriverTemperatureSensor.cpp @@ -9,7 +9,7 @@ #include "Platform.h" #include "RepRap.h" -#ifdef DUET_NG +#if HAS_SMART_DRIVERS TmcDriverTemperatureSensor::TmcDriverTemperatureSensor(unsigned int channel) : TemperatureSensor(channel, "TMC2660 temperature warnings") { diff --git a/src/Heating/Sensors/TmcDriverTemperatureSensor.h b/src/Heating/Sensors/TmcDriverTemperatureSensor.h index be18dd5c..099ec962 100644 --- a/src/Heating/Sensors/TmcDriverTemperatureSensor.h +++ b/src/Heating/Sensors/TmcDriverTemperatureSensor.h @@ -10,7 +10,7 @@ #include "TemperatureSensor.h" -#ifdef DUET_NG +#if HAS_SMART_DRIVERS class TmcDriverTemperatureSensor : public TemperatureSensor { diff --git a/src/Libraries/General/StringRef.cpp b/src/Libraries/General/StringRef.cpp index a05e6070..7b0840c1 100644 --- a/src/Libraries/General/StringRef.cpp +++ b/src/Libraries/General/StringRef.cpp @@ -8,6 +8,7 @@ #include "StringRef.h" #include <cstring> #include <cstdio> +#include "WMath.h" // Need to define strnlen here because it isn't ISO standard size_t strnlen(const char *s, size_t n) @@ -110,6 +111,16 @@ size_t StringRef::StripTrailingSpaces() const return slen; } +size_t StringRef::Prepend(const char *src) const +{ + const size_t slen = ::strlen(src); + const size_t dlen = strlen(); + const size_t newLen = min<size_t>(dlen + slen, len - 1); + memmove(p + slen, p, newLen - slen + 1); + memcpy(p, src, slen); + return newLen; +} + // End diff --git a/src/Libraries/General/StringRef.h b/src/Libraries/General/StringRef.h index 9660389a..09ddaa24 100644 --- a/src/Libraries/General/StringRef.h +++ b/src/Libraries/General/StringRef.h @@ -33,7 +33,7 @@ public: void Clear() const { p[0] = 0; } - int printf(const char *fmt, ...) const __attribute__ ((format (printf, 2, 3))); + int printf(const char *fmt, ...) const __attribute__ ((format (printf, 2, 3))); int vprintf(const char *fmt, va_list vargs) const; int catf(const char *fmt, ...) const __attribute__ ((format (printf, 2, 3))); int vcatf(const char *fmt, va_list vargs) const; @@ -41,6 +41,7 @@ public: size_t cat(const char *src) const; size_t cat(char c) const; size_t StripTrailingSpaces() const; + size_t Prepend(const char *src) const; bool IsEmpty() const { return p[0] == 0; } }; diff --git a/src/Logger.cpp b/src/Logger.cpp index e472f291..7d19237a 100644 --- a/src/Logger.cpp +++ b/src/Logger.cpp @@ -22,7 +22,7 @@ private: bool& b; }; -Logger::Logger() : logFile(), dirty(false), inLogger(false) +Logger::Logger() : logFile(), lastFlushTime(0), lastFlushFileSize(0), dirty(false), inLogger(false) { } @@ -35,7 +35,8 @@ void Logger::Start(time_t time, const StringRef& filename) if (f != nullptr) { logFile.Set(f); - logFile.Seek(logFile.Length()); + lastFlushFileSize = logFile.Length(); + logFile.Seek(lastFlushFileSize); InternalLogMessage(time, "Event logging started\n"); } } @@ -109,13 +110,26 @@ void Logger::InternalLogMessage(time_t time, const char *message) } } +// This is called regularly by Platform to give the logger an opportunity to flush the file buffer void Logger::Flush() { if (logFile.IsLive() && dirty && !inLogger) { - Lock loggerLock(inLogger); - logFile.Flush(); - dirty = false; + // Log file is dirty and can be flushed. + // To avoid excessive disk write operations, flush it only if one of the following is true: + // 1. We have possibly allocated a new cluster since the last flush. To avoid lost clusters if we power down before flushing, + // we should flush early in this case. Rather than determine the cluster size, we flush if we have started a new 512-byte sector. + // 2. If it hasn't been flushed for LogFlushInterval milliseconds. + const FilePosition currentPos = logFile.GetPosition(); + const uint32_t now = millis(); + if (now - lastFlushTime >= LogFlushInterval || currentPos/512 != lastFlushFileSize/512) + { + Lock loggerLock(inLogger); + logFile.Flush(); + lastFlushTime = millis(); + lastFlushFileSize = currentPos; + dirty = false; + } } } diff --git a/src/Logger.h b/src/Logger.h index 79b8df19..04397efe 100644 --- a/src/Logger.h +++ b/src/Logger.h @@ -30,6 +30,8 @@ private: void InternalLogMessage(time_t time, const char *message); FileData logFile; + uint32_t lastFlushTime; + FilePosition lastFlushFileSize; bool dirty; bool inLogger; }; diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index a76a25c6..896e588c 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -26,17 +26,30 @@ struct MoveParameters float accelDistance; float steadyDistance; float decelDistance; + float requestedSpeed; float startSpeed; float topSpeed; float endSpeed; + float targetNextSpeed; + uint32_t endstopChecks; + uint16_t flags; MoveParameters() { - accelDistance = steadyDistance = decelDistance = startSpeed = topSpeed = endSpeed = 0.0; + accelDistance = steadyDistance = decelDistance = requestedSpeed = startSpeed = topSpeed = endSpeed = targetNextSpeed = 0.0; + endstopChecks = 0; + flags = 0; + } + + void DebugPrint() const + { + reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f,%f,%f,%08" PRIX32 ",%04x\n", + (double)accelDistance, (double)steadyDistance, (double)decelDistance, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, + (double)targetNextSpeed, endstopChecks, flags); } }; -const size_t NumSavedMoves = 256; +const size_t NumSavedMoves = 128; static MoveParameters savedMoves[NumSavedMoves]; static size_t savedMovePointer = 0; @@ -47,8 +60,7 @@ static size_t savedMovePointer = 0; // Print the saved moved in CSV format for (size_t i = 0; i < NumSavedMoves; ++i) { - const MoveParameters& m = savedMoves[savedMovePointer]; - reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f\n", m.accelDistance, m.steadyDistance, m.decelDistance, m.startSpeed, m.topSpeed, m.endSpeed); + savedMoves[savedMovePointer].DebugPrint(); savedMovePointer = (savedMovePointer + 1) % NumSavedMoves; } } @@ -132,22 +144,21 @@ inline void DDA::InsertDM(DriveMovement *dm) *dmp = dm; } -// Remove this drive from the list of drives with steps due, and return its DM or nullptr if not there +// Remove this drive from the list of drives with steps due // Called from the step ISR only. -DriveMovement *DDA::RemoveDM(size_t drive) +void DDA::RemoveDM(size_t drive) { DriveMovement **dmp = &firstDM; while (*dmp != nullptr) { - DriveMovement *dm = *dmp; + DriveMovement * const dm = *dmp; if (dm->drive == drive) { (*dmp) = dm->nextDM; - return dm; + break; } dmp = &(dm->nextDM); } - return nullptr; } void DDA::DebugPrintVector(const char *name, const float *vec, size_t len) const @@ -225,7 +236,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) const Move& move = reprap.GetMove(); if (doMotorMapping) { - if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, !nextMove.isCoordinated)) // transform the axis coordinates if on a delta or CoreXY printer + if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, nextMove.isCoordinated)) // transform the axis coordinates if on a delta or CoreXY printer { return false; // throw away the move if it couldn't be transformed } @@ -237,7 +248,6 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) isDeltaMovement = false; } - isPrintingMove = false; xyMoving = false; bool extruding = false; // we set this true if extrusion was commanded, even if it is too small to do bool realMove = false; @@ -338,6 +348,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) isPrintingMove = xyMoving && extruding; usePressureAdvance = nextMove.usePressureAdvance; virtualExtruderPosition = nextMove.virtualExtruderPosition; + proportionRemaining = nextMove.proportionRemaining; hadLookaheadUnderrun = false; isLeadscrewAdjustmentMove = false; goingSlow = false; @@ -417,41 +428,13 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) // for diagonal moves. On a delta, this is not OK and any movement in the XY plane should be limited to the X/Y axis values, which we assume to be equal. if (doMotorMapping) { - switch (reprap.GetMove().GetKinematics().GetKinematicsType()) - { - case KinematicsType::cartesian: - // On a Cartesian printer the axes accelerate independently - break; - - case KinematicsType::coreXY: - case KinematicsType::coreXYU: - // Preferably we would specialise these, but for now fall through to the default implementation - default: - // On other types of printer, the relationship between motor movement and head movement is complex. - // Limit all moves to the lower of X and Y speeds and accelerations. - { - const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); - const float * const maxSpeeds = reprap.GetPlatform().MaxFeedrates(); - const float maxSpeed = min<float>(maxSpeeds[X_AXIS], maxSpeeds[Y_AXIS]); - if (requestedSpeed * xyFactor > maxSpeed) - { - requestedSpeed = maxSpeed/xyFactor; - } - - const float maxAcceleration = min<float>(normalAccelerations[X_AXIS], normalAccelerations[Y_AXIS]); - if (acceleration * xyFactor > maxAcceleration) - { - acceleration = maxAcceleration/xyFactor; - } - } - break; - } + reprap.GetMove().GetKinematics().LimitSpeedAndAcceleration(*this, normalisedDirectionVector); // give the kinematics the chance to further restrict the speed and acceleration } // 7. Calculate the provisional accelerate and decelerate distances and the top speed endSpeed = 0.0; // until the next move asks us to adjust it - if (prev->state != provisional || isPrintingMove != prev->isPrintingMove) + if (prev->state != provisional || isPrintingMove != prev->isPrintingMove || xyMoving != prev->xyMoving) { // There is no previous move that we can adjust, so this move must start at zero speed. startSpeed = 0.0; @@ -460,7 +443,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) { // Try to meld this move to the previous move to avoid stop/start // Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as - prev->targetNextSpeed = min<float>(sqrtf(acceleration * totalDistance * 2.0), requestedSpeed); + prev->targetNextSpeed = sqrtf(acceleration * totalDistance * 2.0); DoLookahead(prev); startSpeed = prev->targetNextSpeed; } @@ -562,61 +545,73 @@ inline bool DDA::IsDecelerationMove() const && prev->decelDistance > 0.0); // if the previous move has no deceleration phase then no point in adjus6ting it } -// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed +// Return true if there is no reason to delay preparing this move +bool DDA::IsGoodToPrepare() const +{ + return endSpeed >= topSpeed; // if it never decelerates, we can't improve it +} + +// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed. +// Only called if this move ands the next one are both printing moves. /*static*/ void DDA::DoLookahead(DDA *laDDA) pre(state == provisional) { // if (reprap.Debug(moduleDda)) debugPrintf("Adjusting, %f\n", laDDA->targetNextSpeed); unsigned int laDepth = 0; - bool goingUp = true; + bool recurse = true; for(;;) // this loop is used to nest lookahead without making recursive calls { - bool recurse = false; - if (goingUp) + if (recurse) { // We have been asked to adjust the end speed of this move to match the next move starting at targetNextSpeed + if (laDDA->targetNextSpeed > laDDA->requestedSpeed) + { + laDDA->targetNextSpeed = laDDA->requestedSpeed; + } if (laDDA->topSpeed >= laDDA->requestedSpeed) { // This move already reaches its top speed, so just need to adjust the deceleration part - laDDA->endSpeed = laDDA->requestedSpeed; // remove the deceleration phase - laDDA->CalcNewSpeeds(); // put it back if necessary + laDDA->endSpeed = laDDA->targetNextSpeed; // ideally, maintain constant speed between the two moves + laDDA->CalcNewSpeeds(); // adjust it if necessary + recurse = false; } else if (laDDA->IsDecelerationMove()) { // This is a deceleration-only move, so we may have to adjust the previous move as well to get optimum behaviour - if (laDDA->prev->state == provisional) + if (laDDA->prev->state == provisional && laDDA->prev->isPrintingMove == laDDA->isPrintingMove && laDDA->prev->xyMoving == laDDA->xyMoving) { - laDDA->endSpeed = laDDA->requestedSpeed; + laDDA->endSpeed = laDDA->targetNextSpeed; laDDA->CalcNewSpeeds(); const float maxStartSpeed = sqrtf(fsquare(laDDA->endSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); - if (maxStartSpeed < laDDA->requestedSpeed) - { - laDDA->prev->targetNextSpeed = maxStartSpeed; - // Still provisionally a decelerate-only move - } - else - { - laDDA->prev->targetNextSpeed = laDDA->requestedSpeed; - } - recurse = true; + laDDA->prev->targetNextSpeed = min<float>(maxStartSpeed, laDDA->requestedSpeed); + // leave 'recurse' true } else { // This move is a deceleration-only move but we can't adjust the previous one laDDA->hadLookaheadUnderrun = true; - laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), - laDDA->requestedSpeed); + laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), laDDA->requestedSpeed); laDDA->CalcNewSpeeds(); + recurse = false; } } else { // This move doesn't reach its requested speed, but it isn't a deceleration-only move // Set its end speed to the minimum of the requested speed and the highest we can reach - laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), - laDDA->requestedSpeed); + const float maxReachableSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); + if (maxReachableSpeed >= laDDA->targetNextSpeed) + { + laDDA->endSpeed = laDDA->targetNextSpeed; + } + else + { + // Looks like this is an acceleration segment, so to ensure smooth acceleration we should reduce targetNextSpeed to endSpeed as well + laDDA->targetNextSpeed = laDDA->endSpeed = maxReachableSpeed; + } laDDA->CalcNewSpeeds(); + recurse = false; } } else @@ -646,7 +641,7 @@ pre(state == provisional) } else { - // Either just stopped going up. or going down + // Either just stopped going up, or going down laDDA->RecalculateMove(); if (laDepth == 0) @@ -657,7 +652,6 @@ pre(state == provisional) laDDA = laDDA->next; --laDepth; - goingUp = false; } } } @@ -758,18 +752,15 @@ float DDA::AdvanceBabyStepping(float amount) pdm->state = DMState::moving; } - if (pdm != nullptr) + if (steps >= 0) { - if (steps >= 0) - { - pdm->direction = true; - pdm->totalSteps = (uint32_t)steps; - } - else - { - pdm->direction = false; - pdm->totalSteps = (uint32_t)(-steps); - } + pdm->direction = true; + pdm->totalSteps = (uint32_t)steps; + } + else + { + pdm->direction = false; + pdm->totalSteps = (uint32_t)(-steps); } } } @@ -785,19 +776,19 @@ float DDA::AdvanceBabyStepping(float amount) // This may cause a move that we intended to be a deceleration-only move to have a tiny acceleration segment at the start void DDA::RecalculateMove() { - accelDistance = (fsquare(requestedSpeed) - fsquare(startSpeed))/(2.0 * acceleration); - decelDistance = (fsquare(requestedSpeed) - fsquare(endSpeed))/(2.0 * acceleration); - if (accelDistance + decelDistance >= totalDistance) + const float accelDiff = fsquare(requestedSpeed) - fsquare(startSpeed); + const float decelDiff = fsquare(requestedSpeed) - fsquare(endSpeed); + if (accelDiff + decelDiff >= acceleration * totalDistance * 2) { - // This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only move. + // This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only or decelerate-only move. // If V is the peak speed, then (V^2 - u^2)/2a + (V^2 - v^2)/2a = distance. // So (2V^2 - u^2 - v^2)/2a = distance // So V^2 = a * distance + 0.5(u^2 + v^2) - float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed)); + const float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed)); // Calculate accelerate distance from: V^2 = u^2 + 2as if (vsquared >= 0.0) { - accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2.0 * acceleration), 0.0); + accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2 * acceleration), 0.0); decelDistance = totalDistance - accelDistance; topSpeed = sqrtf(vsquared); } @@ -811,20 +802,22 @@ void DDA::RecalculateMove() accelDistance = totalDistance; decelDistance = 0.0; topSpeed = endSpeed; - acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2.0 * totalDistance); + acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2 * totalDistance); } else { accelDistance = 0.0; decelDistance = totalDistance; topSpeed = startSpeed; - acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2.0 * totalDistance); + acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2 * totalDistance); } } } else { topSpeed = requestedSpeed; + accelDistance = accelDiff/(2 * acceleration); + decelDistance = decelDiff/(2 * acceleration); } if (canPauseAfter && endSpeed != 0.0) @@ -839,6 +832,12 @@ void DDA::RecalculateMove() } } } + + // We need to set the number of clocks needed here because we use it before the move has been frozen + const float accelStopTime = (topSpeed - startSpeed)/acceleration; + const float decelStartTime = accelStopTime + (totalDistance - accelDistance - decelDistance)/topSpeed; + const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration; + clocksNeeded = (uint32_t)(totalTime * stepClockRate); } // Decide what speed we would really like this move to end at. @@ -963,12 +962,6 @@ pre(disableDeltaMapping || drive < MaxAxes) } } -// Calculate the time needed for this move -float DDA::CalcTime() const -{ - return (float)clocksNeeded/stepClockRate; -} - // Prepare this DDA for execution. // This must not be called with interrupts disabled, because it calls Platform::EnableDrive. void DDA::Prepare(uint8_t simMode) @@ -976,98 +969,26 @@ void DDA::Prepare(uint8_t simMode) PrepParams params; params.decelStartDistance = totalDistance - decelDistance; - // Convert the accelerate/decelerate distances to times - const float accelStopTime = (topSpeed - startSpeed)/acceleration; - const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed; - const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration; - - clocksNeeded = (uint32_t)(totalTime * stepClockRate); - if (simMode == 0) { if (isDeltaMovement) { - // This code used to be in DDA::Init but we moved it here so that we can implement babystepping in it, - // also it speeds up simulation because we no longer execute this code when simulating. - // However, this code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move. + // This code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move. // Therefore the Move code must not store a new move in that entry until this one has been prepared! (It took me ages to track this down.) - // Ideally we would store the initial X any Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build. - a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); + // Ideally we would store the initial X and Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build. cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc); - - const float initialX = prev->GetEndCoordinate(X_AXIS, false); - const float initialY = prev->GetEndCoordinate(Y_AXIS, false); - const LinearDeltaKinematics& dparams = (const LinearDeltaKinematics&)reprap.GetMove().GetKinematics(); - const float diagonalSquared = dparams.GetDiagonalSquared(); - const float a2b2D2 = a2plusb2 * diagonalSquared; - - for (size_t drive = 0; drive < DELTA_AXES; ++drive) - { - const float A = initialX - dparams.GetTowerX(drive); - const float B = initialY - dparams.GetTowerY(drive); - const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); - DriveMovement* const pdm = pddm[drive]; - if (pdm != nullptr) - { - const float aAplusbB = A * directionVector[X_AXIS] + B * directionVector[Y_AXIS]; - const float dSquaredMinusAsquaredMinusBsquared = diagonalSquared - fsquare(A) - fsquare(B); - const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); - pdm->mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2); - pdm->mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2); - pdm->mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = - (int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); - - // Calculate the distance at which we need to reverse direction. - if (a2plusb2 <= 0.0) - { - // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. - pdm->direction = (directionVector[Z_AXIS] >= 0.0); - pdm->reverseStartStep = pdm->totalSteps + 1; - } - else - { - // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, - // the other root corresponds to the carriages being above the bed. - const float drev = ((directionVector[Z_AXIS] * sqrtf(a2b2D2 - fsquare(A * directionVector[Y_AXIS] - B * directionVector[X_AXIS]))) - - aAplusbB)/a2plusb2; - if (drev > 0.0 && drev < totalDistance) // if the reversal point is within range - { - // Calculate how many steps we need to move up before reversing - const float hrev = directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev)); - const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); - - // We may be almost at the peak height already, in which case we don't really have a reversal. - if (numStepsUp < 1 || (pdm->direction && (uint32_t)numStepsUp <= pdm->totalSteps)) - { - pdm->reverseStartStep = pdm->totalSteps + 1; - } - else - { - pdm->reverseStartStep = (uint32_t)numStepsUp + 1; - - // Correct the initial direction and the total number of steps - if (pdm->direction) - { - // Net movement is up, so we will go up a bit and then down by a lesser amount - pdm->totalSteps = (2 * numStepsUp) - pdm->totalSteps; - } - else - { - // Net movement is down, so we will go up first and then down by a greater amount - pdm->direction = true; - pdm->totalSteps = (2 * numStepsUp) + pdm->totalSteps; - } - } - } - else - { - pdm->reverseStartStep = pdm->totalSteps + 1; - } - } - } - } + params.a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); + params.initialX = prev->GetEndCoordinate(X_AXIS, false); + params.initialY = prev->GetEndCoordinate(Y_AXIS, false); + params.dparams = static_cast<const LinearDeltaKinematics*>(&(reprap.GetMove().GetKinematics())); + params.diagonalSquared = params.dparams->GetDiagonalSquared(); + params.a2b2D2 = params.a2plusb2 * params.diagonalSquared; } + // Convert the accelerate/decelerate distances to times + const float accelStopTime = (topSpeed - startSpeed)/acceleration; + const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed; + params.startSpeedTimesCdivA = (uint32_t)((startSpeed * stepClockRate)/acceleration); params.topSpeedTimesCdivA = (uint32_t)((topSpeed * stepClockRate)/acceleration); params.decelStartClocks = (uint32_t)(decelStartTime * stepClockRate); @@ -1138,7 +1059,7 @@ void DDA::Prepare(uint8_t simMode) // Prepare for the first step pdm->nextStep = 0; pdm->nextStepTime = 0; - pdm->stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step + pdm->stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step pdm->stepsTillRecalc = 0; // so that we don't skip the calculation const bool stepsToDo = (isDeltaMovement && drive < numAxes) ? pdm->CalcNextStepTimeDelta(*this, false) @@ -1164,9 +1085,13 @@ void DDA::Prepare(uint8_t simMode) m.accelDistance = accelDistance; m.decelDistance = decelDistance; m.steadyDistance = totalDistance - accelDistance - decelDistance; + m.requestedSpeed = requestedSpeed; m.startSpeed = startSpeed; m.topSpeed = topSpeed; m.endSpeed = endSpeed; + m.targetNextSpeed = targetNextSpeed; + m.endstopChecks = endStopsToCheck; + m.flags = flags; savedMovePointer = (savedMovePointer + 1) % NumSavedMoves; #endif } @@ -1285,7 +1210,7 @@ void DDA::CheckEndstops(Platform& platform) case EndStopHit::lowHit: MoveAborted(); // set the state to completed and recalculate the endpoints reprap.GetGCodes().MoveStoppedByZProbe(); - break; + return; case EndStopHit::lowNear: ReduceHomingSpeed(); @@ -1334,12 +1259,19 @@ void DDA::CheckEndstops(Platform& platform) { case EndStopHit::lowHit: case EndStopHit::highHit: + if ((endStopsToCheck & UseSpecialEndstop) != 0) // use only one (probably non-default) endstop while probing a tool offset + { + MoveAborted(); + return; + } + else { ClearBit(endStopsToCheck, drive); // clear this check so that we can check for more const Kinematics& kin = reprap.GetMove().GetKinematics(); if (endStopsToCheck == 0 || kin.QueryTerminateHomingMove(drive)) { MoveAborted(); // no more endstops to check, or this axis uses shared motors, so stop the entire move + return; } else { @@ -1354,8 +1286,7 @@ void DDA::CheckEndstops(Platform& platform) break; case EndStopHit::lowNear: - // Only reduce homing speed if there are no more axes to be homed. - // This allows us to home X and Y simultaneously. + // Only reduce homing speed if there are no more axes to be homed. This allows us to home X and Y simultaneously. if (endStopsToCheck == MakeBitmap<AxesBitmap>(drive)) { ReduceHomingSpeed(); @@ -1453,7 +1384,7 @@ pre(state == frozen) return true; // schedule another interrupt immediately } -extern uint32_t maxReps; +uint32_t DDA::maxReps = 0; // this holds he maximum ISR loop count // This is called by the interrupt service routine to execute steps. // It returns true if it needs to be called again on the DDA of the new current move, otherwise false. @@ -1564,16 +1495,17 @@ bool DDA::Step() return false; } -// Stop a drive and re-calculate the corresponding endpoint +// Stop a drive and re-calculate the corresponding endpoint. +// For extruder drivers, we need to be able to calculate how much of the extrusion was completed after calling this. void DDA::StopDrive(size_t drive) { DriveMovement* const pdm = pddm[drive]; - if (pdm->state == DMState::moving) + if (pdm != nullptr && pdm->state == DMState::moving) { - endPoint[drive] -= pdm->GetNetStepsLeft(); pdm->state = DMState::idle; if (drive < reprap.GetGCodes().GetTotalAxes()) { + endPoint[drive] -= pdm->GetNetStepsLeft(); endCoordinatesValid = false; // the XYZ position is no longer valid } RemoveDM(drive); @@ -1585,7 +1517,8 @@ void DDA::StopDrive(size_t drive) } // This is called when we abort a move because we have hit an endstop. -// It adjusts the end points of the current move to account for how far through the move we got. +// It stop all drives and adjusts the end points of the current move to account for how far through the move we got. +// The caller must call MoveCompleted at some point after calling this. void DDA::MoveAborted() { if (state == executing) @@ -1598,6 +1531,40 @@ void DDA::MoveAborted() state = completed; } +// Return the approximate (to within 1%) proportion of extrusion for the complete multi-segment move that remains to be done. +// The move was either not started or was aborted. +float DDA::GetProportionDone() const +{ + // Get the proportion of extrusion already done at the start of this segment + unsigned int proportionDone = (filePos != noFilePosition && filePos == prev->filePos) + ? 256 - prev->proportionRemaining + : 0; + if (state == completed) + { + // The move was aborted, so subtract how much was done + const unsigned int proportionDoneAtEnd = 256 - proportionRemaining; + if (proportionDoneAtEnd > proportionDone) + { + int32_t taken = 0, left = 0; + for (size_t drive = reprap.GetGCodes().GetTotalAxes(); drive < DRIVES; ++drive) + { + const DriveMovement* const pdm = pddm[drive]; + if (pdm != nullptr) // if this extruder is active + { + taken += pdm->GetNetStepsTaken(); + left += pdm->GetNetStepsLeft(); + } + } + const int32_t total = taken + left; + if (total > 0) // if the move has net extrusion + { + proportionDone += (((proportionDoneAtEnd - proportionDone) * taken) + (total/2)) / total; + } + } + } + return (float)proportionDone/256.0; +} + // Reduce the speed of this move to the indicated speed. // This is called from the ISR, so interrupts are disabled and nothing else can mess with us. // As this is only called for homing moves and with very low speeds, we assume that we don't need acceleration or deceleration phases. @@ -1611,7 +1578,7 @@ void DDA::ReduceHomingSpeed() for (size_t drive = 0; drive < DRIVES; ++drive) { DriveMovement* const pdm = pddm[drive]; - if (pdm->state == DMState::moving) + if (pdm != nullptr && pdm->state == DMState::moving) { pdm->ReduceSpeed(*this, factor); RemoveDM(pdm->drive); @@ -1657,4 +1624,16 @@ int32_t DDA::GetStepsTaken(size_t drive) const return (dmp != nullptr) ? dmp->GetNetStepsTaken() : 0; } +void DDA::LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration) +{ + if (requestedSpeed > maxSpeed) + { + requestedSpeed = maxSpeed; + } + if (acceleration > maxAcceleration) + { + acceleration = maxAcceleration; + } +} + // End diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index ebb560dc..83a8fc9c 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -41,14 +41,13 @@ public: bool Init(GCodes::RawMove &nextMove, bool doMotorMapping) __attribute__ ((hot)); // Set up a new move, returning true if it represents real movement bool Init(const float_t steps[DRIVES]); // Set up a raw (unmapped) motor move void Init(); // Set up initial positions for machine startup - bool Start(uint32_t tim); // Start executing the DDA, i.e. move the move. + bool Start(uint32_t tim) __attribute__ ((hot)); // Start executing the DDA, i.e. move the move. bool Step() __attribute__ ((hot)); // Take one step of the DDA, called by timed interrupt. void SetNext(DDA *n) { next = n; } void SetPrevious(DDA *p) { prev = p; } void Complete() { state = completed; } bool Free(); void Prepare(uint8_t simMode) __attribute__ ((hot)); // Calculate all the values and freeze this DDA - float CalcTime() const; // Calculate the time needed for this move (used for simulation) bool HasStepError() const; bool CanPauseAfter() const { return canPauseAfter; } bool CanPauseBefore() const { return canPauseBefore; } @@ -71,8 +70,15 @@ public: bool IsHomingAxes() const { return (endStopsToCheck & HomeAxes) != 0; } uint32_t GetXAxes() const { return xAxes; } uint32_t GetYAxes() const { return yAxes; } + float GetTotalDistance() const { return totalDistance; } + void LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration); // Limit the speed an acceleration of this move int32_t GetStepsTaken(size_t drive) const; + float GetProportionDone() const; // Return the proportion of extrusion for the complete multi-segment move already done + + void MoveAborted(); + + bool IsGoodToPrepare() const; #if SUPPORT_IOBITS uint32_t GetMoveStartTime() const { return moveStartTime; } @@ -91,14 +97,14 @@ public: // the calculation can just be managed in time at speeds of 15000mm/min (step interval 50us), but not at 20000mm/min (step interval 37.5us). // Therefore, where the step interval falls below 60us, we don't calculate on every step. // Note: the above measurements were taken some time ago, before some firmware optimisations. -#ifdef DUET_NG +#if SAM4E || SAM4S static constexpr int32_t MinCalcIntervalDelta = (40 * stepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks static constexpr int32_t MinCalcIntervalCartesian = (40 * stepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t minInterruptInterval = 6; // about 2us minimum interval between interrupts, in clocks + static constexpr uint32_t minInterruptInterval = 6; // about 6us minimum interval between interrupts, in step clocks #else static constexpr int32_t MinCalcIntervalDelta = (60 * stepClockRate)/1000000; // the smallest sensible interval between calculations (60us) in step timer clocks static constexpr int32_t MinCalcIntervalCartesian = (60 * stepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t minInterruptInterval = 6; // about 2us minimum interval between interrupts, in clocks + static constexpr uint32_t minInterruptInterval = 4; // about 6us minimum interval between interrupts, in step clocks #endif static void PrintMoves(); // print saved moves for debugging @@ -109,14 +115,15 @@ public: static int32_t loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions]; #endif + static uint32_t maxReps; + private: void RecalculateMove() __attribute__ ((hot)); void CalcNewSpeeds() __attribute__ ((hot)); void ReduceHomingSpeed(); // called to reduce homing speed when a near-endstop is triggered void StopDrive(size_t drive); // stop movement of a drive and recalculate the endpoint - void MoveAborted(); void InsertDM(DriveMovement *dm) __attribute__ ((hot)); - DriveMovement *RemoveDM(size_t drive) __attribute__ ((hot)); + void RemoveDM(size_t drive); void ReleaseDMs(); bool IsDecelerationMove() const; // return true if this move is or have been might have been intended to be a deceleration-only move void DebugPrintVector(const char *name, const float *vec, size_t len) const; @@ -135,16 +142,25 @@ private: DDA *prev; // The previous one in the ring volatile DDAState state; // What state this DDA is in - uint8_t endCoordinatesValid : 1; // True if endCoordinates can be relied on - uint8_t isDeltaMovement : 1; // True if this is a delta printer movement - uint8_t canPauseAfter : 1; // True if we can pause at the end of this move - uint8_t canPauseBefore : 1; // True if we can pause just before this move - uint8_t isPrintingMove : 1; // True if this move includes XY movement and extrusion - uint8_t usePressureAdvance : 1; // True if pressure advance should be applied to any forward extrusion - uint8_t hadLookaheadUnderrun : 1; // True if the lookahead queue was not long enough to optimise this move - uint8_t xyMoving : 1; // True if movement along an X axis or the Y axis was requested, even it if's too small to do - uint8_t goingSlow : 1; // True if we have slowed the movement because the Z probe is approaching its threshold - uint8_t isLeadscrewAdjustmentMove : 1; // True if this is a leadscrews adjustment move + uint8_t proportionRemaining; // What proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete + // - we use a uint8_t instead of a float to save space because it only affects the extrusion amount, so ~0.4% error is acceptable + union + { + struct + { + uint8_t endCoordinatesValid : 1; // True if endCoordinates can be relied on + uint8_t isDeltaMovement : 1; // True if this is a delta printer movement + uint8_t canPauseAfter : 1; // True if we can pause at the end of this move + uint8_t canPauseBefore : 1; // True if we can pause just before this move + uint8_t isPrintingMove : 1; // True if this move includes XY movement and extrusion + uint8_t usePressureAdvance : 1; // True if pressure advance should be applied to any forward extrusion + uint8_t hadLookaheadUnderrun : 1; // True if the lookahead queue was not long enough to optimise this move + uint8_t xyMoving : 1; // True if movement along an X axis or the Y axis was requested, even it if's too small to do + uint8_t goingSlow : 1; // True if we have slowed the movement because the Z probe is approaching its threshold + uint8_t isLeadscrewAdjustmentMove : 1; // True if this is a leadscrews adjustment move + }; + uint16_t flags; // so that we can print all the flags at once for debugging + }; EndstopChecks endStopsToCheck; // Which endstops we are checking on this move AxesBitmap xAxes; // Which axes are behaving as X axes @@ -161,7 +177,6 @@ private: float virtualExtruderPosition; // the virtual extruder position at the end of this move, used for pause/resume // These are used only in delta calculations - float a2plusb2; // Sum of the squares of the X and Y movement fractions int32_t cKc; // The Z movement fraction multiplied by Kc and converted to integer // These vary depending on how we connect the move with its predecessor and successor, but remain constant while the move is being executed diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index a2aa8228..04b98c05 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -10,6 +10,7 @@ #include "Move.h" #include "RepRap.h" #include "Libraries/Math/Isqrt.h" +#include "Kinematics/LinearDeltaKinematics.h" // Static members @@ -46,16 +47,6 @@ DriveMovement *DriveMovement::Allocate(size_t drive, DMState st) return dm; } -void DriveMovement::Release(DriveMovement *item) -{ - if (item != nullptr) - { - item->nextDM = freeList; - freeList = item; - ++numFree; - } -} - // Constructors DriveMovement::DriveMovement(DriveMovement *next) : nextDM(next) { @@ -102,6 +93,64 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) { const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); + const float A = params.initialX - params.dparams->GetTowerX(drive); + const float B = params.initialY - params.dparams->GetTowerY(drive); + const float aAplusbB = A * dda.directionVector[X_AXIS] + B * dda.directionVector[Y_AXIS]; + const float dSquaredMinusAsquaredMinusBsquared = params.diagonalSquared - fsquare(A) - fsquare(B); + const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); + mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2); + mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2); + mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = + (int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); + + // Calculate the distance at which we need to reverse direction. + if (params.a2plusb2 <= 0.0) + { + // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. + direction = (dda.directionVector[Z_AXIS] >= 0.0); + reverseStartStep = totalSteps + 1; + } + else + { + // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, + // the other root corresponds to the carriages being above the bed. + const float drev = ((dda.directionVector[Z_AXIS] * sqrtf(params.a2b2D2 - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS]))) + - aAplusbB)/params.a2plusb2; + if (drev > 0.0 && drev < dda.totalDistance) // if the reversal point is within range + { + // Calculate how many steps we need to move up before reversing + const float hrev = dda.directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); + const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); + + // We may be almost at the peak height already, in which case we don't really have a reversal. + if (numStepsUp < 1 || (direction && (uint32_t)numStepsUp <= totalSteps)) + { + reverseStartStep = totalSteps + 1; + } + else + { + reverseStartStep = (uint32_t)numStepsUp + 1; + + // Correct the initial direction and the total number of steps + if (direction) + { + // Net movement is up, so we will go up a bit and then down by a lesser amount + totalSteps = (2 * numStepsUp) - totalSteps; + } + else + { + // Net movement is down, so we will go up first and then down by a greater amount + direction = true; + totalSteps = (2 * numStepsUp) + totalSteps; + } + } + } + else + { + reverseStartStep = totalSteps + 1; + } + } + mp.delta.twoCsquaredTimesMmPerStepDivAK = (uint32_t)((float)DDA::stepClockRateSquared/(stepsPerMm * dda.acceleration * (K2/2))); // Acceleration phase parameters diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h index 36b44f95..788f495d 100644 --- a/src/Movement/DriveMovement.h +++ b/src/Movement/DriveMovement.h @@ -10,16 +10,29 @@ #include "RepRapFirmware.h" +class LinearDeltaKinematics; + // Struct for passing parameters to the DriveMovement Prepare methods struct PrepParams { + // Parameters used for all types of motion float decelStartDistance; uint32_t startSpeedTimesCdivA; uint32_t topSpeedTimesCdivA; uint32_t decelStartClocks; uint32_t topSpeedTimesCdivAPlusDecelStartClocks; uint32_t accelClocksMinusAccelDistanceTimesCdivTopSpeed; + + // Parameters used only for extruders float compFactor; + + // Parameters used only for delta moves + float initialX; + float initialY; + const LinearDeltaKinematics *dparams; + float diagonalSquared; + float a2plusb2; // sum of the squares of the X and Y movement fractions + float a2b2D2; }; enum class DMState : uint8_t @@ -35,11 +48,11 @@ class DriveMovement public: DriveMovement(DriveMovement *next); - bool CalcNextStepTimeCartesian(const DDA &dda, bool live); - bool CalcNextStepTimeDelta(const DDA &dda, bool live); - void PrepareCartesianAxis(const DDA& dda, const PrepParams& params); - void PrepareDeltaAxis(const DDA& dda, const PrepParams& params); - void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation); + bool CalcNextStepTimeCartesian(const DDA &dda, bool live) __attribute__ ((hot)); + bool CalcNextStepTimeDelta(const DDA &dda, bool live) __attribute__ ((hot)); + void PrepareCartesianAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot)); + void PrepareDeltaAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot)); + void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation) __attribute__ ((hot)); void ReduceSpeed(const DDA& dda, float inverseSpeedFactor); void DebugPrint(char c, bool withDelta) const; int32_t GetNetStepsLeft() const; @@ -53,8 +66,8 @@ public: static void Release(DriveMovement *item); private: - bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live); - bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live); + bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live) __attribute__ ((hot)); + bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live) __attribute__ ((hot)); static DriveMovement *freeList; static int numFree; @@ -144,6 +157,7 @@ inline bool DriveMovement::CalcNextStepTimeCartesian(const DDA &dda, bool live) // Calculate the time since the start of the move when the next step for the specified DriveMovement is due // Return true if there are more steps to do. When finished, leave nextStep == totalSteps + 1. +// We inline this part to speed things up when we are doing double/quad/octal stepping. inline bool DriveMovement::CalcNextStepTimeDelta(const DDA &dda, bool live) { ++nextStep; @@ -201,4 +215,12 @@ inline int32_t DriveMovement::GetNetStepsTaken() const return (direction) ? netStepsTaken : -netStepsTaken; } +// This is inlined because it is only called from one place +inline void DriveMovement::Release(DriveMovement *item) +{ + item->nextDM = freeList; + freeList = item; + ++numFree; +} + #endif /* DRIVEMOVEMENT_H_ */ diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp index db884171..f9a889b1 100644 --- a/src/Movement/Kinematics/CartesianKinematics.cpp +++ b/src/Movement/Kinematics/CartesianKinematics.cpp @@ -19,7 +19,7 @@ const char *CartesianKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates -bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { for (size_t axis = 0; axis < numVisibleAxes; ++axis) { @@ -50,7 +50,14 @@ bool CartesianKinematics::QueryTerminateHomingMove(size_t axis) const void CartesianKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const { const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis); - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); +} + +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds in Cartesian space have already been limited. +void CartesianKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // The axes of a Cartesian printer move independently, so there is nothing to do here } // End diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h index 22051f6f..6b578282 100644 --- a/src/Movement/Kinematics/CartesianKinematics.h +++ b/src/Movement/Kinematics/CartesianKinematics.h @@ -17,11 +17,12 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; HomingMode GetHomingMode() const override { return homeCartesianAxes; } bool QueryTerminateHomingMove(size_t axis) const override; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_CARTESIANKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreBaseKinematics.cpp b/src/Movement/Kinematics/CoreBaseKinematics.cpp index 92bb81b5..f6b74b5e 100644 --- a/src/Movement/Kinematics/CoreBaseKinematics.cpp +++ b/src/Movement/Kinematics/CoreBaseKinematics.cpp @@ -74,7 +74,7 @@ void CoreBaseKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, cons } else { - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); } } diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp index 6f35e321..86d32a33 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYKinematics.cpp @@ -6,6 +6,7 @@ */ #include "CoreXYKinematics.h" +#include "Movement/DDA.h" CoreXYKinematics::CoreXYKinematics() : CoreBaseKinematics(KinematicsType::coreXY) { @@ -18,7 +19,7 @@ const char *CoreXYKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates returning true if successful -bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]); @@ -55,4 +56,24 @@ bool CoreXYKinematics::DriveIsShared(size_t drive) const return drive == X_AXIS || drive == Y_AXIS; } +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds along individual Cartesian axes have already been limited before this is called. +void CoreXYKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + const float vecX = normalisedDirectionVector[0]; + const float vecY = normalisedDirectionVector[1]; + const float vecMax = max<float>(fabs(vecX + vecY), fabs(vecX - vecY)); // pick the case for the motor that is working hardest + if (vecMax > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aX = platform.Acceleration(0); + const float aY = platform.Acceleration(1); + const float vX = platform.MaxFeedrate(0); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMax * (fabs(vecX) * aY + fabs(vecY) * aX)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMax * (fabs(vecX) * vY + fabs(vecY) * vX)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } +} + // End diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h index 4663acc1..711dd3dc 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.h +++ b/src/Movement/Kinematics/CoreXYKinematics.h @@ -17,9 +17,10 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool DriveIsShared(size_t drive) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp index 560037aa..ca327652 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp @@ -7,6 +7,7 @@ #include "CoreXYUKinematics.h" #include "GCodes/GCodes.h" +#include "Movement/DDA.h" const size_t CoreXYU_AXES = 5; const size_t U_AXIS = 3; // X2 @@ -55,7 +56,7 @@ bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef } // Convert Cartesian coordinates to motor coordinates -bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]); @@ -102,4 +103,41 @@ bool CoreXYUKinematics::DriveIsShared(size_t drive) const || drive == V_AXIS; // V doesn't have endstop switches, but include it here just in case } +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds along individual Cartesian axes have already been limited before this is called. +void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + const float vecX = normalisedDirectionVector[0]; + const float vecY = normalisedDirectionVector[1]; + + // Limit the XY motor accelerations + const float vecMaxXY = max<float>(fabs(vecX + vecY), fabs(vecX - vecY)); // pick the case for the motor that is working hardest + if (vecMaxXY > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aX = platform.Acceleration(0); + const float aY = platform.Acceleration(1); + const float vX = platform.MaxFeedrate(0); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMaxXY * (fabs(vecX) * aY + fabs(vecY) * aX)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMaxXY * (fabs(vecX) * vY + fabs(vecY) * vX)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } + + // Limit the UV motor accelerations + const float vecU = normalisedDirectionVector[3]; + const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecY - vecY)); // pick the case for the motor that is working hardest + if (vecMaxUV > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aU = platform.Acceleration(3); + const float aY = platform.Acceleration(1); + const float vU = platform.MaxFeedrate(3); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecX) * aY + fabs(vecY) * aU)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecX) * vY + fabs(vecY) * vU)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } +} + // End diff --git a/src/Movement/Kinematics/CoreXYUKinematics.h b/src/Movement/Kinematics/CoreXYUKinematics.h index 77db9ae9..47a867b0 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.h +++ b/src/Movement/Kinematics/CoreXYUKinematics.h @@ -18,9 +18,10 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool DriveIsShared(size_t drive) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp index d81f13ca..4144aa47 100644 --- a/src/Movement/Kinematics/CoreXZKinematics.cpp +++ b/src/Movement/Kinematics/CoreXZKinematics.cpp @@ -18,7 +18,7 @@ const char *CoreXZKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates -bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Z_AXIS] * axisFactors[Z_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(machinePos[Y_AXIS] * stepsPerMm[Y_AXIS]); @@ -56,4 +56,13 @@ bool CoreXZKinematics::DriveIsShared(size_t drive) const return drive == X_AXIS || drive == Z_AXIS; } +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds in Cartesian space have already been limited. +void CoreXZKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // Ideally we would do something here, but for now we don't. + // This could mean that a simultaneous X and Z movement with Z at maximum speed up and X at 3x that speed would be under-powered, + // but the workaround in that case would be just to lower the maximum Z speed a little, which won't affect printing speed significantly. +} + // End diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h index 3a8d9344..9f9078ef 100644 --- a/src/Movement/Kinematics/CoreXZKinematics.h +++ b/src/Movement/Kinematics/CoreXZKinematics.h @@ -17,11 +17,12 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); } bool DriveIsShared(size_t drive) const override; bool SupportsAutoCalibration() const override { return false; } + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXZKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp index 484f7d35..d85f8d24 100644 --- a/src/Movement/Kinematics/Kinematics.cpp +++ b/src/Movement/Kinematics/Kinematics.cpp @@ -12,6 +12,7 @@ #include "CoreXZKinematics.h" #include "ScaraKinematics.h" #include "CoreXYUKinematics.h" +#include "PolarKinematics.h" #include "RepRap.h" #include "Platform.h" @@ -42,15 +43,15 @@ bool Kinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply // Return true if the specified XY position is reachable by the print head reference point. // This default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 command. -bool Kinematics::IsReachable(float x, float y) const +bool Kinematics::IsReachable(float x, float y, bool isCoordinated) const { const Platform& platform = reprap.GetPlatform(); return x >= platform.AxisMinimum(X_AXIS) && y >= platform.AxisMinimum(Y_AXIS) && x <= platform.AxisMaximum(X_AXIS) && y <= platform.AxisMaximum(Y_AXIS); } -// Limit the Cartesian position that the user wants to move to +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed // This default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. -bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { return LimitPositionFromAxis(coords, 0, numVisibleAxes, axesHomed); } @@ -139,6 +140,8 @@ const char* Kinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alrea return new ScaraKinematics(); case KinematicsType::coreXYU: return new CoreXYUKinematics(); + case KinematicsType::polar: + return new PolarKinematics(); } } diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h index 3d960929..fa9b2e25 100644 --- a/src/Movement/Kinematics/Kinematics.h +++ b/src/Movement/Kinematics/Kinematics.h @@ -26,6 +26,8 @@ enum class KinematicsType : uint8_t linearDelta, scara, coreXYU, + hangprinter, + polar, unknown // this one must be last! }; @@ -52,7 +54,7 @@ public: // Return the name of the current kinematics. // If 'forStatusReport' is true then the string must be the one for that kinematics expected by DuetWebControl and PanelDue. - // Otherwise it should be in a format suitable fore printing. + // Otherwise it should be in a format suitable for printing. // For any new kinematics, the same string can be returned regardless of the parameter. virtual const char *GetName(bool forStatusReport = false) const = 0; @@ -71,7 +73,7 @@ public: // 'numAxes' is the number of machine axes to convert, which will always be at least 3 // 'motorPos' is the output vector of motor positions // Return true if successful, false if we were unable to convert - virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const = 0; + virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const = 0; // Convert motor positions (measured in steps from reference position) to Cartesian coordinates // 'motorPos' is the input vector of motor positions @@ -102,12 +104,12 @@ public: virtual float GetTiltCorrection(size_t axis) const { return 0.0; } // Return true if the specified XY position is reachable by the print head reference point. - // The default implementation assumes a rectangular reachable area, so it just used the bed dimensions give in the M208 commands. - virtual bool IsReachable(float x, float y) const; + // The default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 commands. + virtual bool IsReachable(float x, float y, bool isCoordinated) const; // Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed // The default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. - virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const; + virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const; // Return the set of axes that must have been homed before bed probing is allowed // The default implementation requires just X and Y, but some kinematics require additional axes to be homed (e.g. delta, CoreXZ) @@ -123,7 +125,7 @@ public: virtual size_t NumHomingButtons(size_t numVisibleAxes) const { return numVisibleAxes; } // Override this if the homing buttons are not named after the axes (e.g. SCARA printer) - virtual const char* HomingButtonNames() const { return "XYZUVW"; } + virtual const char* HomingButtonNames() const { return "XYZUVWABC"; } // This function is called when a request is made to home the axes in 'toBeHomed' and the axes in 'alreadyHomed' have already been homed. // If we can proceed with homing some axes, return the name of the homing file to be called. Optionally, update 'alreadyHomed' to indicate @@ -149,6 +151,10 @@ public: // Write any calibration data that we need to resume a print after power fail, returning true if successful. Override where necessary. virtual bool WriteResumeSettings(FileStore *f) const { return true; } + // Limit the speed and acceleration of a move to values that the mechanics can handle. + // The speeds along individual Cartesian axes have already been limited before this is called. + virtual void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const = 0; + // Override this virtual destructor if your constructor allocates any dynamic memory virtual ~Kinematics() { } diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp index 28ed94e4..ecfc093d 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp +++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp @@ -134,7 +134,7 @@ void LinearDeltaKinematics::InverseTransform(float Ha, float Hb, float Hc, float } // Convert Cartesian coordinates to motor steps -bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { //TODO return false if we can't transform the position for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis) @@ -163,20 +163,20 @@ void LinearDeltaKinematics::MotorStepsToCartesian(const int32_t motorPos[], cons } // Return true if the specified XY position is reachable by the print head reference point. -bool LinearDeltaKinematics::IsReachable(float x, float y) const +bool LinearDeltaKinematics::IsReachable(float x, float y, bool isCoordinated) const { return fsquare(x) + fsquare(y) < printRadiusSquared; } // Limit the Cartesian position that the user wants to move to -bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { const AxesBitmap allAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); bool limited = false; if ((axesHomed & allAxes) == allAxes) { // If axes have been homed on a delta printer and this isn't a homing move, check for movements outside limits. - // Skip this check if axes have not been homed, so that extruder-only moved are allowed before homing + // Skip this check if axes have not been homed, so that extruder-only moves are allowed before homing // Constrain the move to be within the build radius const float diagonalSquared = fsquare(coords[X_AXIS]) + fsquare(coords[Y_AXIS]); if (diagonalSquared > printRadiusSquared) @@ -735,7 +735,22 @@ void LinearDeltaKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, c if (highEnd) { const float hitPoint = GetHomedCarriageHeight(axis); - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); + } +} + +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds in Cartesian space have already been limited. +void LinearDeltaKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // Limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration + const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + if (xyFactor > 0.01) + { + const Platform& platform = reprap.GetPlatform(); + const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS)); + const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS)); + dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor); } } diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h index 6a3bdc3f..bc09cb1c 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.h +++ b/src/Movement/Kinematics/LinearDeltaKinematics.h @@ -26,15 +26,15 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool SupportsAutoCalibration() const override { return true; } bool DoAutoCalibration(size_t numFactors, const RandomProbePointSet& probePoints, StringRef& reply) override; void SetCalibrationDefaults() override { Init(); } bool WriteCalibrationParameters(FileStore *f) const override; float GetTiltCorrection(size_t axis) const override; - bool IsReachable(float x, float y) const override; - bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const override; + bool IsReachable(float x, float y, bool isCoordinated) const override; + bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); } MotionType GetMotionType(size_t axis) const override; @@ -45,6 +45,7 @@ public: bool QueryTerminateHomingMove(size_t axis) const override; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; bool WriteResumeSettings(FileStore *f) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; // Public functions specific to this class float GetDiagonalSquared() const { return D2; } diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp new file mode 100644 index 00000000..8d1eb8b8 --- /dev/null +++ b/src/Movement/Kinematics/PolarKinematics.cpp @@ -0,0 +1,260 @@ +/* + * PolarKinematics.cpp + * + * Created on: 13 Oct 2017 + * Author: David + */ + +#include "PolarKinematics.h" + +#include "RepRap.h" +#include "Platform.h" +#include "Storage/MassStorage.h" +#include "GCodes/GCodeBuffer.h" +#include "Movement/DDA.h" + +// Constructor +PolarKinematics::PolarKinematics() + : Kinematics(KinematicsType::polar, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true), + minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0), + maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration) +{ + Recalc(); +} + +// Return the name of the current kinematics. +// If 'forStatusReport' is true then the string must be the one for that kinematics expected by DuetWebControl and PanelDue. +// Otherwise it should be in a format suitable for printing. +// For any new kinematics, the same string can be returned regardless of the parameter. +const char *PolarKinematics::GetName(bool forStatusReport) const +{ + return "Polar"; +} + +// Set or report the parameters from a M665, M666 or M669 command +// If 'mCode' is an M-code used to set parameters for the current kinematics (which should only ever be 665, 666, 667 or 669) +// then search for parameters used to configure the current kinematics. If any are found, perform appropriate actions, +// and return true if the changes affect the geometry. +// If errors were discovered while processing parameters, put an appropriate error message in 'reply' and set 'error' to true. +// If no relevant parameters are found, print the existing ones to 'reply' and return false. +// If 'mCode' does not apply to this kinematics, call the base class version of this function, which will print a suitable error message. +bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) +{ + if (mCode == 669) + { + bool seenNonGeometry = false; + gb.TryGetFValue('S', segmentsPerSecond, seenNonGeometry); + gb.TryGetFValue('T', minSegmentLength, seenNonGeometry); + + bool seen = false; + if (gb.Seen('R')) + { + seen = true; + float radiusLimits[2]; + size_t numRadiusLimits = 2; + gb.GetFloatArray(radiusLimits, numRadiusLimits, false); + if (numRadiusLimits == 2) + { + minRadius = radiusLimits[0]; + maxRadius = radiusLimits[1]; + } + else + { + minRadius = 0.0; + maxRadius = radiusLimits[0]; + } + homedRadius = minRadius; // set up default + } + + gb.TryGetFValue('H', homedRadius, seen); + gb.TryGetFValue('A', maxTurntableAcceleration, seenNonGeometry); + gb.TryGetFValue('F', maxTurntableSpeed, seenNonGeometry); + + if (seen || seenNonGeometry) + { + Recalc(); + } + else + { + reply.printf("Printer mode is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f", + (double)minRadius, (double)maxRadius, (double)homedRadius, + (int)segmentsPerSecond, (double)minSegmentLength); + } + return seen; + } + else + { + return Kinematics::Configure(mCode, gb, reply, error); + } +} + +// Convert Cartesian coordinates to motor positions measured in steps from reference position +// 'machinePos' is a set of axis and extruder positions to convert +// 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree. +// 'numAxes' is the number of machine axes to convert, which will always be at least 3 +// 'motorPos' is the output vector of motor positions +// Return true if successful, false if we were unable to convert +bool PolarKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const +{ + motorPos[0] = lrintf(sqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]); + motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]); + + // Transform remaining axes linearly + for (size_t axis = Z_AXIS; axis < numVisibleAxes; ++axis) + { + motorPos[axis] = lrintf(machinePos[axis] * stepsPerMm[axis]); + } + return true; +} + +// Convert motor positions (measured in steps from reference position) to Cartesian coordinates +// 'motorPos' is the input vector of motor positions +// 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree. +// 'numDrives' is the number of machine drives to convert, which will always be at least 3 +// 'machinePos' is the output set of converted axis and extruder positions +void PolarKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const +{ + const float angle = (motorPos[1] * DegreesToRadians)/stepsPerMm[1]; + const float radius = (float)motorPos[0]/stepsPerMm[0]; + machinePos[0] = radius * cosf(angle); + machinePos[1] = radius * sinf(angle); + + // Convert any additional axes linearly + for (size_t drive = Z_AXIS; drive < numVisibleAxes; ++drive) + { + machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; + } +} + +// Return true if the specified XY position is reachable by the print head reference point. +bool PolarKinematics::IsReachable(float x, float y, bool isCoordinated) const +{ + const float r2 = fsquare(x) + fsquare(y); + return r2 >= minRadiusSquared && r2 <= maxRadiusSquared; +} + +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed +bool PolarKinematics::LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const +{ + const bool m208Limited = Kinematics::LimitPositionFromAxis(position, Z_AXIS, numAxes, axesHomed); // call base class function to limit Z and higher axes + const float r2 = fsquare(position[0]) + fsquare(position[1]); + bool radiusLimited; + if (r2 < minRadiusSquared) + { + radiusLimited = true; + const float r = sqrtf(r2); + if (r < 0.01) + { + position[0] = minRadius; + position[1] = 0.0; + } + else + { + position[0] *= minRadius/r; + position[1] *= minRadius/r; + } + } + else if (r2 > maxRadiusSquared) + { + radiusLimited = true; + const float r = sqrtf(r2); + position[0] *= maxRadius/r; + position[1] *= maxRadius/r; + } + else + { + radiusLimited = false; + } + + return m208Limited || radiusLimited; +} + +// Return the initial Cartesian coordinates we assume after switching to this kinematics +void PolarKinematics::GetAssumedInitialPosition(size_t numAxes, float positions[]) const +{ + for (size_t i = 0; i < numAxes; ++i) + { + positions[i] = 0.0; + } +} + +// Return the axes that we can assume are homed after executing a G92 command to set the specified axis coordinates +AxesBitmap PolarKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const +{ + // If both X and Y have been specified then we know the positions the radius motor and the turntable, otherwise we don't + const AxesBitmap xyAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS); + if ((g92Axes & xyAxes) != xyAxes) + { + g92Axes &= ~xyAxes; + } + return g92Axes; +} + +// This function is called when a request is made to home the axes in 'toBeHomed' and the axes in 'alreadyHomed' have already been homed. +// If we can proceed with homing some axes, return the name of the homing file to be called. Optionally, update 'alreadyHomed' to indicate +// that some additional axes should be considered not homed. +// If we can't proceed because other axes need to be homed first, return nullptr and pass those axes back in 'mustBeHomedFirst'. +const char* PolarKinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const +{ + // Ask the base class which homing file we should call first + const char* ret = Kinematics::GetHomingFileName(toBeHomed, alreadyHomed, numVisibleAxes, mustHomeFirst); + // Change the returned name if it is X or Y + if (ret == StandardHomingFileNames[X_AXIS]) + { + ret = HomeRadiusFileName; + } + else if (ret == StandardHomingFileNames[Y_AXIS]) + { + ret = HomeBedFileName; + } + return ret; +} + +// This function is called from the step ISR when an endstop switch is triggered during homing. +// Return true if the entire homing move should be terminated, false if only the motor associated with the endstop switch should be stopped. +bool PolarKinematics::QueryTerminateHomingMove(size_t axis) const +{ + return false; +} + +// This function is called from the step ISR when an endstop switch is triggered during homing after stopping just one motor or all motors. +// Take the action needed to define the current position, normally by calling dda.SetDriveCoordinate() and return false. +void PolarKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const +{ + switch(axis) + { + case X_AXIS: // radius + dda.SetDriveCoordinate(lrintf(homedRadius * stepsPerMm[axis]), axis); + break; + + case Y_AXIS: // bed + dda.SetDriveCoordinate(0, axis); + break; + + default: + const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); + break; + } +} + +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds in Cartesian space have already been limited. +void PolarKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + const int32_t turntableMovement = labs(dda.DriveCoordinates()[1] - dda.GetPrevious()->DriveCoordinates()[1]); + if (turntableMovement != 0) + { + const float stepRatio = dda.GetTotalDistance() * reprap.GetPlatform().DriveStepsPerUnit(1)/turntableMovement; + dda.LimitSpeedAndAcceleration(stepRatio * maxTurntableSpeed, stepRatio * maxTurntableAcceleration); + } +} + +// Update the derived parameters after the master parameters have been changed +void PolarKinematics::Recalc() +{ + minRadiusSquared = (minRadius <= 0.0) ? 0.0 : fsquare(minRadius); + maxRadiusSquared = fsquare(maxRadius); +} + +// End diff --git a/src/Movement/Kinematics/PolarKinematics.h b/src/Movement/Kinematics/PolarKinematics.h new file mode 100644 index 00000000..fe473d9b --- /dev/null +++ b/src/Movement/Kinematics/PolarKinematics.h @@ -0,0 +1,51 @@ +/* + * PolarKinematics.h + * + * Created on: 13 Oct 2017 + * Author: David + */ + +#ifndef SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ +#define SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ + +#include "Kinematics.h" + +class PolarKinematics : public Kinematics +{ +public: + PolarKinematics(); + + // Overridden base class functions. See Kinematics.h for descriptions. + const char *GetName(bool forStatusReport) const override; + bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; + void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; + bool IsReachable(float x, float y, bool isCoordinated) const override; + bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override; + void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; + const char* HomingButtonNames() const override { return "RTZUVWABC"; } + HomingMode GetHomingMode() const override { return homeIndividualMotors; } + AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override; + const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override; + bool QueryTerminateHomingMove(size_t axis) const override; + void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; + +private: + static constexpr float DefaultSegmentsPerSecond = 100.0; + static constexpr float DefaultMinSegmentSize = 0.2; + static constexpr float DefaultMaxRadius = 150.0; + static constexpr float DefaultMaxTurntableSpeed = 30.0; // degrees per second + static constexpr float DefaultMaxTurntableAcceleration = 30.0; // degrees per second per second + static constexpr const char *HomeRadiusFileName = "homeradius.g"; + static constexpr const char *HomeBedFileName = "homebed.g"; + + void Recalc(); + + float minRadius, maxRadius, homedRadius; + float maxTurntableSpeed, maxTurntableAcceleration; + + float minRadiusSquared, maxRadiusSquared; +}; + +#endif /* SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index a350f42b..9a5580b0 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -12,14 +12,16 @@ #include "GCodes/GCodeBuffer.h" #include "Movement/DDA.h" +#include <limits> + ScaraKinematics::ScaraKinematics() : ZLeadscrewKinematics(KinematicsType::scara, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true), proximalArmLength(DefaultProximalArmLength), distalArmLength(DefaultDistalArmLength), xOffset(0.0), yOffset(0.0) { thetaLimits[0] = DefaultMinTheta; thetaLimits[1] = DefaultMaxTheta; - phiLimits[0] = DefaultMinPhi; - phiLimits[1] = DefaultMaxPhi; + psiLimits[0] = DefaultMinPsi; + psiLimits[1] = DefaultMaxPsi; crosstalk[0] = crosstalk[1] = crosstalk[2] = 0.0; Recalc(); } @@ -30,11 +32,11 @@ const char *ScaraKinematics::GetName(bool forStatusReport) const return "Scara"; } -// Convert Cartesian coordinates to motor coordinates -// In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm -bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const +// Calculate theta, psi and the new arm mode form a target position. +// If the position is not reachable because it is out of radius limits, set theta and psi to NaN and return false. +// Otherwise set theta and psi to the required values and return true if they are in range. +bool ScaraKinematics::CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) const { - // No need to limit x,y to reachable positions here, we already did that in class GCodes const float x = machinePos[X_AXIS] + xOffset; const float y = machinePos[Y_AXIS] + yOffset; const float cosPsi = (fsquare(x) + fsquare(y) - proximalArmLengthSquared - distalArmLengthSquared) / twoPd; @@ -43,40 +45,46 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa const float square = 1.0f - fsquare(cosPsi); if (square < 0.01f) { + theta = psi = std::numeric_limits<float>::quiet_NaN(); return false; // not reachable } + psi = acosf(cosPsi); const float sinPsi = sqrtf(square); - float psi = acosf(cosPsi); const float SCARA_K1 = proximalArmLength + distalArmLength * cosPsi; const float SCARA_K2 = distalArmLength * sinPsi; - float theta; // Try the current arm mode, then the other one bool switchedMode = false; for (;;) { - if (isDefaultArmMode != switchedMode) + if (armMode != switchedMode) { // The following equations choose arm mode 0 i.e. distal arm rotated anticlockwise relative to proximal arm - theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y); - if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) + if (psi >= psiLimits[0] && psi <= psiLimits[1]) { - break; + theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y); + if (theta >= thetaLimits[0] && theta <= thetaLimits[1]) + { + break; + } } } else { // The following equations choose arm mode 1 i.e. distal arm rotated clockwise relative to proximal arm - theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y); - if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) + if ((-psi) >= psiLimits[0] && (-psi) <= psiLimits[1]) { - psi = -psi; - break; + theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y); + if (theta >= thetaLimits[0] && theta <= thetaLimits[1]) + { + psi = -psi; + break; + } } } - if (switchedMode) + if (isCoordinated || switchedMode) { return false; // not reachable } @@ -86,7 +94,31 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa // Now that we know we are going to do the move, update the arm mode if (switchedMode) { - isDefaultArmMode = !isDefaultArmMode; + armMode = !armMode; + } + + return true; +} + +// Convert Cartesian coordinates to motor coordinates, returning true if successful +// In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm +bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const +{ + float theta, psi; + if (machinePos[0] == cachedX && machinePos[1] == cachedY) + { + theta = cachedTheta; + psi = cachedPsi; + currentArmMode = cachedArmMode; + } + else + { + bool armMode = currentArmMode; + if (!CalculateThetaAndPsi(machinePos, isCoordinated, theta, psi, armMode)) + { + return false; + } + currentArmMode = armMode; } //debugPrintf("psi = %.2f, theta = %.2f\n", psi * RadiansToDegrees, theta * RadiansToDegrees); @@ -107,11 +139,11 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa // For Scara, the X and Y components of stepsPerMm are actually steps per degree angle. void ScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { - const float arm1Angle = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians; - const float arm2Angle = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians; + const float psi = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians; + const float theta = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians; - machinePos[X_AXIS] = (cosf(arm1Angle) * proximalArmLength + cosf(arm1Angle + arm2Angle) * distalArmLength) - xOffset; - machinePos[Y_AXIS] = (sinf(arm1Angle) * proximalArmLength + sinf(arm1Angle + arm2Angle) * distalArmLength) - yOffset; + machinePos[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset; + machinePos[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset; // On some machines (e.g. Helios), the X and/or Y arm motors also affect the Z height machinePos[Z_AXIS] = ((float)motorPos[Z_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[1]) + ((float)motorPos[Y_AXIS] * crosstalk[2]))/stepsPerMm[Z_AXIS]; @@ -139,14 +171,17 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& gb.TryGetFValue('Y', yOffset, seen); if (gb.TryGetFloatArray('A', 2, thetaLimits, reply, seen)) { + error = true; return true; } - if (gb.TryGetFloatArray('B', 2, phiLimits, reply, seen)) + if (gb.TryGetFloatArray('B', 2, psiLimits, reply, seen)) { + error = true; return true; } if (gb.TryGetFloatArray('C', 3, crosstalk, reply, seen)) { + error = true; return true; } @@ -159,7 +194,7 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply.printf("Printer mode is Scara with proximal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", distal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", crosstalk %.1f:%.1f:%.1f, bed origin (%.1f, %.1f), segments/sec %d, min. segment length %.2f", (double)proximalArmLength, (double)thetaLimits[0], (double)thetaLimits[1], - (double)distalArmLength, (double)phiLimits[0], (double)phiLimits[1], + (double)distalArmLength, (double)psiLimits[0], (double)psiLimits[1], (double)crosstalk[0], (double)crosstalk[1], (double)crosstalk[2], (double)xOffset, (double)yOffset, (int)segmentsPerSecond, (double)minSegmentLength); @@ -173,57 +208,95 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& } // Return true if the specified XY position is reachable by the print head reference point. -// TODO add an arm mode parameter? -bool ScaraKinematics::IsReachable(float x, float y) const +bool ScaraKinematics::IsReachable(float x, float y, bool isCoordinated) const { - // TODO The following isn't quite right, in particular it doesn't take account of the maximum arm travel - const float r = sqrtf(fsquare(x + xOffset) + fsquare(y + yOffset)); - return r >= minRadius && r <= maxRadius && (x + xOffset) > 0.0; + // Check the M208 limits first + float coords[2] = {x, y}; + if (Kinematics::LimitPosition(coords, 2, LowestNBits<AxesBitmap>(2), isCoordinated)) + { + return false; + } + + // See if we can transform the position + float theta, psi; + bool armMode = currentArmMode; + const bool reachable = CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode); + if (reachable) + { + // Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position + cachedX = x; + cachedY = y; + cachedTheta = theta; + cachedPsi = psi; + cachedArmMode = armMode; + } + + return reachable; } -// Limit the Cartesian position that the user wants to move to -// TODO take account of arm angle limits -bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed +bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { // First limit all axes according to M208 - const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed); - - // Now check that the XY position is within radius limits, in case the M208 limits are too generous - bool radiusLimited = false; - float x = coords[X_AXIS] + xOffset; - float y = coords[Y_AXIS] + yOffset; - const float r2 = fsquare(x) + fsquare(y); - if (r2 < minRadiusSquared) + const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed, isCoordinated); + + float theta, psi; + bool armMode = currentArmMode; + if (CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode)) { - const float r = sqrtf(r2); - // The user may have specified x=0 y=0 so allow for this - if (r < 1.0) + // Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position + cachedX = coords[0]; + cachedY = coords[1]; + cachedTheta = theta; + cachedPsi = psi; + cachedArmMode = armMode; + return m208Limited; + } + + // The requested position was not reachable + if (isnan(theta)) + { + // We are radius-limited + float x = coords[X_AXIS] + xOffset; + float y = coords[Y_AXIS] + yOffset; + const float r = sqrtf(fsquare(x) + fsquare(y)); + if (r < minRadius) { - x = minRadius; - y = 0.0; + // Radius is too small. The user may have specified x=0 y=0 so allow for this. + if (r < 1.0) + { + x = minRadius; + y = 0.0; + } + else + { + x *= minRadius/r; + y *= minRadius/r; + } } else { - x *= minRadius/r; - y *= minRadius/r; + // Radius must be too large + x *= maxRadius/r; + y *= maxRadius/r; } - radiusLimited = true; - } - else if (r2 > maxRadiusSquared) - { - const float r = sqrtf(r2); - x *= maxRadius/r; - y *= maxRadius/r; - radiusLimited = true; - } - if (radiusLimited) - { coords[X_AXIS] = x - xOffset; coords[Y_AXIS] = y - yOffset; } - return m208Limited || radiusLimited; + // Recalculate theta and psi, but don't allow arm mode changes this time + if (!CalculateThetaAndPsi(coords, true, theta, psi, armMode) && !isnan(theta)) + { + // Radius is in range but at least one arm angle isn't + cachedTheta = theta = constrain<float>(theta, thetaLimits[0], thetaLimits[1]); + cachedPsi = psi = constrain<float>(psi, psiLimits[0], psiLimits[1]); + cachedX = coords[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset; + cachedY = coords[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset; + cachedArmMode = currentArmMode; + } + + return true; } // Return the initial Cartesian coordinates we assume after switching to this kinematics @@ -302,16 +375,33 @@ bool ScaraKinematics::QueryTerminateHomingMove(size_t axis) const // This function is called from the step ISR when an endstop switch is triggered during homing after stopping just one motor or all motors. // Take the action needed to define the current position, normally by calling dda.SetDriveCoordinate() and return false. +//TODO this doesn't appear to take account of the crosstalk factors yet void ScaraKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const { const float hitPoint = (axis == 0) ? ((highEnd) ? thetaLimits[1] : thetaLimits[0]) // proximal joint homing switch : (axis == 1) - ? ((highEnd) ? phiLimits[1] : phiLimits[0]) // distal joint homing switch + ? ((highEnd) ? psiLimits[1] : psiLimits[0]) // distal joint homing switch : (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) // other axis (e.g. Z) high end homing switch : reprap.GetPlatform().AxisMinimum(axis); // other axis (e.g. Z) low end homing switch - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); +} + +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds in Cartesian space have already been limited. +void ScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration. + // Limiting the angular rates of the arms would be better. + const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + if (xyFactor > 0.01) + { + const Platform& platform = reprap.GetPlatform(); + const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS)); + const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS)); + dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor); + } } // Recalculate the derived parameters @@ -321,20 +411,22 @@ void ScaraKinematics::Recalc() distalArmLengthSquared = fsquare(distalArmLength); twoPd = proximalArmLength * distalArmLength * 2.0f; - minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(phiLimits[0] * DegreesToRadians), cosf(phiLimits[1] * DegreesToRadians))) * 1.005; - if (phiLimits[0] < 0.0 && phiLimits[1] > 0.0) + minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005; + if (psiLimits[0] < 0.0 && psiLimits[1] > 0.0) { // Zero distal arm angle is reachable maxRadius = proximalArmLength + distalArmLength; } else { - const float minAngle = min<float>(fabs(phiLimits[0]), fabs(phiLimits[1])) * DegreesToRadians; + const float minAngle = min<float>(fabs(psiLimits[0]), fabs(psiLimits[1])) * DegreesToRadians; maxRadius = sqrtf(proximalArmLengthSquared + distalArmLengthSquared + (twoPd * cosf(minAngle))); } maxRadius *= 0.995; minRadiusSquared = fsquare(minRadius); maxRadiusSquared = fsquare(maxRadius); + + cachedX = cachedY = std::numeric_limits<float>::quiet_NaN(); // make sure that the cached values won't match any coordinates } // End diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h index e229468d..43e1807b 100644 --- a/src/Movement/Kinematics/ScaraKinematics.h +++ b/src/Movement/Kinematics/ScaraKinematics.h @@ -28,18 +28,19 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; - bool IsReachable(float x, float y) const override; - bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed) const override; + bool IsReachable(float x, float y, bool isCoordinated) const override; + bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; size_t NumHomingButtons(size_t numVisibleAxes) const override; - const char* HomingButtonNames() const override { return "PDZUVW"; } + const char* HomingButtonNames() const override { return "PDZUVWABC"; } HomingMode GetHomingMode() const override { return homeIndividualMotors; } AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override; const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override; bool QueryTerminateHomingMove(size_t axis) const override; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; private: static constexpr float DefaultSegmentsPerSecond = 100.0; @@ -48,19 +49,20 @@ private: static constexpr float DefaultDistalArmLength = 100.0; static constexpr float DefaultMinTheta = -90.0; // minimum proximal joint angle static constexpr float DefaultMaxTheta = 90.0; // maximum proximal joint angle - static constexpr float DefaultMinPhi = -135.0; // minimum distal joint angle - static constexpr float DefaultMaxPhi = 135.0; // maximum distal joint angle + static constexpr float DefaultMinPsi = -135.0; // minimum distal joint angle + static constexpr float DefaultMaxPsi = 135.0; // maximum distal joint angle static constexpr const char *HomeProximalFileName = "homeproximal.g"; static constexpr const char *HomeDistalFileName = "homedistal.g"; void Recalc(); + bool CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) const; // Primary parameters float proximalArmLength; float distalArmLength; float thetaLimits[2]; // minimum proximal joint angle - float phiLimits[2]; // minimum distal joint angle + float psiLimits[2]; // minimum distal joint angle float crosstalk[3]; // if we rotate the distal arm motor, for each full rotation the Z height goes up by this amount float xOffset; // where bed X=0 is relative to the proximal joint float yOffset; // where bed Y=0 is relative to the proximal joint @@ -73,7 +75,8 @@ private: float twoPd; // State variables - mutable bool isDefaultArmMode; // this should be moved into class Move when it knows about different arm modes + mutable float cachedX, cachedY, cachedTheta, cachedPsi; + mutable bool currentArmMode, cachedArmMode; }; #endif /* SRC_MOVEMENT_KINEMATICS_SCARAKINEMATICS_H_ */ diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index 8096ae79..4304db5d 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -7,13 +7,12 @@ #include "Move.h" #include "Platform.h" -#include "RepRap.h" -#include "Kinematics/LinearDeltaKinematics.h" // temporary -Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0) -{ - active = false; +static constexpr uint32_t UsualMinimumPreparedTime = DDA::stepClockRate/10; // 100ms +static constexpr uint32_t AbsoluteMinimumPreparedTime = DDA::stepClockRate/20; // 50ms +Move::Move() : currentDda(nullptr), active(false), scheduledMoves(0), completedMoves(0) +{ kinematics = Kinematics::Create(KinematicsType::cartesian); // default to Cartesian // Build the DDA ring @@ -21,7 +20,7 @@ Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0) ddaRingGetPointer = ddaRingAddPointer = dda; for (size_t i = 1; i < DdaRingLength; i++) { - DDA *oldDda = dda; + DDA * const oldDda = dda; dda = new DDA(dda); oldDda->SetPrevious(dda); } @@ -74,13 +73,13 @@ void Move::Init() longWait = millis(); idleTimeout = DefaultIdleTimeout; - iState = IdleState::idle; + moveState = MoveState::idle; idleCount = 0; simulationMode = 0; simulationTime = 0.0; longestGcodeWaitInterval = 0; - waitingForMove = specialMoveAvailable = false; + specialMoveAvailable = false; active = true; } @@ -119,9 +118,10 @@ void Move::Spin() ++idleCount; } - // Check for DDA errors to print if Move debug is enabled + // Recycle the DDAs for completed moves, checking for DDA errors to print if Move debug is enabled while (ddaRingCheckPointer->GetState() == DDA::completed) { + // Check for step errors and record/print them if we have any, before we lose the DMs if (ddaRingCheckPointer->HasStepError()) { if (reprap.Debug(moduleMove)) @@ -131,6 +131,8 @@ void Move::Spin() ++stepErrors; reprap.GetPlatform().LogError(ErrorCode::BadMove); } + + // Now release the DMs and check for underrun if (ddaRingCheckPointer->Free()) { ++numLookaheadUnderruns; @@ -139,21 +141,21 @@ void Move::Spin() } // See if we can add another move to the ring - if ( + bool canAddMove = ( #if SUPPORT_ROLAND - !reprap.GetRoland()->Active() && + !reprap.GetRoland()->Active() && #endif - ddaRingAddPointer->GetState() == DDA::empty - && ddaRingAddPointer->GetNext()->GetState() != DDA::provisional // function Prepare needs to access the endpoints in the previous move, so don't change them - && DriveMovement::NumFree() >= (int)DRIVES // check that we won't run out of DMs - ) + ddaRingAddPointer->GetState() == DDA::empty + && ddaRingAddPointer->GetNext()->GetState() != DDA::provisional // function Prepare needs to access the endpoints in the previous move, so don't change them + && DriveMovement::NumFree() >= (int)DRIVES // check that we won't run out of DMs + ); + if (canAddMove) { // In order to react faster to speed and extrusion rate changes, only add more moves if the total duration of - // all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is - // less than 0.5 seconds. + // all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is less than 0.5 seconds. const DDA *dda = ddaRingAddPointer; - float unPreparedTime = 0.0; - float prevMoveTime = 0.0; + uint32_t unPreparedTime = 0; + uint32_t prevMoveTime = 0; for(;;) { dda = dda->GetPrevious(); @@ -162,119 +164,124 @@ void Move::Spin() break; } unPreparedTime += prevMoveTime; - prevMoveTime = dda->CalcTime(); + prevMoveTime = dda->GetClocksNeeded(); } - if (unPreparedTime < 0.5 || unPreparedTime + prevMoveTime < 2.0) + canAddMove = (unPreparedTime < DDA::stepClockRate/2 || unPreparedTime + prevMoveTime < 2 * DDA::stepClockRate); + } + + if (canAddMove) + { + // OK to add another move. First check if a special move is available. + if (specialMoveAvailable) { - // First check for a special move - if (specialMoveAvailable) + if (simulationMode < 2) { - if (simulationMode < 2) + if (ddaRingAddPointer->Init(specialMoveCoords)) { - if (ddaRingAddPointer->Init(specialMoveCoords)) + ddaRingAddPointer = ddaRingAddPointer->GetNext(); + if (moveState == MoveState::idle || moveState == MoveState::timing) { - ddaRingAddPointer = ddaRingAddPointer->GetNext(); - idleCount = 0; + // We were previously idle, so we have a state change + moveState = MoveState::collecting; + const uint32_t now = millis(); + const uint32_t timeWaiting = now - lastStateChangeTime; + if (timeWaiting > longestGcodeWaitInterval) + { + longestGcodeWaitInterval = timeWaiting; + } + lastStateChangeTime = now; } } - specialMoveAvailable = false; } - else + specialMoveAvailable = false; + } + else + { + // If there's a G Code move available, add it to the DDA ring for processing. + GCodes::RawMove nextMove; + if (reprap.GetGCodes().ReadMove(nextMove)) // if we have a new move { - // If there's a G Code move available, add it to the DDA ring for processing. - GCodes::RawMove nextMove; - if (reprap.GetGCodes().ReadMove(nextMove)) // if we have a new move + if (simulationMode < 2) // in simulation mode 2 and higher, we don't process incoming moves beyond this point { - if (waitingForMove) +#if 0 // disabled this because it causes jerky movements on the SCARA printer + // Add on the extrusion left over from last time. + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); + for (size_t drive = numAxes; drive < DRIVES; ++drive) { - waitingForMove = false; - const uint32_t timeWaiting = millis() - gcodeWaitStartTime; - if (timeWaiting > longestGcodeWaitInterval) - { - longestGcodeWaitInterval = timeWaiting; - } + nextMove.coords[drive] += extrusionPending[drive - numAxes]; } - if (simulationMode < 2) // in simulation mode 2 and higher, we don't process incoming moves beyond this point - { -#if 0 // disabled this because it causes jerky movements on the SCARA printer - // Add on the extrusion left over from last time. - const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); - for (size_t drive = numAxes; drive < DRIVES; ++drive) - { - nextMove.coords[drive] += extrusionPending[drive - numAxes]; - } #endif - if (nextMove.moveType == 0) - { - AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true); - } - if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType))) - { - ddaRingAddPointer = ddaRingAddPointer->GetNext(); - idleCount = 0; - scheduledMoves++; - } -#if 0 // see above - // Save the amount of extrusion not done - for (size_t drive = numAxes; drive < DRIVES; ++drive) + if (nextMove.moveType == 0) + { + AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true); + } + if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType))) + { + ddaRingAddPointer = ddaRingAddPointer->GetNext(); + idleCount = 0; + scheduledMoves++; + if (moveState == MoveState::idle || moveState == MoveState::timing) { - extrusionPending[drive - numAxes] = nextMove.coords[drive]; + moveState = MoveState::collecting; + const uint32_t now = millis(); + const uint32_t timeWaiting = now - lastStateChangeTime; + if (timeWaiting > longestGcodeWaitInterval) + { + longestGcodeWaitInterval = timeWaiting; + } + lastStateChangeTime = now; } -#endif } - } - else - { - // We wanted another move, but none was available - if (currentDda != nullptr && !waitingForMove) +#if 0 // see above + // Save the amount of extrusion not done + for (size_t drive = numAxes; drive < DRIVES; ++drive) { - gcodeWaitStartTime = millis(); - waitingForMove = true; + extrusionPending[drive - numAxes] = nextMove.coords[drive]; } +#endif } } } } + // See whether we need to kick off a move if (currentDda == nullptr) { // No DDA is executing, so start executing a new one if possible - if (idleCount > 10) // better to have a few moves in the queue so that we can do lookahead + if (!canAddMove || idleCount > 10) // better to have a few moves in the queue so that we can do lookahead { - DDA *dda = ddaRingGetPointer; + // Prepare one move and execute it. We assume that we will enter the next if-block before it completes, giving us time to prepare more moves. + Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure + DDA * const dda = ddaRingGetPointer; // capture volatile variable if (dda->GetState() == DDA::provisional) { dda->Prepare(simulationMode); - } - if (dda->GetState() == DDA::frozen) - { if (simulationMode != 0) { currentDda = dda; // pretend we are executing this move } else { - Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure if (StartNextMove(Platform::GetInterruptClocks())) // start the next move { Interrupt(); } } - iState = IdleState::busy; + moveState = MoveState::executing; } else if (simulationMode != 0) { - if (iState == IdleState::busy && !reprap.GetGCodes().IsPaused()) + if (moveState == MoveState::executing && !reprap.GetGCodes().IsPaused()) { - lastMoveTime = millis(); // record when we first noticed that the machine was idle - iState = IdleState::timing; + lastStateChangeTime = millis(); // record when we first noticed that the machine was idle + moveState = MoveState::timing; } - else if (iState == IdleState::timing && millis() - lastMoveTime >= idleTimeout) + else if (moveState == MoveState::timing && millis() - lastStateChangeTime >= idleTimeout) { reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold - iState = IdleState::idle; + moveState = MoveState::idle; } } } @@ -283,7 +290,7 @@ void Move::Spin() DDA *cdda = currentDda; // currentDda is volatile, so copy it if (cdda != nullptr) { - // See whether we need to prepare any moves + // See whether we need to prepare any moves. First count how many prepared or executing moves we have and how long they will take. int32_t preparedTime = 0; uint32_t preparedCount = 0; DDA::DDAState st; @@ -298,13 +305,17 @@ void Move::Spin() } } - // If the number of prepared moves will execute in less than the minimum time, prepare another move + // If the number of prepared moves will execute in less than the minimum time, prepare another move. + // Try to avoid preparing deceleration-only moves while (st == DDA::provisional - && preparedTime < (int32_t)(DDA::stepClockRate/8) // prepare moves one eighth of a second ahead of when they will be needed - && preparedCount < DdaRingLength/2 // but don't prepare more than half the ring + && preparedTime < (int32_t)UsualMinimumPreparedTime // prepare moves one eighth of a second ahead of when they will be needed + && preparedCount < DdaRingLength/2 - 1 // but don't prepare as much as half the ring ) { - cdda->Prepare(simulationMode); + if (cdda->IsGoodToPrepare() || preparedTime < (int32_t)AbsoluteMinimumPreparedTime) + { + cdda->Prepare(simulationMode); + } preparedTime += cdda->GetTimeLeft(); ++preparedCount; cdda = cdda->GetNext(); @@ -317,7 +328,7 @@ void Move::Spin() // Simulate completion of the current move //DEBUG //currentDda->DebugPrint(); - simulationTime += currentDda->CalcTime(); + simulationTime += (float)currentDda->GetClocksNeeded()/DDA::stepClockRate; currentDda->Complete(); CurrentMoveCompleted(); } @@ -361,13 +372,10 @@ bool Move::IsRawMotorMove(uint8_t moveType) const bool Move::IsAccessibleProbePoint(float x, float y) const { const ZProbeParameters& params = reprap.GetPlatform().GetCurrentZProbeParameters(); - return kinematics->IsReachable(x - params.xOffset, y - params.yOffset); + return kinematics->IsReachable(x - params.xOffset, y - params.yOffset, false); } -// Pause the print as soon as we can, returning true if we are able to. -// Returns the file position of the first queue move we are going to skip, or noFilePosition we we are not skipping any moves. -// We update 'positions' to the positions and feed rate expected for the next move, and the amount of extrusion in the moves we skipped. -// If we are not skipping any moves then the feed rate, virtual extruder position and iobits are left alone, therefore the caller should set them up first. +// Pause the print as soon as we can, returning true if we are able to skip any moves and updating 'rp' to the first move we skipped. bool Move::PausePrint(RestorePoint& rp) { // Find a move we can pause after. @@ -393,7 +401,7 @@ bool Move::PausePrint(RestorePoint& rp) // In general, we can pause after a move if it is the last segment and its end speed is slow enough. // We can pause before a move if it is the first segment in that move. - const DDA *savedDdaRingAddPointer = ddaRingAddPointer; + const DDA * const savedDdaRingAddPointer = ddaRingAddPointer; bool pauseOkHere; cpu_irq_disable(); @@ -414,7 +422,6 @@ bool Move::PausePrint(RestorePoint& rp) if (pauseOkHere && dda->CanPauseBefore()) { // We can pause before executing this move - (void)dda->Free(); // free the DDA we are going to pause before it so that it won't get executed after we enable interrupts ddaRingAddPointer = dda; break; } @@ -460,7 +467,79 @@ bool Move::PausePrint(RestorePoint& rp) return true; } -uint32_t maxReps = 0; +// Pause the print immediately, returning true if we were able to skip or abort any moves and setting up to the move we aborted +bool Move::LowPowerPause(RestorePoint& rp) +{ + const DDA * const savedDdaRingAddPointer = ddaRingAddPointer; + bool abortedMove = false; + + cpu_irq_disable(); + DDA *dda = currentDda; + if (dda != nullptr && dda->GetFilePosition() != noFilePosition) + { + // We are executing a move that has a file address, so we can interrupt it + Platform::DisableStepInterrupt(); + dda->MoveAborted(); + CurrentMoveCompleted(); // updates live endpoints, extrusion, ddaRingGetPointer, currentDda etc. + --completedMoves; // this move wasn't really completed + abortedMove = true; + } + else + { + if (dda == nullptr) + { + // No move is being executed + dda = ddaRingGetPointer; + } + while (dda != savedDdaRingAddPointer) + { + if (dda->GetFilePosition() != noFilePosition) + { + break; // we can pause before executing this move + } + dda = dda->GetNext(); + } + } + + cpu_irq_enable(); + + if (dda == savedDdaRingAddPointer) + { + return false; // we can't skip any moves + } + + // We are going to skip some moves, or part of a move. + // Store the parameters of the first move we are going to execute when we resume + rp.feedRate = dda->GetRequestedSpeed(); + rp.virtualExtruderPosition = dda->GetVirtualExtruderPosition(); + rp.filePos = dda->GetFilePosition(); + rp.proportionDone = dda->GetProportionDone(); // store how much of the complete multi-segment move's extrusion has been done + +#if SUPPORT_IOBITS + rp.ioBits = dda->GetIoBits(); +#endif + + ddaRingAddPointer = (abortedMove) ? dda->GetNext() : dda; + + // Get the end coordinates of the last move that was or will be completed, or the coordinates of the current move when we aborted it. + DDA * const prevDda = ddaRingAddPointer->GetPrevious(); + const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes(); + for (size_t axis = 0; axis < numVisibleAxes; ++axis) + { + rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false); + } + + InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes()); // we assume that xAxes and yAxes have't changed between the moves + + // Free the DDAs for the moves we are going to skip + for (dda = ddaRingAddPointer; dda != savedDdaRingAddPointer; dda = dda->GetNext()) + { + (void)dda->Free(); + scheduledMoves--; + } + + return true; +} #if 0 // For debugging @@ -473,8 +552,8 @@ void Move::Diagnostics(MessageType mtype) Platform& p = reprap.GetPlatform(); p.Message(mtype, "=== Move ===\n"); p.MessageF(mtype, "MaxReps: %" PRIu32 ", StepErrors: %u, FreeDm: %d, MinFreeDm %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n", - maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns); - maxReps = 0; + DDA::maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns); + DDA::maxReps = 0; numLookaheadUnderruns = numPrepareUnderruns = 0; longestGcodeWaitInterval = 0; DriveMovement::ResetMinFree(); @@ -584,10 +663,10 @@ void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes // Convert Cartesian coordinates to motor steps, axes only, returning true if successful. // Used to perform movement and G92 commands. -bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const +bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) const { const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), - reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, allowModeChange); + reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, isCoordinated); if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda)) { if (b) @@ -861,9 +940,9 @@ void Move::AdjustMotorPositions(const float_t adjustment[], size_t numMotors) const int32_t * const endCoordinates = lastQueuedMove->DriveCoordinates(); const float * const driveStepsPerUnit = reprap.GetPlatform().GetDriveStepsPerUnit(); - for (size_t drive = 0; drive < DELTA_AXES; ++drive) + for (size_t drive = 0; drive < numMotors; ++drive) { - const int32_t ep = endCoordinates[drive] + (int32_t)(adjustment[drive] * driveStepsPerUnit[drive]); + const int32_t ep = endCoordinates[drive] + lrintf(adjustment[drive] * driveStepsPerUnit[drive]); lastQueuedMove->SetDriveCoordinate(ep, drive); liveEndPoints[drive] = ep; } diff --git a/src/Movement/Move.h b/src/Movement/Move.h index ac145c41..38e55e5e 100644 --- a/src/Movement/Move.h +++ b/src/Movement/Move.h @@ -21,7 +21,7 @@ // Each DDA needs one DM per drive that it moves. // However, DM's are large, so we provide fewer than DRIVES * DdaRingLength of them. The planner checks that enough DMs are available before filling in a new DDA. -#ifdef DUET_NG +#if SAM4E || SAM4S const unsigned int DdaRingLength = 30; const unsigned int NumDms = DdaRingLength * 8; // suitable for e.g. a delta + 5 input hot end #else @@ -46,10 +46,9 @@ public: // Return the position (after all queued moves have been executed) in transformed coords int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; } // Get the current position of a motor void LiveCoordinates(float m[DRIVES], AxesBitmap xAxes, AxesBitmap yAxes); // Gives the last point at the end of the last complete DDA transformed to user coords - void Interrupt(); // The hardware's (i.e. platform's) interrupt should call this. - void InterruptTime(); // Test function - not used + void Interrupt() __attribute__ ((hot)); // The hardware's (i.e. platform's) interrupt should call this. bool AllMovesAreFinished(); // Is the look-ahead ring empty? Stops more moves being added as well. - void DoLookAhead(); // Run the look-ahead procedure + void DoLookAhead() __attribute__ ((hot)); // Run the look-ahead procedure void SetNewPosition(const float positionNow[DRIVES], bool doBedCompensation); // Set the current position to be this void SetLiveCoordinates(const float coords[DRIVES]); // Force the live coordinates (see above) to be these void ResetExtruderPositions(); // Resets the extrusion amounts of the live coordinates @@ -75,7 +74,7 @@ public: // Kinematics and related functions Kinematics& GetKinematics() const { return *kinematics; } bool SetKinematics(KinematicsType k); // Set kinematics, return true if successful - bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const; + bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) const; // Convert Cartesian coordinates to delta motor coordinates, return true if successful void MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const; // Convert motor coordinates to machine coordinates @@ -90,8 +89,8 @@ public: bool IsRawMotorMove(uint8_t moveType) const; // Return true if this is a raw motor move - void CurrentMoveCompleted(); // Signal that the current move has just been completed - bool TryStartNextMove(uint32_t startTime); // Try to start another move, returning true if Step() needs to be called immediately + void CurrentMoveCompleted() __attribute__ ((hot)); // Signal that the current move has just been completed + bool TryStartNextMove(uint32_t startTime) __attribute__ ((hot)); // Try to start another move, returning true if Step() needs to be called immediately float IdleTimeout() const; // Returns the idle timeout in seconds void SetIdleTimeout(float timeout); // Set the idle timeout in seconds @@ -100,6 +99,8 @@ public: void PrintCurrentDda() const; // For debugging bool PausePrint(RestorePoint& rp); // Pause the print as soon as we can, returning true if we were able to + bool LowPowerPause(RestorePoint& rp); // Pause the print immediately, returning true if we were able to + bool NoLiveMovement() const; // Is a move running, or are there any queued? bool IsExtruding() const; // Is filament being extruded? @@ -122,9 +123,15 @@ public: static float MotorEndpointToPosition(int32_t endpoint, size_t drive); // Convert number of motor steps to motor position private: - enum class IdleState : uint8_t { idle, busy, timing }; + enum class MoveState : uint8_t + { + idle, // no moves being executed or in queue, motors are at idle hold + collecting, // no moves currently being executed but we are collecting moves ready to execute them + executing, // we are executing moves + timing // no moves being executed or in queue, motors are at full current + }; - bool StartNextMove(uint32_t startTime); // Start the next move, returning true if Step() needs to be called immediately + bool StartNextMove(uint32_t startTime) __attribute__ ((hot)); // Start the next move, returning true if Step() needs to be called immediately void BedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the bed compensations void InverseBedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Go from a bed-transformed point back to user coordinates void AxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the axis-angle compensations @@ -142,12 +149,12 @@ private: bool active; // Are we live and running? uint8_t simulationMode; // Are we simulating, or really printing? - bool waitingForMove; // True if we are waiting for a new move + MoveState moveState; // whether the idle timer is active + unsigned int numLookaheadUnderruns; // How many times we have run out of moves to adjust during lookahead unsigned int numPrepareUnderruns; // How many times we wanted a new move but there were only un-prepared moves in the queue unsigned int idleCount; // The number of times Spin was called and had no new moves to process - uint32_t longestGcodeWaitInterval; // the longest we had to wait for a new gcode - uint32_t gcodeWaitStartTime; // When we last asked for a gcode and didn't get one + uint32_t longestGcodeWaitInterval; // the longest we had to wait for a new GCode float simulationTime; // Print time since we started simulating float extrusionPending[MaxExtruders]; // Extrusion not done due to rounding to nearest step @@ -170,9 +177,8 @@ private: float taperHeight; // Height over which we taper uint32_t idleTimeout; // How long we wait with no activity before we reduce motor currents to idle, in milliseconds - uint32_t lastMoveTime; // The approximate time at which the last move was completed + uint32_t lastStateChangeTime; // The approximate time at which the state last changed, except we don't record timing->idle uint32_t longWait; // A long time for things that need to be done occasionally - IdleState iState; // whether the idle timer is active Kinematics *kinematics; // What kinematics we are using diff --git a/src/Platform.cpp b/src/Platform.cpp index d1b72425..b9a79ad4 100644 --- a/src/Platform.cpp +++ b/src/Platform.cpp @@ -31,14 +31,18 @@ #include "Scanner.h" #include "Version.h" #include "SoftTimer.h" +#include "Logger.h" #include "Libraries/Math/Isqrt.h" #include "sam/drivers/tc/tc.h" #include "sam/drivers/hsmci/hsmci.h" #include "sd_mmc.h" -#ifdef DUET_NG +#if HAS_SMART_DRIVERS # include "TMC2660.h" +#endif + +#ifdef DUET_NG # include "FirmwareUpdater.h" #endif @@ -52,7 +56,7 @@ extern "C" char *sbrk(int i); # error Missing feature definition #endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR inline constexpr float AdcReadingToPowerVoltage(uint16_t adcVal) { @@ -254,10 +258,12 @@ bool ZProbeParameters::WriteParameters(FileStore *f, unsigned int probeType) con //************************************************************************************************* // Platform class +uint8_t Platform::softwareResetDebugInfo = 0; // extra info for debugging + Platform::Platform() : - logger(nullptr), lastLogFlushTime(0), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0), lastFanCheckTime(0), + logger(nullptr), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0), lastFanCheckTime(0), auxGCodeReply(nullptr), fileStructureInitialised(false), tickState(0), debugCode(0) -#ifdef DUET_NG +#if HAS_SMART_DRIVERS , lastWarningMillis(0), nextDriveToPoll(0), onBoardDriversFanRunning(false), offBoardDriversFanRunning(false), onBoardDriversFanStartMillis(0), offBoardDriversFanStartMillis(0) #endif @@ -411,6 +417,7 @@ void Platform::Init() // AXES ARRAY_INIT(axisMaxima, AXIS_MAXIMA); ARRAY_INIT(axisMinima, AXIS_MINIMA); + axisMaximaProbed = axisMinimaProbed = 0; idleCurrentFactor = DefaultIdleCurrentFactor; @@ -436,7 +443,7 @@ void Platform::Init() for (size_t drive = 0; drive < DRIVES; drive++) { - enableValues[drive] = false; // assume active low enable signal + enableValues[drive] = 0; // assume active low enable signal directions[drive] = true; // drive moves forwards by default // Map axes and extruders straight through @@ -501,22 +508,23 @@ void Platform::Init() switch (expansionBoard) { case ExpansionBoardType::DueX2: - numTMC2660Drivers = 7; + numSmartDrivers = 7; break; case ExpansionBoardType::DueX5: - numTMC2660Drivers = 10; + numSmartDrivers = 10; break; case ExpansionBoardType::none: case ExpansionBoardType::DueX0: default: - numTMC2660Drivers = 5; // assume that additional drivers are dumb enable/step/dir ones + numSmartDrivers = 5; // assume that any additional drivers are dumb enable/step/dir ones break; } DuetExpansion::AdditionalOutputInit(); // Initialise TMC2660 driver module driversPowered = false; - TMC2660::Init(ENABLE_PINS, numTMC2660Drivers); + SmartDrivers::Init(ENABLE_PINS, numSmartDrivers); + pauseOnStallDrivers = rehomeOnStallDrivers = 0; // Set up the VSSA sense pin. Older Duet WiFis don't have it connected, so we enable the pulldown resistor to keep it inactive. { @@ -559,7 +567,13 @@ void Platform::Init() { if (heatOnPins[heater] != NoPin) { - pinMode(heatOnPins[heater], (HEAT_ON) ? OUTPUT_LOW : OUTPUT_HIGH); + pinMode(heatOnPins[heater], +#if ACTIVE_LOW_HEAT_ON + OUTPUT_LOW +#else + OUTPUT_HIGH +#endif + ); } AnalogChannelNumber chan = PinToAdcChannel(tempSensePins[heater]); // translate the Arduino Due Analog pin number to the SAM ADC channel number pinMode(tempSensePins[heater], AIN); @@ -611,10 +625,10 @@ void Platform::Init() AnalogInEnableChannel(temperatureAdcChannel, true); highestMcuTemperature = 0; // the highest output we have seen from the ADC filter lowestMcuTemperature = 4095 * ThermistorAverageReadings; // the lowest output we have seen from the ADC filter -#endif mcuTemperatureAdjust = 0.0; +#endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Power monitoring vInMonitorAdcChannel = PinToAdcChannel(PowerMonitorVinDetectPin); pinMode(PowerMonitorVinDetectPin, AIN); @@ -1329,7 +1343,7 @@ void Platform::Spin() for (;;) {} } -#ifdef DUET_NG +#if HAS_SMART_DRIVERS // Check whether the TMC drivers need to be initialised. // The tick ISR also looks for over-voltage events, but it just disables the driver without changing driversPowerd or numOverVoltageEvents if (driversPowered) @@ -1347,44 +1361,46 @@ void Platform::Spin() else { // Poll one TMC2660 for temperature warning or temperature shutdown - const uint32_t stat = TMC2660::GetStatus(nextDriveToPoll); - const uint16_t mask = 1 << nextDriveToPoll; - if (stat & TMC_RR_OT) - { - temperatureShutdownDrivers |= mask; - temperatureWarningDrivers &= ~mask; // no point in setting warning as well as error - } - else + if (enableValues[nextDriveToPoll] >= 0) // don't poll driver if it is flagged "no poll" { - temperatureShutdownDrivers &= ~mask; - if (stat & TMC_RR_OTPW) + const uint32_t stat = SmartDrivers::GetStatus(nextDriveToPoll); + const DriversBitmap mask = MakeBitmap<DriversBitmap>(nextDriveToPoll); + if (stat & TMC_RR_OT) { - temperatureWarningDrivers |= mask; + temperatureShutdownDrivers |= mask; + temperatureWarningDrivers &= ~mask; // no point in setting warning as well as error } else { - temperatureWarningDrivers &= ~mask; + temperatureShutdownDrivers &= ~mask; + if (stat & TMC_RR_OTPW) + { + temperatureWarningDrivers |= mask; + } + else + { + temperatureWarningDrivers &= ~mask; + } + } + if (stat & TMC_RR_S2G) + { + shortToGroundDrivers |= mask; + } + else + { + shortToGroundDrivers &= ~mask; + } + if ((stat & (TMC_RR_OLA | TMC_RR_OLB)) != 0 && (stat & TMC_RR_STST) == 0) + { + openLoadDrivers |= mask; + } + else + { + openLoadDrivers &= ~mask; } } - if (stat & TMC_RR_S2G) - { - shortToGroundDrivers |= mask; - } - else - { - shortToGroundDrivers &= ~mask; - } - if ((stat & (TMC_RR_OLA | TMC_RR_OLB)) != 0 && (stat & TMC_RR_STST) == 0) - { - openLoadDrivers |= mask; - } - else - { - openLoadDrivers &= ~mask; - } - ++nextDriveToPoll; - if (nextDriveToPoll > numTMC2660Drivers) + if (nextDriveToPoll > numSmartDrivers) { nextDriveToPoll = 0; } @@ -1394,7 +1410,7 @@ void Platform::Spin() { driversPowered = true; } - TMC2660::SetDriversPowered(driversPowered); + SmartDrivers::SetDriversPowered(driversPowered); #endif const uint32_t now = millis(); @@ -1418,7 +1434,7 @@ void Platform::Spin() fans[fan].Check(); } -#ifdef DUET_NG +#if HAS_SMART_DRIVERS // Check whether it is time to report any faults (do this after checking fans in case driver cooling fans are turned on) if (now - lastWarningMillis > MinimumWarningInterval) { @@ -1457,7 +1473,7 @@ void Platform::Spin() #endif } -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Check for auto-pause, shutdown or resume if (autoSaveEnabled) { @@ -1473,7 +1489,7 @@ void Platform::Spin() case AutoSaveState::normal: if (currentVin < autoPauseReading) { - if (reprap.GetGCodes().AutoPause()) + if (reprap.GetGCodes().LowVoltagePause()) { autoSaveState = AutoSaveState::autoPaused; } @@ -1481,26 +1497,9 @@ void Platform::Spin() break; case AutoSaveState::autoPaused: - if (currentVin < autoShutdownReading) - { - if (reprap.GetGCodes().AutoShutdown()) - { - autoSaveState = AutoSaveState::autoShutdown; - } - } - else if (currentVin >= autoResumeReading) - { - if (reprap.GetGCodes().AutoResume()) - { - autoSaveState = AutoSaveState::normal; - } - } - break; - - case AutoSaveState::autoShutdown: if (currentVin >= autoResumeReading) { - if (reprap.GetGCodes().AutoResumeAfterShutdown()) + if (reprap.GetGCodes().LowVoltageResume()) { autoSaveState = AutoSaveState::normal; } @@ -1516,21 +1515,17 @@ void Platform::Spin() // Flush the log file it it is time. This may take some time, so do it last. if (logger != nullptr) { - if (now - lastLogFlushTime >= LogFlushInterval) - { - logger->Flush(); - lastLogFlushTime = now; - } + logger->Flush(); } ClassReport(longWait); } -#ifdef DUET_NG +#if HAS_SMART_DRIVERS // Report driver status conditions that require attention. // Sets 'reported' if we reported anything, else leaves 'reported' alone. -void Platform::ReportDrivers(uint16_t whichDrivers, const char* text, bool& reported) +void Platform::ReportDrivers(DriversBitmap whichDrivers, const char* text, bool& reported) { if (whichDrivers != 0) { @@ -1548,36 +1543,35 @@ void Platform::ReportDrivers(uint16_t whichDrivers, const char* text, bool& repo } } -// Configure auto save on power fail -bool Platform::ConfigureAutoSave(GCodeBuffer& gb, StringRef& reply) +#endif + +#if HAS_VOLTAGE_MONITOR + +void Platform::DisableAutoSave() { - bool seen = false; - float autoSaveVoltages[3]; - if (gb.TryGetFloatArray('S', 3, autoSaveVoltages, reply, seen)) - { - return true; - } + autoSaveEnabled = false; +} - if (seen) - { - autoSaveEnabled = (autoSaveVoltages[0] >= 5.0 && autoSaveVoltages[1] > autoSaveVoltages[0] && autoSaveVoltages[2] > autoSaveVoltages[1]); - if (autoSaveEnabled) - { - autoShutdownReading = PowerVoltageToAdcReading(autoSaveVoltages[0]); - autoPauseReading = PowerVoltageToAdcReading(autoSaveVoltages[1]); - autoResumeReading = PowerVoltageToAdcReading(autoSaveVoltages[2]); - } - } - else if (!autoSaveEnabled) - { - reply.copy("Auto save is disabled"); - } - else +bool Platform::IsPowerOk() const +{ + return currentVin > autoPauseReading; +} + +void Platform::EnableAutoSave(float saveVoltage, float resumeVoltage) +{ + autoPauseReading = PowerVoltageToAdcReading(saveVoltage); + autoResumeReading = PowerVoltageToAdcReading(resumeVoltage); + autoSaveEnabled = true; +} + +bool Platform::GetAutoSaveSettings(float& saveVoltage, float&resumeVoltage) +{ + if (autoSaveEnabled) { - reply.printf(" Auto shutdown at %.1fV, save/pause at %.1fV, resume at %.1fV", - (double)AdcReadingToPowerVoltage(autoShutdownReading), (double)AdcReadingToPowerVoltage(autoPauseReading), (double)AdcReadingToPowerVoltage(autoResumeReading)); + saveVoltage = AdcReadingToPowerVoltage(autoPauseReading); + resumeVoltage = AdcReadingToPowerVoltage(autoResumeReading); } - return false; + return autoSaveEnabled; } #endif @@ -1593,16 +1587,22 @@ bool Platform::WriteFanSettings(FileStore *f) const return ok; } +#if HAS_CPU_TEMP_SENSOR + float Platform::AdcReadingToCpuTemperature(uint32_t adcVal) const { - float voltage = (float)adcVal * (3.3/(float)(4096 * ThermistorAverageReadings)); -#if SAM4E + const float voltage = (float)adcVal * (3.3/(float)(4096 * ThermistorAverageReadings)); +#if SAM4E || SAM4S return (voltage - 1.44) * (1000.0/4.7) + 27.0 + mcuTemperatureAdjust; // accuracy at 27C is +/-13C #elif SAM3XA return (voltage - 0.8) * (1000.0/2.65) + 27.0 + mcuTemperatureAdjust; // accuracy at 27C is +/-45C +#else +# error undefined CPU temp conversion #endif } +#endif + // Perform a software reset. 'stk' points to the program counter on the stack if the cause is an exception, otherwise it is nullptr. void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk) { @@ -1635,17 +1635,20 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk) reason |= (uint16_t)SoftwareResetReason::inAuxOutput; // if we are resetting because we are stuck in a Spin function, record whether we are trying to send to aux } } - reason |= (uint16_t)reprap.GetSpinningModule(); + reason |= (uint8_t)reprap.GetSpinningModule(); + reason |= (softwareResetDebugInfo & 0x07) << 5; // Record the reason for the software reset // First find a free slot (wear levelling) size_t slot = SoftwareResetData::numberOfSlots; SoftwareResetData srdBuf[SoftwareResetData::numberOfSlots]; -#if SAM4E +#if SAM4E || SAM4S if (flash_read_user_signature(reinterpret_cast<uint32_t*>(srdBuf), sizeof(srdBuf)/sizeof(uint32_t)) == FLASH_RC_OK) -#else +#elif SAM3XA DueFlashStorage::read(SoftwareResetData::nvAddress, srdBuf, sizeof(srdBuf)); +#else +# error #endif { while (slot != 0 && srdBuf[slot - 1].isVacant()) @@ -1657,7 +1660,7 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk) if (slot == SoftwareResetData::numberOfSlots) { // No free slots, so erase the area -#if SAM4E +#if SAM4E || SAM4S flash_erase_user_signature(); #endif memset(srdBuf, 0xFF, sizeof(srdBuf)); @@ -1680,7 +1683,7 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk) } // Save diagnostics data to Flash -#if SAM4E +#if SAM4E || SAM4S flash_write_user_signature(srdBuf, sizeof(srdBuf)/sizeof(uint32_t)); #else DueFlashStorage::write(SoftwareResetData::nvAddress, srdBuf, sizeof(srdBuf)); @@ -1696,11 +1699,13 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk) // Interrupts #if HAS_LWIP_NETWORKING + void NETWORK_TC_HANDLER() { tc_get_status(NETWORK_TC, NETWORK_TC_CHAN); reprap.GetNetwork().Interrupt(); } + #endif static void FanInterrupt(void*) @@ -1720,7 +1725,7 @@ void Platform::InitialiseInterrupts() // Set the tick interrupt to the highest priority. We need to to monitor the heaters and kick the watchdog. NVIC_SetPriority(SysTick_IRQn, NvicPrioritySystick); // set priority for tick interrupts -#if SAM4E +#if SAM4E || SAM4S NVIC_SetPriority(UART0_IRQn, NvicPriorityUart); // set priority for UART interrupt - must be higher than step interrupt #else NVIC_SetPriority(UART_IRQn, NvicPriorityUart); // set priority for UART interrupt - must be higher than step interrupt @@ -1763,11 +1768,12 @@ void Platform::InitialiseInterrupts() NVIC_SetPriority(PIOE_IRQn, NvicPriorityPins); #endif -#if SAM3XA - NVIC_SetPriority(UOTGHS_IRQn, NvicPriorityUSB); -#endif -#if SAM4E +#if SAM4E || SAM4S NVIC_SetPriority(UDP_IRQn, NvicPriorityUSB); +#elif SAM3XA + NVIC_SetPriority(UOTGHS_IRQn, NvicPriorityUSB); +#else +# error #endif NVIC_SetPriority(TWI1_IRQn, NvicPriorityTwi); @@ -1804,7 +1810,7 @@ void Platform::Diagnostics(MessageType mtype) MessageF(mtype, "%s version %s running on %s", FIRMWARE_NAME, VERSION, GetElectronicsString()); #ifdef DUET_NG - const char* expansionName = DuetExpansion::GetExpansionBoardName(); + const char* const expansionName = DuetExpansion::GetExpansionBoardName(); if (expansionName != nullptr) { MessageF(mtype, " + %s", expansionName); @@ -1813,7 +1819,7 @@ void Platform::Diagnostics(MessageType mtype) Message(mtype, "\n"); -#ifdef DUET_NG +#if SAM4E || SAM4S // Print the unique ID { uint32_t idBuf[5]; @@ -1883,7 +1889,7 @@ void Platform::Diagnostics(MessageType mtype) // Print memory stats and error codes to USB and copy them to the current webserver reply const char *ramstart = -#if SAM4E +#if SAM4E || SAM4S (char *) 0x20000000; #elif SAM3XA (char *) 0x20070000; @@ -1902,7 +1908,7 @@ void Platform::Diagnostics(MessageType mtype) // Show the up time and reason for the last reset const uint32_t now = (uint32_t)(millis64()/1000u); // get up time in seconds const char* resetReasons[8] = { "power up", "backup", "watchdog", "software", -#if SAM4E +#if SAM4E || SAM4S // On the SAM4E a watchdog reset may be reported as a user reset because of the capacitor on the NRST pin "reset button or watchdog", #else @@ -1919,7 +1925,7 @@ void Platform::Diagnostics(MessageType mtype) memset(srdBuf, 0, sizeof(srdBuf)); int slot = -1; -#if SAM4E +#if SAM4E || SAM4S // Work around bug in ASF flash library: flash_read_user_signature calls a RAMFUNC without disabling interrupts first. // This caused a crash (watchdog timeout) sometimes if we run M122 while a print is in progress const irqflags_t flags = cpu_irq_save(); @@ -2002,7 +2008,7 @@ void Platform::Diagnostics(MessageType mtype) lowestMcuTemperature = highestMcuTemperature = currentMcuTemperature; #endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Show the supply voltage MessageF(mtype, "Supply voltage: min %.1f, current %.1f, max %.1f, under voltage events: %" PRIu32 ", over voltage events: %" PRIu32 "\n", (double)AdcReadingToPowerVoltage(lowestVin), (double)AdcReadingToPowerVoltage(currentVin), (double)AdcReadingToPowerVoltage(highestVin), @@ -2016,9 +2022,9 @@ void Platform::Diagnostics(MessageType mtype) MessageF(mtype, "Expansion motor(s) stall indication: %s\n", (stalled) ? "yes" : "no"); } - for (size_t drive = 0; drive < numTMC2660Drivers; ++drive) + for (size_t drive = 0; drive < numSmartDrivers; ++drive) { - const uint32_t stat = TMC2660::GetStatus(drive); + const uint32_t stat = SmartDrivers::GetStatus(drive); MessageF(mtype, "Driver %d:%s%s%s%s%s%s\n", drive, (stat & TMC_RR_SG) ? " stalled" : "", (stat & TMC_RR_OT) ? " temperature-shutdown!" @@ -2097,10 +2103,12 @@ void Platform::DiagnosticTest(int d) case (int)DiagnosticTestType::BusFault: // Read from the "Undefined (Abort)" area -#ifdef DUET_NG +#if SAM4E || SAM4S (void)RepRap::ReadDword(reinterpret_cast<const char*>(0x20800000)); -#else +#elif SAM3XA (void)RepRap::ReadDword(reinterpret_cast<const char*>(0x20200000)); +#else +# error #endif break; @@ -2168,7 +2176,8 @@ void Platform::ClassReport(uint32_t &lastTime) } } -#ifdef DUET_NG +#if HAS_SMART_DRIVERS + // This is called when a fan that monitors driver temperatures is turned on when it was off void Platform::DriverCoolingFansOn(uint32_t driverChannelsMonitored) { @@ -2183,6 +2192,7 @@ void Platform::DriverCoolingFansOn(uint32_t driverChannelsMonitored) offBoardDriversFanRunning = true; } } + #endif // Power is a fraction in [0,1] @@ -2191,7 +2201,13 @@ void Platform::SetHeater(size_t heater, float power) if (heatOnPins[heater] != NoPin) { const uint16_t freq = (reprap.GetHeat().UseSlowPwm(heater)) ? SlowHeaterPwmFreq : NormalHeaterPwmFreq; - IoPort::WriteAnalog(heatOnPins[heater], (HEAT_ON) ? power : 1.0 - power, freq); + const float pwm = +#if ACTIVE_LOW_HEAT_ON + 1.0 - power; +#else + power; +#endif + IoPort::WriteAnalog(heatOnPins[heater], pwm, freq); } } @@ -2277,10 +2293,20 @@ EndStopHit Platform::GetZProbeResult() const : EndStopHit::noStop; } -// Write the Z probe parameters to file -bool Platform::WriteZProbeParameters(FileStore *f) const +// Write the platform parameters to file +bool Platform::WritePlatformParameters(FileStore *f) const { - bool ok = f->Write("; Z probe parameters\n"); + bool ok = WriteAxisLimits(f, axisMinimaProbed, axisMinima, 1); + if (ok) + { + ok = WriteAxisLimits(f, axisMaximaProbed, axisMaxima, 0); + } + +#if 0 // From version 1.20 we no longer write the Z probe parameters, but keep the code for now in case too many users complain + if (ok) + { + ok = f->Write("; Z probe parameters\n"); + } if (ok) { ok = irZProbeParameters.WriteParameters(f, 1); @@ -2293,9 +2319,27 @@ bool Platform::WriteZProbeParameters(FileStore *f) const { ok = switchZProbeParameters.WriteParameters(f, 4); } +#endif + return ok; } +bool Platform::WriteAxisLimits(FileStore *f, AxesBitmap axesProbed, const float limits[MaxAxes], int sParam) +{ + if (axesProbed == 0) + { + return true; + } + + scratchString.printf("M208 S%d", sParam); + for (size_t axis = 0; axis < MaxAxes && IsBitSet(axesProbed, axis); ++axis) + { + scratchString.catf(" %c%.2f", GCodes::axisLetters[axis], (double)limits[axis]); + } + scratchString.cat('\n'); + return f->Write(scratchString.Pointer()); +} + // This is called from the step ISR as well as other places, so keep it fast // If drive >= DRIVES then we are setting an individual motor direction void Platform::SetDirection(size_t drive, bool direction) @@ -2326,16 +2370,16 @@ void Platform::EnableDriver(size_t driver) driverState[driver] = DriverStatus::enabled; UpdateMotorCurrent(driver); // the current may have been reduced by the idle timeout -#if defined(DUET_NG) - if (driver < numTMC2660Drivers) +#if HAS_SMART_DRIVERS + if (driver < numSmartDrivers) { - TMC2660::EnableDrive(driver, true); + SmartDrivers::EnableDrive(driver, true); } else { #endif - digitalWrite(ENABLE_PINS[driver], enableValues[driver]); -#if defined(DUET_NG) + digitalWrite(ENABLE_PINS[driver], enableValues[driver] > 0); +#if HAS_SMART_DRIVERS } #endif } @@ -2346,16 +2390,16 @@ void Platform::DisableDriver(size_t driver) { if (driver < DRIVES) { -#if defined(DUET_NG) - if (driver < numTMC2660Drivers) +#if HAS_SMART_DRIVERS + if (driver < numSmartDrivers) { - TMC2660::EnableDrive(driver, false); + SmartDrivers::EnableDrive(driver, false); } else { #endif - digitalWrite(ENABLE_PINS[driver], !enableValues[driver]); -#if defined(DUET_NG) + digitalWrite(ENABLE_PINS[driver], enableValues[driver] <= 0); +#if HAS_SMART_DRIVERS } #endif driverState[driver] = DriverStatus::disabled; @@ -2470,13 +2514,13 @@ void Platform::UpdateMotorCurrent(size_t driver) current *= motorCurrentFraction[driver]; } -#if defined(DUET_NG) - if (driver < numTMC2660Drivers) +#if HAS_SMART_DRIVERS + if (driver < numSmartDrivers) { - TMC2660::SetCurrent(driver, current); + SmartDrivers::SetCurrent(driver, current); } #elif defined (DUET_06_085) - unsigned short pot = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDigipotVoltage/2)/maxStepperDigipotVoltage); + const uint16_t pot = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDigipotVoltage/2)/maxStepperDigipotVoltage); if (driver < 4) { mcpDuet.setNonVolatileWiper(potWipes[driver], pot); @@ -2555,10 +2599,10 @@ bool Platform::SetDriverMicrostepping(size_t driver, int microsteps, int mode) { if (driver < DRIVES) { -#if defined(DUET_NG) - if (driver < numTMC2660Drivers) +#if HAS_SMART_DRIVERS + if (driver < numSmartDrivers) { - return TMC2660::SetMicrostepping(driver, microsteps, mode); + return SmartDrivers::SetMicrostepping(driver, microsteps, mode); } else { @@ -2599,10 +2643,10 @@ bool Platform::SetMicrostepping(size_t drive, int microsteps, int mode) // Get the microstepping for a driver unsigned int Platform::GetDriverMicrostepping(size_t driver, int mode, bool& interpolation) const { -#if defined(DUET_NG) - if (driver < numTMC2660Drivers) +#if HAS_SMART_DRIVERS + if (driver < numSmartDrivers) { - return TMC2660::GetMicrostepping(driver, mode, interpolation); + return SmartDrivers::GetMicrostepping(driver, mode, interpolation); } // On-board drivers only support x16 microstepping without interpolation interpolation = false; @@ -2635,6 +2679,23 @@ unsigned int Platform::GetMicrostepping(size_t drive, int mode, bool& interpolat } } +void Platform::SetEnableValue(size_t driver, int8_t eVal) +{ + enableValues[driver] = eVal; + DisableDriver(driver); // disable the drive, because the enable polarity may have been wrong before +#if HAS_SMART_DRIVERS + if (eVal == -1) + { + // User has asked to disable status monitoring for this driver, so clear its error bits + DriversBitmap mask = ~MakeBitmap<DriversBitmap>(driver); + temperatureShutdownDrivers &= mask; + temperatureWarningDrivers &= mask; + shortToGroundDrivers &= mask; + openLoadDrivers &= mask; + } +#endif +} + void Platform::SetAxisDriversConfig(size_t drive, const AxisDriversConfig& config) { axisDrivers[drive] = config; @@ -2914,9 +2975,16 @@ void Platform::RawMessage(MessageType type, const char *message) if ((type & BlockingUsbMessage) != 0) { - // Debug messages in blocking mode - potentially DANGEROUS, use with care! - SERIAL_MAIN_DEVICE.write(message); - SERIAL_MAIN_DEVICE.flush(); + // Debug output sends messages in blocking mode. We now give up sending if we are close to software watchdog timeout. + const char *p = message; + size_t len = strlen(p); + while (SERIAL_MAIN_DEVICE && len != 0 && !reprap.SpinTimeoutImminent()) + { + const size_t written = SERIAL_MAIN_DEVICE.write(p, len); + len -= written; + p += written; + } + // We no longer flush afterwards } else if ((type & UsbMessage) != 0) { @@ -3153,7 +3221,7 @@ bool Platform::ConfigureLogging(GCodeBuffer& gb, StringRef& reply) return false; } -// This is called form EmergencyStop. It closes the log file and stops logging. +// This is called from EmergencyStop. It closes the log file and stops logging. void Platform::StopLogging() { if (logger != nullptr) @@ -3352,7 +3420,11 @@ bool Platform::GetFirmwarePin(LogicalPin logicalPin, PinAccess access, Pin& firm if (!reprap.GetHeat().IsHeaterEnabled(logicalPin - Heater0LogicalPin)) { firmwarePin = heatOnPins[logicalPin - Heater0LogicalPin]; - invert = !HEAT_ON; +#if ACTIVE_LOW_HEAT_ON + invert = true; +#else + invert = false; +#endif } } else if (logicalPin >= Fan0LogicalPin && logicalPin < Fan0LogicalPin + (int)NUM_FANS) // pins 20- correspond to fan channels @@ -3497,6 +3569,25 @@ Pin Platform::GetEndstopPin(int endstop) const return (endstop >= 0 && endstop < (int)ARRAY_SIZE(endStopPins)) ? endStopPins[endstop] : NoPin; } +// Axis limits +void Platform::SetAxisMaximum(size_t axis, float value, bool byProbing) +{ + axisMaxima[axis] = value; + if (byProbing) + { + SetBit(axisMaximaProbed, axis); + } +} + +void Platform::SetAxisMinimum(size_t axis, float value, bool byProbing) +{ + axisMinima[axis] = value; + if (byProbing) + { + SetBit(axisMinimaProbed, axis); + } +} + #if SUPPORT_INKJET // Fire the inkjet (if any) in the given pattern @@ -3552,7 +3643,7 @@ void Platform::GetMcuTemperatures(float& minT, float& currT, float& maxT) const } #endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Power in voltage void Platform::GetPowerVoltages(float& minV, float& currV, float& maxV) const @@ -3562,6 +3653,10 @@ void Platform::GetPowerVoltages(float& minV, float& currV, float& maxV) const maxV = AdcReadingToPowerVoltage(highestVin); } +#endif + +#if HAS_SMART_DRIVERS + // TMC driver temperatures float Platform::GetTmcDriversTemperature(unsigned int board) const { @@ -3571,6 +3666,127 @@ float Platform::GetTmcDriversTemperature(unsigned int board) const : 0.0; } +// Configure the motor stall detection, returning true if an error was encountered +bool Platform::ConfigureStallDetection(GCodeBuffer& gb, StringRef& reply) +{ + // Build a bitmap of all the drivers referenced + // First looks for explicit driver numbers + DriversBitmap drivers = 0; + if (gb.Seen('P')) + { + long int drives[DRIVES]; + size_t dCount = DRIVES; + gb.GetLongArray(drives, dCount); + for (size_t i = 0; i < dCount; i++) + { + if (drives[i] < 0 || (size_t)drives[i] >= DRIVES) + { + reply.printf("Invalid drive number '%ld'", drives[i]); + return true; + } + SetBit(drivers, i); + } + } + + // Now look for axes + for (size_t i = 0; i < reprap.GetGCodes().GetTotalAxes(); ++i) + { + if (gb.Seen(GCodes::axisLetters[i])) + { + for (size_t j = 0; j < axisDrivers[i].numDrivers; ++j) + { + SetBit(drivers, axisDrivers[i].driverNumbers[j]); + } + } + } + + // Now check for values to change + bool seen = false; + if (gb.Seen('S')) + { + seen = true; + const int sgThreshold = gb.GetIValue(); + for (size_t drive = 0; drive < numSmartDrivers; ++drive) + { + if (IsBitSet(drivers, drive)) + { + SmartDrivers::SetStallThreshold(drive, sgThreshold); + } + } + } + if (gb.Seen('F')) + { + seen = true; + const int sgFilter = (gb.GetIValue() == 1); + for (size_t drive = 0; drive < numSmartDrivers; ++drive) + { + if (IsBitSet(drivers, drive)) + { + SmartDrivers::SetStallFilter(drive, sgFilter); + } + } + } + if (gb.Seen('T')) + { + seen = true; + const uint16_t coolStepConfig = (uint16_t)gb.GetUIValue(); + for (size_t drive = 0; drive < numSmartDrivers; ++drive) + { + if (IsBitSet(drivers, drive)) + { + SmartDrivers::SetCoolStep(drive, coolStepConfig); + } + } + } + if (gb.Seen('R')) + { + seen = true; + const int action = gb.GetIValue(); + switch (action) + { + case 0: + default: + pauseOnStallDrivers &= ~drivers; + rehomeOnStallDrivers &= ~drivers; + break; + + case 1: + rehomeOnStallDrivers &= ~drivers; + pauseOnStallDrivers |= drivers; + break; + + case 2: + pauseOnStallDrivers &= ~drivers; + rehomeOnStallDrivers |= drivers; + break; + } + } + + if (!seen) + { + if (drivers == 0) + { + drivers = LowestNBits<DriversBitmap>(numSmartDrivers); + } + bool printed = false; + for (size_t drive = 0; drive < numSmartDrivers; ++drive) + { + if (IsBitSet(drivers, drive)) + { + if (printed) + { + reply.cat('\n'); + } + reply.catf("Driver %u: ", drive); + SmartDrivers::AppendStallConfig(drive, reply); + printed = true; + } + } + + } + return false; +} + #endif // Real-time clock @@ -3591,8 +3807,11 @@ bool Platform::SetDateTime(time_t time) const bool ok = (gmtime_r(&time, &brokenDateTime) != nullptr); if (ok) { - Message(LogMessage, "Time and date set\n"); - realTime = time; + realTime = time; // set the date and time + + // Write a log message, giving the time since power up in same format as the logger does + const uint32_t timeSincePowerUp = (uint32_t)(millis64()/1000u); + MessageF(LogMessage, "Date and time set at power up + %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 "\n", timeSincePowerUp/3600u, (timeSincePowerUp % 3600u)/60u, timeSincePowerUp % 60u); timeLastUpdatedMillis = millis(); } return ok; @@ -3628,7 +3847,7 @@ void STEP_TC_HANDLER() { const irqflags_t flags = cpu_irq_save(); const int32_t diff = (int32_t)(tim - GetInterruptClocks()); // see how long we have to go - if (diff < (int32_t)DDA::minInterruptInterval) // if less than about 2us or already passed + if (diff < (int32_t)DDA::minInterruptInterval) // if less than about 6us or already passed { cpu_irq_restore(flags); return true; // tell the caller to simulate an interrupt instead @@ -3661,7 +3880,7 @@ void STEP_TC_HANDLER() { const irqflags_t flags = cpu_irq_save(); const int32_t diff = (int32_t)(tim - GetInterruptClocks()); // see how long we have to go - if (diff < (int32_t)DDA::minInterruptInterval) // if less than about 2us or already passed + if (diff < (int32_t)DDA::minInterruptInterval) // if less than about 6us or already passed { cpu_irq_restore(flags); return true; // tell the caller to simulate an interrupt instead @@ -3700,7 +3919,7 @@ void Platform::Tick() { if (tickState != 0) { -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Read the power input voltage currentVin = AnalogInReadChannel(vInMonitorAdcChannel); if (currentVin > highestVin) @@ -3713,7 +3932,7 @@ void Platform::Tick() } if (driversPowered && currentVin > driverOverVoltageAdcReading) { - TMC2660::SetDriversPowered(false); + SmartDrivers::SetDriversPowered(false); // We deliberately do not clear driversPowered here or increase the over voltage event count - we let the spin loop handle that } #endif diff --git a/src/Platform.h b/src/Platform.h index a5e4425b..773ec33f 100644 --- a/src/Platform.h +++ b/src/Platform.h @@ -49,7 +49,6 @@ Licence: GPL #include "Storage/MassStorage.h" // must be after Pins.h because it needs NumSdCards defined #include "MessageType.h" #include "ZProbeProgrammer.h" -#include "Logger.h" #if defined(DUET_NG) # include "DueXn.h" @@ -107,7 +106,7 @@ const uint32_t Z_PROBE_AXES = (1 << Z_AXIS); // Axes for which the Z-probe i // HEATERS - The bed is assumed to be the at index 0 -// Define the number of temperature readings we average for each thermistor. This should be a power of 2 and at least 4 ** AD_OVERSAMPLE_BITS. +// Define the number of temperature readings we average for each thermistor. This should be a power of 2 and at least 4 ^ AD_OVERSAMPLE_BITS. // Keep THERMISTOR_AVERAGE_READINGS * NUM_HEATERS * 2ms no greater than HEAT_SAMPLE_TIME or the PIDs won't work well. const unsigned int ThermistorAverageReadings = 32; @@ -394,7 +393,7 @@ public: void SetDirection(size_t drive, bool direction); void SetDirectionValue(size_t driver, bool dVal); bool GetDirectionValue(size_t driver) const; - void SetEnableValue(size_t driver, bool eVal); + void SetEnableValue(size_t driver, int8_t eVal); bool GetEnableValue(size_t driver) const; void EnableDriver(size_t driver); void DisableDriver(size_t driver); @@ -436,9 +435,9 @@ public: void SetInstantDv(size_t drive, float value); EndStopHit Stopped(size_t drive) const; float AxisMaximum(size_t axis) const; - void SetAxisMaximum(size_t axis, float value); + void SetAxisMaximum(size_t axis, float value, bool byProbing); float AxisMinimum(size_t axis) const; - void SetAxisMinimum(size_t axis, float value); + void SetAxisMinimum(size_t axis, float value, bool byProbing); float AxisTotalLength(size_t axis) const; float GetPressureAdvance(size_t drive) const; void SetPressureAdvance(size_t extruder, float factor); @@ -481,7 +480,7 @@ public: const ZProbeParameters& GetCurrentZProbeParameters() const { return GetZProbeParameters(zProbeType); } void SetZProbeParameters(int32_t probeType, const struct ZProbeParameters& params); bool HomingZWithProbe() const; - bool WriteZProbeParameters(FileStore *f) const; + bool WritePlatformParameters(FileStore *f) const; void SetProbing(bool isProbing); bool ProgramZProbe(GCodeBuffer& gb, StringRef& reply); void SetZProbeModState(bool b) const; @@ -537,16 +536,23 @@ public: // MCU temperature #if HAS_CPU_TEMP_SENSOR void GetMcuTemperatures(float& minT, float& currT, float& maxT) const; -#endif void SetMcuTemperatureAdjust(float v) { mcuTemperatureAdjust = v; } float GetMcuTemperatureAdjust() const { return mcuTemperatureAdjust; } +#endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Power in voltage void GetPowerVoltages(float& minV, float& currV, float& maxV) const; + bool IsPowerOk() const; + void DisableAutoSave(); + void EnableAutoSave(float saveVoltage, float resumeVoltage); + bool GetAutoSaveSettings(float& saveVoltage, float&resumeVoltage); +#endif + +#if HAS_SMART_DRIVERS float GetTmcDriversTemperature(unsigned int board) const; void DriverCoolingFansOn(uint32_t driverChannelsMonitored); - bool ConfigureAutoSave(GCodeBuffer& gb, StringRef& reply); + bool ConfigureStallDetection(GCodeBuffer& gb, StringRef& reply); #endif // User I/O and servo support @@ -582,6 +588,8 @@ public: void SetLaserPwmFrequency(float freq); float GetLaserPwmFrequency() const { return laserPort.GetFrequency(); } + static uint8_t softwareResetDebugInfo; // extra info for debugging + //------------------------------------------------------------------------------------------------------- private: @@ -592,8 +600,8 @@ private: void ResetChannel(size_t chan); // re-initialise a serial channel float AdcReadingToCpuTemperature(uint32_t reading) const; -#ifdef DUET_NG - void ReportDrivers(uint16_t whichDrivers, const char* text, bool& reported); +#if HAS_SMART_DRIVERS + void ReportDrivers(DriversBitmap whichDrivers, const char* text, bool& reported); #endif // These are the structures used to hold our non-volatile data. @@ -640,7 +648,7 @@ private: } }; -#if SAM4E +#if SAM4E || SAM4S static_assert(SoftwareResetData::numberOfSlots * sizeof(SoftwareResetData) <= 512, "Can't fit software reset data in SAM4E user signature area"); #else static_assert(SoftwareResetData::numberOfSlots * sizeof(SoftwareResetData) <= FLASH_DATA_LENGTH, "NVData too large"); @@ -648,7 +656,6 @@ private: // Logging Logger *logger; - uint32_t lastLogFlushTime; // Z probes ZProbeParameters switchZProbeParameters; // Z probe values for the switch Z-probe @@ -687,7 +694,7 @@ private: volatile DriverStatus driverState[DRIVES]; bool directions[DRIVES]; - bool enableValues[DRIVES]; + int8_t enableValues[DRIVES]; Pin endStopPins[DRIVES]; float maxFeedrates[DRIVES]; float accelerations[DRIVES]; @@ -705,9 +712,12 @@ private: uint32_t slowDrivers; // bitmap of driver port bits that need extended step pulse timing float idleCurrentFactor; -#if defined(DUET_NG) - size_t numTMC2660Drivers; // the number of TMC2660 drivers we have, the remaining are simple enable/step/dir drivers -#elif defined(DUET_06_085) +#if HAS_SMART_DRIVERS + size_t numSmartDrivers; // the number of TMC2660 drivers we have, the remaining are simple enable/step/dir drivers + DriversBitmap pauseOnStallDrivers, rehomeOnStallDrivers; +#endif + +#if defined(DUET_06_085) // Digipots MCP4461 mcpDuet; MCP4461 mcpExpansion; @@ -743,9 +753,12 @@ private: float axisMaxima[MaxAxes]; float axisMinima[MaxAxes]; + AxesBitmap axisMinimaProbed, axisMaximaProbed; EndStopType endStopType[MaxAxes]; bool endStopLogicLevel[MaxAxes]; - + + static bool WriteAxisLimits(FileStore *f, AxesBitmap axesProbed, const float limits[MaxAxes], int sParam); + // Heaters - bed is assumed to be the first Pin tempSensePins[Heaters]; @@ -828,36 +841,38 @@ private: #if HAS_CPU_TEMP_SENSOR // reading temperature on the RADDS messes up one of the heater pins, so don't do it AnalogChannelNumber temperatureAdcChannel; uint32_t highestMcuTemperature, lowestMcuTemperature; -#endif float mcuTemperatureAdjust; +#endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR AnalogChannelNumber vInMonitorAdcChannel; volatile uint16_t currentVin, highestVin, lowestVin; uint32_t numUnderVoltageEvents; volatile uint32_t numOverVoltageEvents; - uint32_t lastWarningMillis; // When we last sent a warning message about a Vssa short - uint16_t temperatureShutdownDrivers, temperatureWarningDrivers, shortToGroundDrivers, openLoadDrivers; - uint8_t nextDriveToPoll; - bool driversPowered; - bool vssaSenseWorking; - bool onBoardDriversFanRunning; // true if a fan is running to cool the on-board drivers - bool offBoardDriversFanRunning; // true if a fan is running to cool the drivers on the DueX - uint32_t onBoardDriversFanStartMillis; // how many times we have suppressed a temperature warning - uint32_t offBoardDriversFanStartMillis; // how many times we have suppressed a temperature warning - uint16_t autoShutdownReading, autoPauseReading, autoResumeReading; bool autoSaveEnabled; enum class AutoSaveState : uint8_t { starting = 0, normal, - autoPaused, - autoShutdown + autoPaused }; AutoSaveState autoSaveState; #endif +#if HAS_SMART_DRIVERS + uint32_t lastWarningMillis; // When we last sent a warning message about a Vssa short + DriversBitmap temperatureShutdownDrivers, temperatureWarningDrivers, shortToGroundDrivers, openLoadDrivers; + uint8_t nextDriveToPoll; + bool driversPowered; + bool vssaSenseWorking; + bool onBoardDriversFanRunning; // true if a fan is running to cool the on-board drivers + bool offBoardDriversFanRunning; // true if a fan is running to cool the drivers on the DueX + uint32_t onBoardDriversFanStartMillis; // how many times we have suppressed a temperature warning + uint32_t offBoardDriversFanStartMillis; // how many times we have suppressed a temperature warning + uint16_t autoPauseReading, autoResumeReading; +#endif + // RTC time_t realTime; // the current date/time, or zero if never set uint32_t timeLastUpdatedMillis; // the milliseconds counter when we last incremented the time @@ -977,12 +992,6 @@ inline void Platform::SetDriverDirection(uint8_t driver, bool direction) digitalWrite(DIRECTION_PINS[driver], d); } -inline void Platform::SetEnableValue(size_t driver, bool eVal) -{ - enableValues[driver] = eVal; - DisableDriver(driver); // disable the drive, because the enable polarity may have been wrong before -} - inline bool Platform::GetEnableValue(size_t driver) const { return enableValues[driver]; @@ -993,21 +1002,11 @@ inline float Platform::AxisMaximum(size_t axis) const return axisMaxima[axis]; } -inline void Platform::SetAxisMaximum(size_t axis, float value) -{ - axisMaxima[axis] = value; -} - inline float Platform::AxisMinimum(size_t axis) const { return axisMinima[axis]; } -inline void Platform::SetAxisMinimum(size_t axis, float value) -{ - axisMinima[axis] = value; -} - inline float Platform::AxisTotalLength(size_t axis) const { return axisMaxima[axis] - axisMinima[axis]; @@ -1177,7 +1176,7 @@ inline OutputBuffer *Platform::GetAuxGCodeReply() return temp; } -// *** These next two functions must use the same bit assignments in the drivers bitmap *** +// *** These next three functions must use the same bit assignments in the drivers bitmap *** // The bitmaps are organised like this: // Duet WiFi: // All step pins are on port D, so the bitmap is just the map of bits in port D. @@ -1188,6 +1187,8 @@ inline OutputBuffer *Platform::GetAuxGCodeReply() // RADDS: // Step pins are PA2,9,12,15 PB16,19 PC3,12 PD6 // PC12 clashes with PA12 so we shift PC3,12 left one bit +// Alligator: +// Pins on ports B,C,D are used but the bit numbers are all different, so we use their actual positions // Calculate the step bit for a driver. This doesn't need to be fast. /*static*/ inline uint32_t Platform::CalcDriverBitmap(size_t driver) diff --git a/src/PrintMonitor.cpp b/src/PrintMonitor.cpp index b88b8917..3093a6df 100644 --- a/src/PrintMonitor.cpp +++ b/src/PrintMonitor.cpp @@ -144,7 +144,10 @@ void PrintMonitor::Spin() LayerComplete(); currentLayer++; - lastLayerZ = liveCoordinates[Z_AXIS]; + // If we know the layer height, compute what the current layer height should be. This is to handle slicers that use a different layer height for support. + lastLayerZ = (printingFileInfo.layerHeight > 0.0) + ? printingFileInfo.firstLayerHeight + (currentLayer - 1) * printingFileInfo.layerHeight + : liveCoordinates[Z_AXIS]; lastLayerChangeTime = GetPrintDuration(); } } diff --git a/src/PrintMonitor.h b/src/PrintMonitor.h index 7cc6783e..72df1e51 100644 --- a/src/PrintMonitor.h +++ b/src/PrintMonitor.h @@ -25,7 +25,7 @@ Licence: GPL const FilePosition GCODE_HEADER_SIZE = 20000uL; // How many bytes to read from the header - I (DC) have a Kisslicer file with a layer height comment 14Kb from the start const FilePosition GCODE_FOOTER_SIZE = 400000uL; // How many bytes to read from the footer -#ifdef DUET_NG +#if SAM4E || SAM4S const size_t GCODE_READ_SIZE = 4096; // How many bytes to read in one go in GetFileInfo() (should be a multiple of 512 for read efficiency) #else const size_t GCODE_READ_SIZE = 1024; // How many bytes to read in one go in GetFileInfo() (should be a multiple of 512 for read efficiency) diff --git a/src/RADDS/Pins_RADDS.h b/src/RADDS/Pins_RADDS.h index 6f4b26cb..eb56525c 100644 --- a/src/RADDS/Pins_RADDS.h +++ b/src/RADDS/Pins_RADDS.h @@ -7,6 +7,9 @@ #define HAS_LWIP_NETWORKING 0 #define HAS_CPU_TEMP_SENSOR 0 // enabling the CPU temperature sensor disables Due pin 13 due to bug in SAM3X #define HAS_HIGH_SPEED_SD 0 +#define HAS_SMART_DRIVERS 0 +#define HAS_VOLTAGE_MONITOR 0 +#define ACTIVE_LOW_HEAT_ON 0 const size_t NumFirmwareUpdateModules = 1; #define IAP_UPDATE_FILE "iapradds.bin" @@ -76,10 +79,6 @@ const Pin END_STOP_PINS[DRIVES] = { 28, 30, 32, 39, NoPin, NoPin, NoPin, NoPin } // HEATERS - The bed is assumed to be the at index 0 -// 0 for inverted heater (e.g. Duet v0.6) -// 1 for not (e.g. Duet v0.4; RADDS) -const bool HEAT_ON = true; - // Analogue pin numbers const Pin TEMP_SENSE_PINS[Heaters] = HEATERS_(4, 0, 1, 2, e, f, g, h); diff --git a/src/RepRap.cpp b/src/RepRap.cpp index 34fda25a..25ebfd99 100644 --- a/src/RepRap.cpp +++ b/src/RepRap.cpp @@ -170,63 +170,63 @@ void RepRap::Spin() if(!active) return; - spinningModule = modulePlatform; ticksInSpinState = 0; + spinningModule = modulePlatform; platform->Spin(); - spinningModule = moduleNetwork; ticksInSpinState = 0; + spinningModule = moduleNetwork; network->Spin(true); - spinningModule = moduleWebserver; ticksInSpinState = 0; + spinningModule = moduleWebserver; - spinningModule = moduleGcodes; ticksInSpinState = 0; + spinningModule = moduleGcodes; gCodes->Spin(); - spinningModule = moduleMove; ticksInSpinState = 0; + spinningModule = moduleMove; move->Spin(); - spinningModule = moduleHeat; ticksInSpinState = 0; + spinningModule = moduleHeat; heat->Spin(); #if SUPPORT_ROLAND - spinningModule = moduleRoland; ticksInSpinState = 0; + spinningModule = moduleRoland; roland->Spin(); #endif #if SUPPORT_SCANNER - spinningModule = moduleScanner; ticksInSpinState = 0; + spinningModule = moduleScanner; scanner->Spin(); #endif #if SUPPORT_IOBITS - spinningModule = modulePortControl; ticksInSpinState = 0; + spinningModule = modulePortControl; portControl->Spin(true); #endif - spinningModule = modulePrintMonitor; ticksInSpinState = 0; + spinningModule = modulePrintMonitor; printMonitor->Spin(); #ifdef DUET_NG - spinningModule = moduleDuetExpansion; ticksInSpinState = 0; + spinningModule = moduleDuetExpansion; DuetExpansion::Spin(true); #endif - spinningModule = moduleFilamentSensors; ticksInSpinState = 0; + spinningModule = moduleFilamentSensors; FilamentSensor::Spin(true); - spinningModule = noModule; ticksInSpinState = 0; + spinningModule = noModule; // Check if we need to display a cold extrusion warning const uint32_t now = millis(); @@ -534,7 +534,7 @@ void RepRap::Tick() { platform->Tick(); ++ticksInSpinState; - if (ticksInSpinState >= 20000) // if we stall for 20 seconds, save diagnostic data and reset + if (ticksInSpinState >= MaxTicksInSpinState) // if we stall for 20 seconds, save diagnostic data and reset { resetting = true; for(size_t i = 0; i < Heaters; i++) @@ -554,6 +554,12 @@ void RepRap::Tick() } } +// Return true if we are close to timeout +bool RepRap::SpinTimeoutImminent() const +{ + return ticksInSpinState >= HighTicksInSpinState; +} + // Get the JSON status response for the web server (or later for the M105 command). // Type 1 is the ordinary JSON status response. // Type 2 is the same except that static parameters are also included. @@ -1046,7 +1052,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source) } #endif -#ifdef DUET_NG +#if HAS_VOLTAGE_MONITOR // Power in voltages { float minV, currV, maxV; @@ -1748,6 +1754,30 @@ bool RepRap::WriteToolSettings(FileStore *f) const return ok; } +// Save some information in config-override.g +bool RepRap::WriteToolParameters(FileStore *f) const +{ + bool ok = true; + for (const Tool *t = toolList; ok && t != nullptr; t = t->Next()) + { + const AxesBitmap axesProbed = t->GetAxisOffsetsProbed(); + if (axesProbed != 0) + { + scratchString.printf("G10 P%d", t->Number()); + for (size_t axis = 0; axis < MaxAxes; ++axis) + { + if (IsBitSet(axesProbed, axis)) + { + scratchString.catf(" %c%.2f", GCodes::axisLetters[axis], (double)(t->GetOffsets()[axis])); + } + } + } + scratchString.cat('\n'); + ok = f->Write(scratchString.Pointer()); + } + return ok; +} + // Helper function for diagnostic tests in Platform.cpp, to cause a deliberate divide-by-zero /*static*/ uint32_t RepRap::DoDivide(uint32_t a, uint32_t b) { @@ -1760,4 +1790,10 @@ bool RepRap::WriteToolSettings(FileStore *f) const return *reinterpret_cast<const uint32_t*>(p); } +// Report an internal error +void RepRap::ReportInternalError(const char *file, const char *func, int line) const +{ + platform->MessageF(ErrorMessage, "Internal Error in %s at %s(%d)\n", func, file, line); +} + // End diff --git a/src/RepRap.h b/src/RepRap.h index 2c3ceeeb..c84d1416 100644 --- a/src/RepRap.h +++ b/src/RepRap.h @@ -87,7 +87,7 @@ public: #endif void Tick(); - uint16_t GetTicksInSpinState() const; + bool SpinTimeoutImminent() const; bool IsStopped() const; uint16_t GetExtrudersInUse() const; @@ -104,7 +104,10 @@ public: void SetAlert(const char *msg, const char *title, int mode, float timeout, AxesBitmap controls); void ClearAlert(); - bool WriteToolSettings(FileStore *f) const; // Save some resume information + bool WriteToolSettings(FileStore *f) const; // save some information for the resume file + bool WriteToolParameters(FileStore *f) const; // save some information in config-override.g + + void ReportInternalError(const char *file, const char *func, int line) const; // Report an internal error static uint32_t DoDivide(uint32_t a, uint32_t b); // helper function for diagnostic tests static uint32_t ReadDword(const char* p); // helper function for diagnostic tests @@ -114,6 +117,9 @@ private: char GetStatusCharacter() const; + static constexpr uint32_t MaxTicksInSpinState = 20000; // timeout before we reset the processor + static constexpr uint32_t HighTicksInSpinState = 16000; // how long before we warn that timeout is approaching + Platform* platform; Network* network; Move* move; @@ -178,7 +184,8 @@ inline Tool* RepRap::GetCurrentTool() const { return currentTool; } inline uint16_t RepRap::GetExtrudersInUse() const { return activeExtruders; } inline uint16_t RepRap::GetToolHeatersInUse() const { return activeToolHeaters; } inline bool RepRap::IsStopped() const { return stopped; } -inline uint16_t RepRap::GetTicksInSpinState() const { return ticksInSpinState; } + +#define INTERNAL_ERROR do { reprap.ReportInternalError((__FILE__), (__func__), (__LINE__)); } while(0) #endif diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h index e7a0fcd9..679e2099 100644 --- a/src/RepRapFirmware.h +++ b/src/RepRapFirmware.h @@ -62,7 +62,10 @@ class Platform; class GCodes; class Move; class DDA; +class Kinematics; class Heat; +class PID; +class TemperatureSensor; class Tool; class Roland; class Scanner; @@ -75,13 +78,14 @@ class GCodeBuffer; class GCodeQueue; class FilamentSensor; class RandomProbePointSet; +class Logger; #if SUPPORT_IOBITS class PortControl; #endif // Define floating point type to use for calculations where we would like high precision in matrix calculations -#ifdef DUET_NG +#if SAM4E || SAM4S typedef double floatc_t; // type of matrix element used for calibration #else // We are more memory-constrained on the SAM3X @@ -89,6 +93,7 @@ typedef float floatc_t; // type of matrix element used for calibration #endif typedef uint32_t AxesBitmap; // Type of a bitmap representing a set of axes +typedef uint32_t DriversBitmap; // Type of a bitmap representing a set of driver numbers typedef uint32_t FansBitmap; // Type of a bitmap representing a set of fan numbers // A single instance of the RepRap class contains all the others diff --git a/src/Storage/FileWriteBuffer.h b/src/Storage/FileWriteBuffer.h index 1e7130c4..9cbc3b8a 100644 --- a/src/Storage/FileWriteBuffer.h +++ b/src/Storage/FileWriteBuffer.h @@ -11,7 +11,7 @@ #include "RepRapFirmware.h" -#ifdef DUET_NG +#if SAM4E || SAM4S const size_t NumFileWriteBuffers = 2; // Number of write buffers const size_t FileWriteBufLen = 8192; // Size of each write buffer #else diff --git a/src/Storage/MassStorage.cpp b/src/Storage/MassStorage.cpp index aab6f287..19f9d9a0 100644 --- a/src/Storage/MassStorage.cpp +++ b/src/Storage/MassStorage.cpp @@ -71,7 +71,7 @@ void MassStorage::Init() // Try to mount the first SD card only char replyBuffer[100]; StringRef reply(replyBuffer, ARRAY_SIZE(replyBuffer)); - do { } while (!Mount(0, reply, false)); + do { } while (Mount(0, reply, false) == GCodeResult::notFinished); if (reply.strlen() != 0) { delay(3000); // Wait a few seconds so users have a chance to see this @@ -342,19 +342,19 @@ bool MassStorage::SetLastModifiedTime(const char* directory, const char *fileNam // Mount the specified SD card, returning true if done, false if needs to be called again. // If an error occurs, return true with the error message in 'reply'. // This may only be called to mount one card at a time. -bool MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess) +GCodeResult MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess) { if (card >= NumSdCards) { reply.copy("SD card number out of range"); - return true; + return GCodeResult::error; } if (isMounted[card] && platform->AnyFileOpen(&fileSystems[card])) { // Don't re-mount the card if any files are open on it reply.copy("SD card has open file(s)"); - return true; + return GCodeResult::error; } static bool mounting = false; @@ -374,58 +374,57 @@ bool MassStorage::Mount(size_t card, StringRef& reply, bool reportSuccess) if (err != SD_MMC_OK && millis() - startTime < 5000) { delay(2); - return false; + return GCodeResult::notFinished; } mounting = false; if (err != SD_MMC_OK) { reply.printf("Cannot initialise SD card %u: %s", card, TranslateCardError(err)); + return GCodeResult::error; } - else + + // Mount the file systems + memset(&fileSystems[card], 0, sizeof(FATFS)); // f_mount doesn't initialise the file structure, we must do it ourselves + FRESULT mounted = f_mount(card, &fileSystems[card]); + if (mounted != FR_OK) + { + reply.printf("Cannot mount SD card %u: code %d", card, mounted); + return GCodeResult::error; + } + + isMounted[card] = true; + if (reportSuccess) { - // Mount the file systems - memset(&fileSystems[card], 0, sizeof(FATFS)); // f_mount doesn't initialise the file structure, we must do it ourselves - FRESULT mounted = f_mount(card, &fileSystems[card]); - if (mounted != FR_OK) + float capacity = sd_mmc_get_capacity(card)/1024; // get capacity and convert to Mbytes + const char* capUnits; + if (capacity >= 1024.0) { - reply.printf("Cannot mount SD card %u: code %d", card, mounted); + capacity /= 1024.0; + capUnits = "Gb"; } else { - isMounted[card] = true; - if (reportSuccess) - { - float capacity = sd_mmc_get_capacity(card)/1024; // get capacity and convert to Mbytes - const char* capUnits; - if (capacity >= 1024.0) - { - capacity /= 1024.0; - capUnits = "Gb"; - } - else - { - capUnits = "Mb"; - } - reply.printf("%s card mounted in slot %u, capacity %.2f%s", TranslateCardType(sd_mmc_get_type(card)), card, (double)capacity, capUnits); - } - else - { - reply.Clear(); - } + capUnits = "Mb"; } + reply.printf("%s card mounted in slot %u, capacity %.2f%s", TranslateCardType(sd_mmc_get_type(card)), card, (double)capacity, capUnits); } - return true; + else + { + reply.Clear(); + } + + return GCodeResult::ok; } // Unmount the specified SD card, returning true if done, false if needs to be called again. // If an error occurs, return true with the error message in 'reply'. -bool MassStorage::Unmount(size_t card, StringRef& reply) +GCodeResult MassStorage::Unmount(size_t card, StringRef& reply) { if (card >= NumSdCards) { reply.copy("SD card number out of range"); - return true; + return GCodeResult::error; } platform->InvalidateFiles(&fileSystems[card]); @@ -433,7 +432,7 @@ bool MassStorage::Unmount(size_t card, StringRef& reply) sd_mmc_unmount(card); isMounted[card] = false; reply.Clear(); - return true; + return GCodeResult::ok; } // Check if the drive referenced in the specified path is mounted. Return true if it is. diff --git a/src/Storage/MassStorage.h b/src/Storage/MassStorage.h index d2f2ed4f..b6e87c05 100644 --- a/src/Storage/MassStorage.h +++ b/src/Storage/MassStorage.h @@ -5,6 +5,7 @@ #include "Pins.h" #include "FileWriteBuffer.h" #include "Libraries/Fatfs/ff.h" +#include "GCodes/GCodeResult.h" #include <ctime> // Info returned by FindFirst/FindNext calls @@ -34,8 +35,8 @@ public: bool DirectoryExists(const char* directory, const char* subDirectory); time_t GetLastModifiedTime(const char* directory, const char *fileName) const; bool SetLastModifiedTime(const char* directory, const char *file, time_t time); - bool Mount(size_t card, StringRef& reply, bool reportSuccess); - bool Unmount(size_t card, StringRef& reply); + GCodeResult Mount(size_t card, StringRef& reply, bool reportSuccess); + GCodeResult Unmount(size_t card, StringRef& reply); bool IsDriveMounted(size_t drive) const { return drive < NumSdCards && isMounted[drive]; } bool CheckDriveMounted(const char* path); diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp index 4453e2c7..8f69ba33 100644 --- a/src/Tools/Tool.cpp +++ b/src/Tools/Tool.cpp @@ -100,6 +100,7 @@ Tool * Tool::freelist = nullptr; t->yMapping = yMap; t->fanMapping = fanMap; t->heaterFault = false; + t->axisOffsetsProbed = 0; t->displayColdExtrudeWarning = false; for (size_t axis = 0; axis < MaxAxes; axis++) @@ -463,10 +464,19 @@ bool Tool::WriteSettings(FileStore *f) const void Tool::SetOffsets(const float offs[MaxAxes]) { - for(size_t i = 0; i < MaxAxes; ++i) + for (size_t i = 0; i < MaxAxes; ++i) { offset[i] = offs[i]; } } +void Tool::SetOffset(size_t axis, float offs, bool byProbing) +{ + offset[axis] = offs; + if (byProbing) + { + SetBit(axisOffsetsProbed, axis); + } +} + // End diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h index 9af30d4a..4b0d2909 100644 --- a/src/Tools/Tool.h +++ b/src/Tools/Tool.h @@ -49,7 +49,8 @@ public: const float *GetOffsets() const; void SetOffsets(const float offs[MaxAxes]); - void SetOffset(size_t axis, float offs) pre(axis < MaxAxes); + void SetOffset(size_t axis, float offs, bool byProbing) pre(axis < MaxAxes); + AxesBitmap GetAxisOffsetsProbed() const { return axisOffsetsProbed; } size_t DriveCount() const; int Drive(size_t driveNumber) const; bool ToolCanDrive(bool extrude); @@ -99,6 +100,7 @@ private: float standbyTemperatures[Heaters]; size_t heaterCount; float offset[MaxAxes]; + AxesBitmap axisOffsetsProbed; AxesBitmap xMapping, yMapping; FansBitmap fanMapping; Filament *filament; @@ -149,9 +151,4 @@ inline const float *Tool::GetOffsets() const return offset; } -inline void Tool::SetOffset(size_t axis, float offs) -{ - offset[axis] = offs; -} - #endif /* TOOL_H_ */ diff --git a/src/Version.h b/src/Version.h index f7d87e17..d4e6ab47 100644 --- a/src/Version.h +++ b/src/Version.h @@ -9,11 +9,11 @@ #define SRC_VERSION_H_ #ifndef VERSION -# define VERSION "1.20beta1" +# define VERSION "1.20beta2" #endif #ifndef DATE -# define DATE "2017-10-02" +# define DATE "2017-10-25" #endif #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman" |