diff --git a/README.md b/README.md
index 1fce505..33e2ddd 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,5 @@
-# The-Legend-of-Delta
+# Firmware
+Repetier firmware for all RAMBo controlled SeeMeCNC 3D Printers. One firmware, configuarable for all RAMBo deltas.
+
+This firmware is based off of Repetier Firmware, through many years and versions, as well as some integration with features of marlin and others work.
diff --git a/Repetier Firmware/Repetier/Commands.cpp b/Repetier Firmware/Repetier/Commands.cpp
new file mode 100644
index 0000000..a214c60
--- /dev/null
+++ b/Repetier Firmware/Repetier/Commands.cpp
@@ -0,0 +1,2738 @@
+/*
+ This file is part of Repetier-Firmware.
+
+ Repetier-Firmware is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Repetier-Firmware is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Repetier-Firmware. If not, see .
+
+ This firmware is a nearly complete rewrite of the sprinter firmware
+ by kliment (https://github.com/kliment/Sprinter)
+ which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.
+*/
+
+#include "Repetier.h"
+
+uint8_t mpu_threshold = 50;
+//float probes[9];
+
+#include
+
+#define ACCELEROMETER_I2C_ADDR 0x19
+
+void accelerometer_send(uint8_t val)
+{
+#if FEATURE_Z_PROBE == 1
+ Wire.beginTransmission(ACCELEROMETER_I2C_ADDR);
+ Wire.write(val);
+ if(Wire.endTransmission(false))
+ //Myserial.println(F("send i2c error."));
+ Com::printFLN(PSTR("accelerometer send i2c failed."));
+#endif
+}
+
+void accelerometer_write(uint8_t reg, uint8_t val)
+{
+#if FEATURE_Z_PROBE == 1
+ Wire.beginTransmission(ACCELEROMETER_I2C_ADDR);
+ Wire.write(reg);
+ Wire.write(val);
+ if(Wire.endTransmission())
+ //Myserial.println(F("write i2c error."));
+ Com::printFLN(PSTR("accelerometer write i2c failed."));
+#endif
+}
+
+bool accelerometer_recv(uint8_t reg)
+{
+#if FEATURE_Z_PROBE == 1
+ uint8_t receiveByte;
+
+ accelerometer_send(reg); //Send an 8bit register to be read
+
+ Wire.requestFrom(ACCELEROMETER_I2C_ADDR,1); //Request one 8bit response
+
+ if(Wire.available())
+ {
+ receiveByte = Wire.read();
+
+// Com::printF(PSTR("read reg "),reg);
+// Com::printFLN(PSTR(" value: "),receiveByte);
+ return true;
+ }
+ else
+ {
+ Com::printFLN(PSTR("accelerometer i2c recv failed."));
+ return false;
+ //Serial.println(F("i2c recv error."));
+ }
+#else
+ return false;
+#endif
+}
+
+void accelerometer_init()
+{
+#if FEATURE_Z_PROBE == 1
+ Com::printFLN(PSTR("iis2dh accelerometer initializing..."));
+ Wire.begin(); // join i2c bus
+
+ accelerometer_recv(0x0F); //WHO AM I = 0x6A
+
+ accelerometer_recv(0x31); //INT1_SRC (31h)
+
+ //CTRL_REG1 (20h)
+ accelerometer_recv(0x20);
+ accelerometer_write(0x20,0b10011100); // ODR 5.376kHz in LPMode [7-4]. Low power enable [3]. Z enable [2].
+ accelerometer_recv(0x20);
+
+
+ //CTRL_REG3 (22h)
+ accelerometer_recv(0x22);
+ accelerometer_write(0x22,0b01000000); // CLICK interrupt on INT1 pin [7]. AOI (And Or Interrupt) on INT1 en [6]. AOI on INT2 en [5].
+ accelerometer_recv(0x22);
+
+ //CTRL_REG6 (25h)
+ accelerometer_recv(0x25);
+ accelerometer_write(0x25,0b000000); //Click interrupt on INT2 pin [7]. Interrupt 1 function enable on INT2 pin [6]. Interrupt 2 on INT2 pin enable [5]. 0=INT Active High [1].
+ accelerometer_recv(0x25);
+
+ //CTRL_REG4 (23h)
+ accelerometer_recv(0x23);
+ accelerometer_write(0x23,0b00110000); // Full-scale selection 16G [5-4]. High resolution mode [3].
+ accelerometer_recv(0x23);
+
+
+ //CTRL_REG5 (24h)
+ accelerometer_recv(0x24);
+ accelerometer_write(0x24,0b01001010); // FIFO enable [6]. Latch INT1 [3]. Latch INT2 until cleared by read [1].
+ accelerometer_recv(0x24);
+
+
+ //INT1_CFG (30h)
+ accelerometer_recv(0x30);
+ accelerometer_write(0x30,0b100000); // ZHI events enabled [5]. ZLO events enabled [4].
+ accelerometer_recv(0x30);
+
+ //INT1_SRC (31h)
+ accelerometer_recv(0x31);
+
+ //INT1_THS (32h) this is the i2c probe
+ accelerometer_recv(0x32);
+ accelerometer_write(0x32,Z_PROBE_SENSITIVITY); // 7bits
+ accelerometer_recv(0x32);
+
+ //INT1_DURATION (33h)
+ accelerometer_recv(0x33);
+ accelerometer_write(0x33,0);
+ accelerometer_recv(0x33);
+
+ //INT2_CFG (34h)
+ accelerometer_recv(0x34);
+ accelerometer_write(0x34,0b000000); // ZHI events not enabled on INT2 [5].
+ accelerometer_recv(0x34);
+
+ //INT2_SRC (35h)
+
+ //INT2_THS (36h)
+ accelerometer_recv(0x36);
+ accelerometer_write(0x36,50); // 7bits
+ accelerometer_recv(0x36);
+
+ //INT2_DURATION (37h)
+ accelerometer_recv(0x37);
+ accelerometer_write(0x37,0);
+ accelerometer_recv(0x37);
+
+
+ //CLICK_CFG (38h)
+ accelerometer_recv(0x38);
+ accelerometer_write(0x38,0b10000); //Single Click Z axis
+ accelerometer_recv(0x38);
+
+ //CLICK_SRC (39h)
+ accelerometer_recv(0x39);
+
+ //CLICK_THS (3Ah)
+ accelerometer_recv(0x3A);
+ accelerometer_write(0x3A,50);
+ accelerometer_recv(0x3A);
+#endif
+}
+
+bool accelerometer_status()
+{
+#if FEATURE_Z_PROBE == 1
+ bool retValue = true;
+
+ if(!accelerometer_recv(0x31)) { retValue = false; } //INT1_SRC (31h)
+ if(!accelerometer_recv(0x35)) { retValue = false; } //INT1_SRC (31h)
+ if(!accelerometer_recv(0x39)) { retValue = false; } //INT1_SRC (31h)
+ if(!accelerometer_recv(0x2D)) { retValue = false; } //INT1_SRC (31h)
+
+ return(retValue);
+#else
+ return(false);
+#endif
+}
+
+const int sensitive_pins[] PROGMEM = SENSITIVE_PINS; // Sensitive pin list for M42
+int Commands::lowestRAMValue = MAX_RAM;
+int Commands::lowestRAMValueSend = MAX_RAM;
+
+void Commands::commandLoop()
+{
+ while(true)
+ {
+#ifdef DEBUG_PRINT
+ debugWaitLoop = 1;
+#endif
+ if(!Printer::isBlockingReceive())
+ {
+ GCode::readFromSerial();
+ GCode *code = GCode::peekCurrentCommand();
+ //UI_SLOW; // do longer timed user interface action
+ UI_MEDIUM; // do check encoder
+ if(code)
+ {
+#if SDSUPPORT
+ if(sd.savetosd)
+ {
+ if(!(code->hasM() && code->M == 29)) // still writing to file
+ sd.writeCommand(code);
+ else
+ sd.finishWrite();
+#if ECHO_ON_EXECUTE
+ code->echoCommand();
+#endif
+ }
+ else
+#endif
+ Commands::executeGCode(code);
+ code->popCurrentCommand();
+ }
+ }
+ else
+ {
+ UI_MEDIUM;
+ }
+ Printer::defaultLoopActions();
+ }
+}
+
+void Commands::checkForPeriodicalActions(bool allowNewMoves)
+{
+ if(!executePeriodical) return;
+ executePeriodical = 0;
+ Extruder::manageTemperatures();
+ if(--counter250ms == 0)
+ {
+ if(manageMonitor <= 1 + NUM_EXTRUDER)
+ writeMonitor();
+ counter250ms = 5;
+ }
+ // If called from queueDelta etc. it is an error to start a new move since it
+ // would invalidate old computation resulting in unpredicted behaviour.
+ // lcd controller can start new moves, so we disallow it if called from within
+ // a move command.
+ UI_SLOW(allowNewMoves);
+}
+
+/** \brief Waits until movement cache is empty.
+
+ Some commands expect no movement, before they can execute. This function
+ waits, until the steppers are stopped. In the meanwhile it buffers incoming
+ commands and manages temperatures.
+*/
+void Commands::waitUntilEndOfAllMoves()
+{
+#ifdef DEBUG_PRINT
+ debugWaitLoop = 8;
+#endif
+ while(PrintLine::hasLines())
+ {
+ GCode::readFromSerial();
+ checkForPeriodicalActions(false);
+ UI_MEDIUM;
+ }
+}
+
+void Commands::waitUntilEndOfAllBuffers()
+{
+ GCode *code = NULL;
+#ifdef DEBUG_PRINT
+ debugWaitLoop = 9;
+#endif
+ while(PrintLine::hasLines() || (code != NULL))
+ {
+ GCode::readFromSerial();
+ code = GCode::peekCurrentCommand();
+ UI_MEDIUM; // do check encoder
+ if(code)
+ {
+#if SDSUPPORT
+ if(sd.savetosd)
+ {
+ if(!(code->hasM() && code->M == 29)) // still writing to file
+ sd.writeCommand(code);
+ else
+ sd.finishWrite();
+#if ECHO_ON_EXECUTE
+ code->echoCommand();
+#endif
+ }
+ else
+#endif
+ Commands::executeGCode(code);
+ code->popCurrentCommand();
+ }
+ Commands::checkForPeriodicalActions(false); // only called from memory
+ UI_MEDIUM;
+ }
+}
+
+void Commands::printCurrentPosition(FSTRINGPARAM(s))
+{
+ float x, y, z;
+ Printer::realPosition(x, y, z);
+ if (isnan(x) || isinf(x) || isnan(y) || isinf(y) || isnan(z) || isinf(z))
+ {
+ Com::printErrorFLN(s); // flag where the error condition came from
+ }
+ x += Printer::coordinateOffset[X_AXIS];
+ y += Printer::coordinateOffset[Y_AXIS];
+ z += Printer::coordinateOffset[Z_AXIS];
+ Com::printF(Com::tXColon, x * (Printer::unitIsInches ? 0.03937 : 1), 2);
+ Com::printF(Com::tSpaceYColon, y * (Printer::unitIsInches ? 0.03937 : 1), 2);
+ Com::printF(Com::tSpaceZColon, z * (Printer::unitIsInches ? 0.03937 : 1), 3);
+ Com::printFLN(Com::tSpaceEColon, Printer::currentPositionSteps[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS] * (Printer::unitIsInches ? 0.03937 : 1), 4);
+ //Com::printF(PSTR("OffX:"),Printer::offsetX); // to debug offset handling
+ //Com::printFLN(PSTR(" OffY:"),Printer::offsetY);
+}
+
+void Commands::printTemperatures(bool showRaw)
+{
+ float temp = Extruder::current->tempControl.currentTemperatureC;
+#if HEATED_BED_SENSOR_TYPE == 0
+ Com::printF(Com::tTColon,temp);
+ Com::printF(Com::tSpaceSlash,Extruder::current->tempControl.targetTemperatureC,0);
+#else
+ Com::printF(Com::tTColon,temp);
+ Com::printF(Com::tSpaceSlash,Extruder::current->tempControl.targetTemperatureC,0);
+#if HAVE_HEATED_BED
+ Com::printF(Com::tSpaceBColon,Extruder::getHeatedBedTemperature());
+ Com::printF(Com::tSpaceSlash,heatedBedController.targetTemperatureC,0);
+ if(showRaw)
+ {
+ Com::printF(Com::tSpaceRaw,(int)NUM_EXTRUDER);
+ Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - heatedBedController.currentTemperature);
+ }
+ Com::printF(Com::tSpaceBAtColon,(pwm_pos[heatedBedController.pwmIndex])); // Show output of autotune when tuning!
+#endif
+#endif
+#if TEMP_PID
+ Com::printF(Com::tSpaceAtColon,(autotuneIndex==255?pwm_pos[Extruder::current->id]:pwm_pos[autotuneIndex])); // Show output of autotune when tuning!
+#endif
+#if NUM_EXTRUDER>1 && MIXING_EXTRUDER == 0
+ for(uint8_t i = 0; i < NUM_EXTRUDER; i++)
+ {
+ Com::printF(Com::tSpaceT,(int)i);
+ Com::printF(Com::tColon,extruder[i].tempControl.currentTemperatureC);
+ Com::printF(Com::tSpaceSlash,extruder[i].tempControl.targetTemperatureC,0);
+#if TEMP_PID
+ Com::printF(Com::tSpaceAt,(int)i);
+ Com::printF(Com::tColon,(pwm_pos[extruder[i].tempControl.pwmIndex])); // Show output of autotune when tuning!
+#endif
+ if(showRaw)
+ {
+ Com::printF(Com::tSpaceRaw,(int)i);
+ Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - extruder[i].tempControl.currentTemperature);
+ }
+ }
+#endif
+ Com::println();
+}
+void Commands::changeFeedrateMultiply(int factor)
+{
+ if(factor < 25) factor = 25;
+ if(factor > 500) factor = 500;
+ Printer::feedrate *= (float)factor / (float)Printer::feedrateMultiply;
+ Printer::feedrateMultiply = factor;
+ Com::printFLN(Com::tSpeedMultiply, factor);
+}
+void Commands::changeHorizontalRadius(float hradius)
+{
+ if (hradius < 60) hradius = 60;
+ if (hradius > 150) hradius = 150;
+ Printer::radius0 = hradius;
+ Com::printFLN(Com::tHorizontalRadius, hradius);
+ // EEPROM::storeDataIntoEEPROM(false);
+}
+void Commands::changeFlowrateMultiply(int factor)
+{
+ if(factor < 25) factor = 25;
+ if(factor > 200) factor = 200;
+ Printer::extrudeMultiply = factor;
+ if(Extruder::current->diameter <= 0)
+ Printer::extrusionFactor = 0.01f * static_cast(factor);
+ else
+ Printer::extrusionFactor = 0.01f * static_cast(factor) * 4.0f / (Extruder::current->diameter * Extruder::current->diameter * 3.141592654f);
+ Com::printFLN(Com::tFlowMultiply, factor);
+}
+
+void Commands::setFanSpeed(int speed,bool wait)
+{
+#if FAN_PIN>-1 && FEATURE_FAN_CONTROL
+ speed = constrain(speed,0,255);
+ Printer::setMenuMode(MENU_MODE_FAN_RUNNING,speed != 0);
+ if(wait)
+ Commands::waitUntilEndOfAllMoves(); // use only if neededthis to change the speed exactly at that point, but it may cause blobs if you do!
+ if(speed != pwm_pos[NUM_EXTRUDER + 2])
+ Com::printFLN(Com::tFanspeed,speed); // send only new values to break update loops!
+ pwm_pos[NUM_EXTRUDER + 2] = speed;
+#endif
+}
+
+void Commands::reportPrinterUsage()
+{
+#if EEPROM_MODE!=0
+ float dist = Printer::filamentPrinted * 0.001 + HAL::eprGetFloat(EPR_PRINTING_DISTANCE);
+ Com::printF(Com::tPrintedFilament,dist,2);
+ Com::printF(Com::tSpacem);
+ bool alloff = true;
+ for(uint8_t i=0; itargetTemperatureC>15) alloff = false;
+
+ int32_t seconds = (alloff ? 0 : (HAL::timeInMilliseconds() - Printer::msecondsPrinting) / 1000) + HAL::eprGetInt32(EPR_PRINTING_TIME);
+ int32_t tmp = seconds / 86400;
+ seconds -= tmp * 86400;
+ Com::printF(Com::tPrintingTime,tmp);
+ tmp=seconds / 3600;
+ Com::printF(Com::tSpaceDaysSpace,tmp);
+ seconds-=tmp * 3600;
+ tmp = seconds / 60;
+ Com::printF(Com::tSpaceHoursSpace,tmp);
+ Com::printFLN(Com::tSpaceMin);
+#endif
+}
+#if STEPPER_CURRENT_CONTROL == CURRENT_CONTROL_DIGIPOT
+// Digipot methods for controling current and microstepping
+
+#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
+int digitalPotWrite(int address, unsigned int value) // From Arduino DigitalPotControl example
+{
+ WRITE(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
+ HAL::spiSend(address); // send in the address and value via SPI:
+ HAL::spiSend(value);
+ WRITE(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
+ //delay(10);
+}
+
+void setMotorCurrent(uint8_t driver, unsigned int current)
+{
+ const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
+ digitalPotWrite(digipot_ch[driver], current);
+}
+#endif
+
+void motorCurrentControlInit() //Initialize Digipot Motor Current
+{
+#if DIGIPOTSS_PIN && DIGIPOTSS_PIN > -1
+ const uint8_t digipot_motor_current[] = MOTOR_CURRENT;
+
+ HAL::spiInit(0); //SPI.begin();
+ SET_OUTPUT(DIGIPOTSS_PIN);
+ for(int i = 0; i <= 4; i++)
+ //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
+ setMotorCurrent(i,digipot_motor_current[i]);
+#endif
+}
+#endif
+#if STEPPER_CURRENT_CONTROL==CURRENT_CONTROL_PWM
+// Controlling motor current directly using PWM
+//unsigned int motor_current_setting[3] = MOTOR_CURRENT_PWM; //unsigned int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
+
+void setMotorCurrent(uint8_t driver, unsigned int current)
+{
+ if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, current); //if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, 50);
+ if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, current); //if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, 50);
+ if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, current); //if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, 50);
+
+ /* OLD for setting current on eris and/or droplit. Use motor currents in configuration.h now
+ #if PRINTER == 4
+ if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, 50);
+ if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, 50);
+ if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, 50);
+ #else
+ if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, motor_current_setting[0]); //if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, 50);
+ if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, motor_current_setting[1]); //if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, 50);
+ if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, motor_current_setting[2]); //if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, 50);
+ #endif
+ */
+}
+void motorCurrentControlInit() //Initialize Digipot Motor Current
+{
+ #if defined MOTOR_CURRENT_PWM_XY_PIN //copied from Marlin
+ const uint8_t pwm_motor_current[] = MOTOR_CURRENT_PWM;
+ SET_OUTPUT(MOTOR_CURRENT_PWM_XY_PIN);
+ SET_OUTPUT(MOTOR_CURRENT_PWM_Z_PIN);
+ SET_OUTPUT(MOTOR_CURRENT_PWM_E_PIN);
+ setMotorCurrent(0, pwm_motor_current[0]);
+ setMotorCurrent(1, pwm_motor_current[1]);
+ setMotorCurrent(2, pwm_motor_current[2]);
+ //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
+ TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
+ #endif
+ }
+#endif
+#if STEPPER_CURRENT_CONTROL==CURRENT_CONTROL_LTC2600
+
+void setMotorCurrent( uint8_t channel, unsigned short level )
+{
+ const uint8_t ltc_channels[] = LTC2600_CHANNELS;
+ if(channel > LTC2600_NUM_CHANNELS) return;
+ uint8_t address = ltc_channels[channel];
+ char i;
+
+
+ // NOTE: Do not increase the current endlessly. In case the engine reaches its current saturation, the engine and the driver can heat up and loss power.
+ // When the saturation is reached, more current causes more heating and more power loss.
+ // In case of engines with lower quality, the saturation current may be reached before the nominal current.
+
+ // configure the pins
+ WRITE( LTC2600_CS_PIN, HIGH );
+ SET_OUTPUT( LTC2600_CS_PIN );
+ WRITE( LTC2600_SCK_PIN, LOW );
+ SET_OUTPUT( LTC2600_SCK_PIN );
+ WRITE( LTC2600_SDI_PIN, LOW );
+ SET_OUTPUT( LTC2600_SDI_PIN );
+
+ // enable the command interface of the LTC2600
+ WRITE( LTC2600_CS_PIN, LOW );
+
+ // transfer command and address
+ for( i = 7; i >= 0; i-- )
+ {
+ WRITE( LTC2600_SDI_PIN, address & (0x01 << i));
+ WRITE( LTC2600_SCK_PIN, 1 );
+ WRITE( LTC2600_SCK_PIN, 0 );
+ }
+
+ // transfer the data word
+ for( i = 15; i >= 0; i-- )
+ {
+ WRITE( LTC2600_SDI_PIN, level & (0x01 << i));
+ WRITE( LTC2600_SCK_PIN, 1 );
+ WRITE( LTC2600_SCK_PIN, 0 );
+ }
+
+ // disable the ommand interface of the LTC2600 -
+ // this carries out the specified command
+ WRITE( LTC2600_CS_PIN, HIGH );
+
+} // setLTC2600
+
+void motorCurrentControlInit() //Initialize LTC2600 Motor Current
+{
+ const unsigned int ltc_current[] = MOTOR_CURRENT;
+ uint8_t i;
+ for(i=0; i -1
+void microstepMS(uint8_t driver, int8_t ms1, int8_t ms2)
+{
+ if(ms1 > -1) switch(driver)
+ {
+ case 0:
+ WRITE( X_MS1_PIN,ms1);
+ break;
+ case 1:
+ WRITE( Y_MS1_PIN,ms1);
+ break;
+ case 2:
+ WRITE( Z_MS1_PIN,ms1);
+ break;
+ case 3:
+ WRITE(E0_MS1_PIN,ms1);
+ break;
+ case 4:
+ WRITE(E1_MS1_PIN,ms1);
+ break;
+ }
+ if(ms2 > -1) switch(driver)
+ {
+ case 0:
+ WRITE( X_MS2_PIN,ms2);
+ break;
+ case 1:
+ WRITE( Y_MS2_PIN,ms2);
+ break;
+ case 2:
+ WRITE( Z_MS2_PIN,ms2);
+ break;
+ case 3:
+ WRITE(E0_MS2_PIN,ms2);
+ break;
+ case 4:
+ WRITE(E1_MS2_PIN,ms2);
+ break;
+ }
+}
+
+void microstepMode(uint8_t driver, uint8_t stepping_mode)
+{
+ switch(stepping_mode)
+ {
+ case 1:
+ microstepMS(driver,MICROSTEP1);
+ break;
+ case 2:
+ microstepMS(driver,MICROSTEP2);
+ break;
+ case 4:
+ microstepMS(driver,MICROSTEP4);
+ break;
+ case 8:
+ microstepMS(driver,MICROSTEP8);
+ break;
+ case 16:
+ microstepMS(driver,MICROSTEP16);
+ break;
+ }
+}
+void microstepReadings()
+{
+ Com::printFLN(Com::tMS1MS2Pins);
+ Com::printF(Com::tXColon,READ(X_MS1_PIN));
+ Com::printFLN(Com::tComma,READ(X_MS2_PIN));
+ Com::printF(Com::tYColon,READ(Y_MS1_PIN));
+ Com::printFLN(Com::tComma,READ(Y_MS2_PIN));
+ Com::printF(Com::tZColon,READ(Z_MS1_PIN));
+ Com::printFLN(Com::tComma,READ(Z_MS2_PIN));
+ Com::printF(Com::tE0Colon,READ(E0_MS1_PIN));
+ Com::printFLN(Com::tComma,READ(E0_MS2_PIN));
+ Com::printF(Com::tE1Colon,READ(E1_MS1_PIN));
+ Com::printFLN(Com::tComma,READ(E1_MS2_PIN));
+}
+#endif
+
+void microstepInit()
+{
+#if defined(X_MS1_PIN) && X_MS1_PIN > -1
+ const uint8_t microstep_modes[] = MICROSTEP_MODES;
+ SET_OUTPUT(X_MS2_PIN);
+ SET_OUTPUT(Y_MS2_PIN);
+ SET_OUTPUT(Z_MS2_PIN);
+ SET_OUTPUT(E0_MS2_PIN);
+ SET_OUTPUT(E1_MS2_PIN);
+ for(int i=0; i<=4; i++) microstepMode(i,microstep_modes[i]);
+#endif
+}
+
+/**
+ \brief Execute the Arc command stored in com.
+*/
+#if ARC_SUPPORT
+void Commands::processArc(GCode *com)
+{
+ float position[Z_AXIS_ARRAY];
+ Printer::realPosition(position[X_AXIS],position[Y_AXIS],position[Z_AXIS]);
+ if(!Printer::setDestinationStepsFromGCode(com)) return; // For X Y Z E F
+ float offset[2] = {Printer::convertToMM(com->hasI() ? com->I : 0),Printer::convertToMM(com->hasJ() ? com->J : 0)};
+ float target[E_AXIS_ARRAY] = {Printer::realXPosition(),Printer::realYPosition(),Printer::realZPosition(),Printer::destinationSteps[E_AXIS]*Printer::invAxisStepsPerMM[E_AXIS]};
+ float r;
+ if (com->hasR())
+ {
+ /*
+ We need to calculate the center of the circle that has the designated radius and passes
+ through both the current position and the target position. This method calculates the following
+ set of equations where [x,y] is the vector from current to target position, d == magnitude of
+ that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
+ the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
+ length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
+ [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
+
+ d^2 == x^2 + y^2
+ h^2 == r^2 - (d/2)^2
+ i == x/2 - y/d*h
+ j == y/2 + x/d*h
+
+ O <- [i,j]
+ - |
+ r - |
+ - |
+ - | h
+ - |
+ [0,0] -> C -----------------+--------------- T <- [x,y]
+ | <------ d/2 ---->|
+
+ C - Current position
+ T - Target position
+ O - center of circle that pass through both C and T
+ d - distance from C to T
+ r - designated radius
+ h - distance from center of CT to O
+
+ Expanding the equations:
+
+ d -> sqrt(x^2 + y^2)
+ h -> sqrt(4 * r^2 - x^2 - y^2)/2
+ i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+ j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+
+ Which can be written:
+
+ i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+ j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+
+ Which we for size and speed reasons optimize to:
+
+ h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
+ i = (x - (y * h_x2_div_d))/2
+ j = (y + (x * h_x2_div_d))/2
+
+ */
+ r = Printer::convertToMM(com->R);
+ // Calculate the change in position along each selected axis
+ double x = target[X_AXIS]-position[X_AXIS];
+ double y = target[Y_AXIS]-position[Y_AXIS];
+
+ double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
+ // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
+ // real CNC, and thus - for practical reasons - we will terminate promptly:
+ if(isnan(h_x2_div_d))
+ {
+ Com::printErrorFLN(Com::tInvalidArc);
+ return;
+ }
+ // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
+ if (com->G==3)
+ {
+ h_x2_div_d = -h_x2_div_d;
+ }
+
+ /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
+ the left hand circle will be generated - when it is negative the right hand circle is generated.
+
+
+ T <-- Target position
+
+ ^
+ Clockwise circles with this center | Clockwise circles with this center will have
+ will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
+ \ | /
+ center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
+ |
+ |
+
+ C <-- Current position */
+
+
+ // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
+ // even though it is advised against ever generating such circles in a single line of g-code. By
+ // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
+ // travel and thus we get the unadvisably long arcs as prescribed.
+ if (r < 0)
+ {
+ h_x2_div_d = -h_x2_div_d;
+ r = -r; // Finished with r. Set to positive for mc_arc
+ }
+ // Complete the operation by calculating the actual center of the arc
+ offset[0] = 0.5*(x-(y*h_x2_div_d));
+ offset[1] = 0.5*(y+(x*h_x2_div_d));
+
+ }
+ else // Offset mode specific computations
+ {
+ r = hypot(offset[0], offset[1]); // Compute arc radius for arc
+ }
+ // Set clockwise/counter-clockwise sign for arc computations
+ uint8_t isclockwise = com->G == 2;
+ // Trace the arc
+ PrintLine::arc(position, target, offset, r, isclockwise);
+}
+#endif
+
+/**
+ \brief Execute the G command stored in com.
+*/
+void Commands::processGCode(GCode *com)
+{
+ uint32_t codenum; //throw away variable
+ switch(com->G)
+ {
+ case 0: // G0 -> G1
+ case 1: // G1
+ if(com->hasS()) Printer::setNoDestinationCheck(com->S != 0);
+ if(Printer::setDestinationStepsFromGCode(com)) // For X Y Z E F
+#if NONLINEAR_SYSTEM
+ if (!PrintLine::queueDeltaMove(ALWAYS_CHECK_ENDSTOPS, true, true))
+ {
+ Com::printWarningFLN(PSTR("executeGCode / queueDeltaMove returns error"));
+ }
+#else
+ PrintLine::queueCartesianMove(ALWAYS_CHECK_ENDSTOPS, true);
+#endif
+#if UI_HAS_KEYS
+ // ui can only execute motion commands if we are not waiting inside a move for an
+ // old move to finish. For normal response times, we always leave one free after
+ // sending a line. Drawback: 1 buffer line less for limited time. Since input cache
+ // gets filled while waiting, the lost is neglectible.
+ PrintLine::waitForXFreeLines(1, true);
+#endif // UI_HAS_KEYS
+ break;
+#if ARC_SUPPORT
+ case 2: // CW Arc
+ case 3: // CCW Arc MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
+ processArc(com);
+ break;
+#endif
+ case 4: // G4 dwell
+ Commands::waitUntilEndOfAllMoves();
+ codenum = 0;
+ if(com->hasP()) codenum = com->P; // milliseconds to wait
+ if(com->hasS()) codenum = com->S * 1000; // seconds to wait
+ codenum += HAL::timeInMilliseconds(); // keep track of when we started waiting
+ while((uint32_t)(codenum-HAL::timeInMilliseconds()) < 2000000000 )
+ {
+ GCode::readFromSerial();
+ Commands::checkForPeriodicalActions(true);
+ }
+ break;
+#if FEATURE_RETRACTION && NUM_EXTRUDER > 0
+ case 10: // G10 S<1 = long retract, 0 = short retract = default> retracts filament accoridng to stored setting
+#if NUM_EXTRUDER > 1
+ Extruder::current->retract(true, com->hasS() && com->S > 0);
+#else
+ Extruder::current->retract(true, false);
+#endif
+ break;
+ case 11: // G11 S<1 = long retract, 0 = short retract = default> = Undo retraction according to stored setting
+#if NUM_EXTRUDER > 1
+ Extruder::current->retract(false, com->hasS() && com->S > 0);
+#else
+ Extruder::current->retract(false, false);
+#endif
+ break;
+#endif // FEATURE_RETRACTION
+ case 20: // G20 Units to inches
+ Printer::unitIsInches = 1;
+ break;
+ case 21: // G21 Units to mm
+ Printer::unitIsInches = 0;
+ break;
+ case 28: //G28 Home all Axis one at a time
+ {
+ uint8_t homeAllAxis = (com->hasNoXYZ() && !com->hasE());
+ if(com->hasE())
+ Printer::currentPositionSteps[E_AXIS] = 0;
+ if(homeAllAxis || !com->hasNoXYZ())
+ Printer::homeAxis(homeAllAxis || com->hasX(),homeAllAxis || com->hasY(),homeAllAxis || com->hasZ());
+ Printer::updateCurrentPosition();
+ if(Printer::isXMaxEndstopHit() || Printer::isYMaxEndstopHit() || Printer::isZMaxEndstopHit()){
+ GCode::executeFString(PSTR("M117 ENDSTOP ERROR"));
+ Com::printF(PSTR("Error: "));
+ if(Printer::isXMaxEndstopHit()) Com::printF(PSTR("X "));
+ if(Printer::isYMaxEndstopHit()) Com::printF(PSTR("Y "));
+ if(Printer::isZMaxEndstopHit()) Com::printF(PSTR("Z "));
+ Com::printFLN(PSTR("Endstop(s) not working properly"));
+ }
+ }
+ break;
+#if FEATURE_Z_PROBE
+ case 29: // G29 Probe for Endstop Offsets, Horizontal Radius, and Z Height
+ {
+ if(!accelerometer_status()){
+ delay(250);
+ if(!accelerometer_status()) {
+ Com::printFLN(PSTR("I2C Error - Calibration Aborted"));
+ GCode::executeFString(PSTR("M117 I2C Error. Aborting"));
+ break;
+ }
+ }
+ GCode::executeFString(PSTR("M104 S0\nM140 S0\nM107"));
+ float xProbe = 0, yProbe = 0, zProbe = 0, verify = 0, oldFeedrate = Printer::feedrate;
+ int32_t probeSensitivity = Z_PROBE_SENSITIVITY;
+ bool failedProbe = false;
+ EEPROM::setDeltaTowerXOffsetSteps(0); // set X offset to 0
+ EEPROM::setDeltaTowerYOffsetSteps(0); // set Y offset to 0
+ EEPROM::setDeltaTowerZOffsetSteps(0); // set Z offset to 0
+ EEPROM::storeDataIntoEEPROM(); // store offsets to 0 before doing anything
+ EEPROM::readDataFromEEPROM();
+
+ //Crank up the max Z accel for the Eris
+#if PRINTER == 3
+ Printer::maxTravelAccelerationMMPerSquareSecond[Z_AXIS] = 1850;
+ Printer::updateDerivedParameter();
+#endif
+
+ do{
+ Printer::homeAxis(true,true,true);
+ if(Printer::isXMaxEndstopHit() || Printer::isYMaxEndstopHit() || Printer::isZMaxEndstopHit()){
+ GCode::executeFString(PSTR("M117 ENDSTOP ERROR"));
+ Com::printF(PSTR("Error: "));
+ if(Printer::isXMaxEndstopHit()) Com::printF(PSTR("X "));
+ if(Printer::isYMaxEndstopHit()) Com::printF(PSTR("Y "));
+ if(Printer::isZMaxEndstopHit()) Com::printF(PSTR("Z "));
+ Com::printFLN(PSTR("Endstop(s) not working properly"));
+ failedProbe = true;
+ break;
+ }
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ Printer::moveTo(EEPROM::zProbeX1(),EEPROM::zProbeY1(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ xProbe = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //First tap
+ verify = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //Second tap
+ if ((xProbe - verify) > Z_PROBE_TOLERANCE || (xProbe - verify) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe (X Tower) failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }else{
+ Com::printFLN(PSTR("Calibration Failed"));
+ GCode::executeFString(PSTR("M117 CALIBRATION FAILED"));
+ Com::printErrorFLN(Com::tZProbeFailed);
+ break;
+ }
+ xProbe = -1; failedProbe = true;
+ continue;
+ }else{
+ xProbe = (xProbe + verify) / 2;
+ }
+ int32_t offsetX = ((xProbe * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsX = EEPROM::deltaTowerXOffsetSteps();
+
+ if(com->hasS() && com->S == 2){
+ Printer::homeAxis(true,true,true);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ }
+ Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ yProbe = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //First tap
+ verify = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //Second tap
+ if ((yProbe - verify) > Z_PROBE_TOLERANCE || (yProbe - verify) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe (Y Tower) failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }else{
+ Com::printFLN(PSTR("Calibration Failed"));
+ GCode::executeFString(PSTR("M117 CALIBRATION FAILED"));
+ Com::printErrorFLN(Com::tZProbeFailed);
+ break;
+ }
+ yProbe = -1; failedProbe = true;
+ continue;
+ }else{
+ yProbe = (yProbe + verify) / 2;
+ }
+ int32_t offsetY = ((yProbe * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsY = EEPROM::deltaTowerYOffsetSteps();
+
+ if(com->hasS() && com->S == 2){
+ Printer::homeAxis(true,true,true);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ }
+ Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ zProbe = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //First tap
+ verify = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //Second tap
+ if ((zProbe - verify) > Z_PROBE_TOLERANCE || (zProbe - verify) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe (Z Tower) failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }else{
+ Com::printFLN(PSTR("Calibration Failed"));
+ GCode::executeFString(PSTR("M117 CALIBRATION FAILED"));
+ Com::printErrorFLN(Com::tZProbeFailed);
+ break;
+ }
+ zProbe = -1; failedProbe = true;
+ continue;
+ }else{
+ zProbe = (zProbe + verify) / 2;
+ }
+ int32_t offsetZ = ((zProbe * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsZ = EEPROM::deltaTowerZOffsetSteps();
+
+ Printer::updateCurrentPosition();
+ Printer::updateDerivedParameter();
+ Com::printInfoFLN(Com::tZProbeZReset);
+ Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength);
+ Printer::feedrate = oldFeedrate;
+
+ if(offsetX < offsetY && offsetX < offsetZ)
+ {
+ offsetY = offsetStepsY + (offsetY - offsetX);
+ offsetZ = offsetStepsZ + (offsetZ - offsetX);
+ offsetX = 0;
+ }
+ else if(offsetY < offsetX && offsetY < offsetZ)
+ {
+ offsetX = offsetStepsX + (offsetX - offsetY);
+ offsetZ = offsetStepsZ + (offsetZ - offsetY);
+ offsetY = 0;
+ }
+ else if(offsetZ < offsetX && offsetZ < offsetY)
+ {
+ offsetX = offsetStepsX + (offsetX - offsetZ);
+ offsetY = offsetStepsY + (offsetY - offsetZ);
+ offsetZ = 0;
+ }
+ if(offsetX > 400 || offsetY > 400 || offsetZ > 400){
+ xProbe = -1; yProbe = -1; zProbe = -1;
+ Com::printFLN(PSTR("OFFSETS OFF BY TOO MUCH. Aborting. Sensitivity: "), probeSensitivity);
+ GCode::executeFString(PSTR("M117 Endstop Offset Error"));
+ Com::printFLN(PSTR("X: "), offsetX);
+ Com::printFLN(PSTR("Y: "), offsetY);
+ Com::printFLN(PSTR("Z: "), offsetZ);
+ failedProbe = true;
+ break;
+ }else{
+ EEPROM::setDeltaTowerXOffsetSteps(offsetX);
+ EEPROM::setDeltaTowerYOffsetSteps(offsetY);
+ EEPROM::setDeltaTowerZOffsetSteps(offsetZ);
+ EEPROM::storeDataIntoEEPROM();
+ failedProbe = false;
+ }
+ }while(failedProbe);
+
+ if(failedProbe) break;
+ Printer::updateCurrentPosition(true);
+ Printer::feedrate = oldFeedrate;
+ printCurrentPosition(PSTR("G69 "));
+ GCode::executeFString(Com::tZProbeEndScript);
+
+ //Horizontal Radius Calc and Z Height
+ float cProbe, hradius;
+#if PRINTER == 5
+ float defaultRadius = 144.0;
+#else
+ float defaultRadius = PRINTER_RADIUS-END_EFFECTOR_HORIZONTAL_OFFSET-CARRIAGE_HORIZONTAL_OFFSET;
+#endif
+ float oldRadius;
+ int radiusLoop;
+ radiusLoop = 0;
+
+ do{
+ radiusLoop++;
+ oldRadius = Printer::radius0;
+ failedProbe = false;
+ EEPROM::storeDataIntoEEPROM(); //save firmware horizontal radius before calibration
+ EEPROM::readDataFromEEPROM();
+ Printer::setAutolevelActive(true);
+ Printer::homeAxis(true,true,true);
+ Printer::setAutolevelActive(false);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::moveTo(0,0,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ cProbe = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ verify = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ if ((cProbe - verify) > Z_PROBE_TOLERANCE || (cProbe - verify) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe (HR - Center Point) failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ radiusLoop--;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }else{
+ Com::printFLN(PSTR("Calibration Failed"));
+ GCode::executeFString(PSTR("M117 CALIBRATION FAILED"));
+ Com::printErrorFLN(Com::tZProbeFailed);
+ break;
+ }
+ cProbe = -1; failedProbe = true;
+ continue;
+ }else{
+ cProbe = (cProbe + verify) / 2;
+ Com::printFLN(PSTR("Old Z Height:"), Printer::zLength );
+ Printer::zLength += cProbe - Printer::currentPosition[Z_AXIS];
+ Printer::updateDerivedParameter();
+ Com::printFLN(PSTR("New Z Height:"), Printer::zLength );
+ }
+
+ Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ zProbe = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ verify = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ if ((zProbe - verify) > Z_PROBE_TOLERANCE || (zProbe - verify) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe (HR - Z tower) failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }else{
+ Com::printFLN(PSTR("Calibration Failed"));
+ GCode::executeFString(PSTR("M117 CALIBRATION FAILED"));
+ Com::printErrorFLN(Com::tZProbeFailed);
+ break;
+ }
+ zProbe = -1; failedProbe = true;
+ continue;
+ }else{
+ zProbe = (zProbe + verify) / 2;
+ }
+
+ hradius = (cProbe - zProbe)*AXIS_STEPS_PER_MM;
+ if(hradius<0)
+ {
+ hradius=-hradius;
+ hradius = (hradius / Printer::radius0)*2;
+ Com::printFLN(Com::tZProbeAverage,hradius);
+ Printer::radius0 = Printer::radius0 - hradius;
+ }else{
+ hradius = (hradius / Printer::radius0)*2;
+ Printer::radius0 = Printer::radius0 + hradius;
+ }
+
+ if(Printer::radius0 / defaultRadius > 1.1 || Printer::radius0 / defaultRadius < 0.9){
+ Com::printFLN(PSTR("Calculated Radius is bad: "), Printer::radius0 );
+ Printer::radius0 = defaultRadius;
+ }
+ Com::printFLN(PSTR("Old Horizontal Radius: "), oldRadius );
+ Com::printFLN(PSTR("New Horizontal Radius: "), Printer::radius0 );
+ }while(radiusLoop < 4 && (failedProbe || ((Printer::radius0 - oldRadius) > Z_PROBE_TOLERANCE) || ((Printer::radius0 - oldRadius) < -Z_PROBE_TOLERANCE) ));
+
+ //Reset the max Z accel for the Eris
+#if PRINTER == 3
+ Printer::maxTravelAccelerationMMPerSquareSecond[Z_AXIS] = 400;
+ Printer::updateDerivedParameter();
+#endif
+ Printer::setAutolevelActive(true);
+ Printer::updateCurrentPosition(true);
+ printCurrentPosition(PSTR("hehe"));
+ GCode::executeFString(Com::tZProbeEndScript);
+ Printer::feedrate = oldFeedrate;
+ Printer::homeAxis(true,true,true);
+ EEPROM::storeDataIntoEEPROM();
+#if SDSUPPORT == 1
+ if(sd.selectFile("g29cal.gcode", true)){
+ sd.stopPrint();
+ sd.deleteFile("g29cal.gcode");
+ sd.initsd();
+ }
+#endif
+ GCode::executeFString(PSTR("M117 CALIBRATION COMPLETE"));
+ }
+ break;
+
+ case 30: // G30 single probe set Z0
+ {
+ if(com->hasS() && com->S == 2){
+ float sum = 0, sum1 = 0,last,oldFeedrate = Printer::feedrate;
+ do{
+ if(com->hasS() && com->S == 2){ // only reset eeprom if saving new value
+ Printer::zLength = Z_MAX_LENGTH; // set Z height to firmware default
+ EEPROM::storeDataIntoEEPROM(); // store default before calibration
+ EEPROM::readDataFromEEPROM(); // would not take effect unless read!
+ }
+ Printer::homeAxis(true,true,true);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ Printer::moveTo(0,0,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ sum1 = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); // First tap
+ sum = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); // Second tap
+ if ((sum1 - sum) > Z_PROBE_TOLERANCE || (sum1 - sum) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printErrorFLN(Com::tZProbeFailed);
+ sum = -1;
+ }else{
+ sum = (sum + sum1) / 2;
+ }
+ }while(sum < 1);
+ if(com->hasS() && com->S)
+ {
+ Printer::zLength += sum - Printer::currentPosition[Z_AXIS];
+ Printer::updateDerivedParameter();
+ }
+ Printer::feedrate = oldFeedrate;
+ Printer::setAutolevelActive(true);
+ if(com->hasS() && com->S == 2)
+ EEPROM::storeDataIntoEEPROM();
+ Printer::updateCurrentPosition(true);
+ printCurrentPosition(PSTR("G30 "));
+ GCode::executeFString(Com::tZProbeEndScript);
+ Printer::feedrate = oldFeedrate;
+ Printer::homeAxis(true,true,true);
+ }else{
+ /*
+ if(com->hasP() && com->P >= 10){
+ for(int i=0;i<10;i++){
+ Com::printF(PSTR("Point "), i);
+ Com::printFLN(PSTR(" - PROBE OFFSET:"), probes[i] );
+ }
+ }else{
+ */
+#if PRINTER == 3
+ Printer::maxTravelAccelerationMMPerSquareSecond[Z_AXIS] = 1850;
+ Printer::updateDerivedParameter();
+#endif
+ float pProbe, verify;
+ int32_t probeSensitivity = Z_PROBE_SENSITIVITY;
+ if(com->hasX() && com->hasY()){
+ if(com->hasF()){ Printer::moveTo(com->X,com->Y,IGNORE_COORDINATE,IGNORE_COORDINATE,com->F); }
+ else{ Printer::moveTo(com->X,com->Y,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); }
+ }
+ pProbe = Printer::runZProbe(true,false,1,false);
+ pProbe -= Printer::currentPosition[Z_AXIS];
+ verify = Printer::runZProbe(false,true,1,false);
+ verify -= Printer::currentPosition[Z_AXIS];
+ if ((pProbe - verify) > Z_PROBE_TOLERANCE || (pProbe - verify) < - Z_PROBE_TOLERANCE){
+ Com::printFLN(PSTR("Probes do not match. Off by "), (pProbe - verify) );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ GCode::executeFString(PSTR("G30"));
+ }
+ }else{
+ pProbe = (pProbe + verify) / 2;
+ //if(com->hasP()){
+ // if(com->P < 10){ probes[com->P] = pProbe; }
+ //}else{
+ Com::printFLN(PSTR("PROBE-ZOFFSET:"), pProbe );
+ //}
+ }
+#if PRINTER == 3
+ Printer::maxTravelAccelerationMMPerSquareSecond[Z_AXIS] = 400;
+ Printer::updateDerivedParameter();
+#endif
+ //}
+ }
+ }
+ break;
+ case 31: // G31 display hall sensor output
+ Com::printF(Com::tZProbeState);
+ Com::print(Printer::isZProbeHit() ? 'H' : 'L');
+ Com::println();
+ break;
+#if FEATURE_AUTOLEVEL
+ case 32: // G32 Auto-Bed leveling
+ {
+ GCode::executeFString(Com::tZProbeStartScript);
+ //bool iterate = com->hasP() && com->P>0;
+ Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0;
+ Printer::setAutolevelActive(false); // iterate
+ float h1,h2,h3,hc,oldFeedrate = Printer::feedrate;
+ Printer::moveTo(EEPROM::zProbeX1(),EEPROM::zProbeY1(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ h1 = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ if(h1 < 0) break;
+ Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ h2 = Printer::runZProbe(false,false);
+ if(h2 < 0) break;
+ Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ h3 = Printer::runZProbe(false,true);
+ if(h3 < 0) break;
+ Printer::buildTransformationMatrix(h1,h2,h3);
+ //-(Rxx*Ryz*y-Rxz*Ryx*y+(Rxz*Ryy-Rxy*Ryz)*x)/(Rxy*Ryx-Rxx*Ryy)
+ // z = z-deviation from origin due to bed transformation
+ float z = -((Printer::autolevelTransformation[0] * Printer::autolevelTransformation[5] -
+ Printer::autolevelTransformation[2] * Printer::autolevelTransformation[3]) *
+ (float)Printer::currentPositionSteps[Y_AXIS] * Printer::invAxisStepsPerMM[Y_AXIS] +
+ (Printer::autolevelTransformation[2] * Printer::autolevelTransformation[4] -
+ Printer::autolevelTransformation[1] * Printer::autolevelTransformation[5]) *
+ (float)Printer::currentPositionSteps[X_AXIS] * Printer::invAxisStepsPerMM[X_AXIS]) /
+ (Printer::autolevelTransformation[1] * Printer::autolevelTransformation[3] - Printer::autolevelTransformation[0] * Printer::autolevelTransformation[4]);
+ Printer::zMin = 0;
+ if(com->hasS() && com->S < 3 && com->S > 0)
+ {
+#if MAX_HARDWARE_ENDSTOP_Z
+#if DRIVE_SYSTEM == DELTA
+ Printer::zLength += (h3 + z) - Printer::currentPosition[Z_AXIS];
+#else
+ int32_t zBottom = Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS];
+ Printer::zLength = Printer::runZMaxProbe() + zBottom * Printer::invAxisStepsPerMM[Z_AXIS] - ENDSTOP_Z_BACK_ON_HOME;
+#endif
+ Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength);
+#else // max hardware endstop
+#if DRIVE_SYSTEM != DELTA
+ Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS];
+#endif
+#endif
+ Printer::setAutolevelActive(true);
+ if(com->S == 2)
+ EEPROM::storeDataIntoEEPROM();
+ }
+ else
+ {
+#if DRIVE_SYSTEM != DELTA
+ Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS];
+#endif
+ if(com->hasS() && com->S == 3)
+ EEPROM::storeDataIntoEEPROM();
+ }
+ Printer::setAutolevelActive(true);
+ Printer::updateDerivedParameter();
+ Printer::updateCurrentPosition(true);
+ printCurrentPosition(PSTR("G32 "));
+#if DRIVE_SYSTEM == DELTA
+ Printer::homeAxis(true, true, true);
+#endif
+ Printer::feedrate = oldFeedrate;
+ }
+ break;
+#endif
+ case 68: // probes center X0 Y0 then probes x/y 3rd position, commonly the z tower on deltas
+ {
+#if DISTORTION_CORRECTION
+ float oldFeedrate = Printer::feedrate;
+ Printer::measureDistortion();
+ Printer::feedrate = oldFeedrate;
+#else
+ //bool oldAutolevel = Printer::isAutolevelActive();
+ float sum = 0, sum1 = 0, last, hradius,oldFeedrate = Printer::feedrate;
+ int32_t probeSensitivity = Z_PROBE_SENSITIVITY;
+ float defaultRadius = PRINTER_RADIUS-END_EFFECTOR_HORIZONTAL_OFFSET-CARRIAGE_HORIZONTAL_OFFSET;
+ float oldRadius;
+ do{
+ //Printer::radius0 = PRINTER_RADIUS-END_EFFECTOR_HORIZONTAL_OFFSET-CARRIAGE_HORIZONTAL_OFFSET; // set horizontal radius to firmware default
+ oldRadius = Printer::radius0;
+ EEPROM::storeDataIntoEEPROM(); //save firmware horizontal radius before calibration
+ EEPROM::readDataFromEEPROM();
+ Printer::homeAxis(true,true,true);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ Printer::moveTo(0,0,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+ sum1 = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ sum = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false);
+ if ((sum1 - sum) > Z_PROBE_TOLERANCE || (sum1 - sum) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }
+ sum = -1;
+ continue;
+ }else{
+ sum = (sum + sum1) / 2;
+ }
+
+ Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+
+ sum1 = Printer::runZProbe(false,true); // First tap
+ last = Printer::runZProbe(false,true); // Second tap
+ if ((sum1 - last) > Z_PROBE_TOLERANCE || (sum1 - last) < - Z_PROBE_TOLERANCE){
+ Com::printFLN(PSTR("Z probe failed on sensitivity: "), probeSensitivity );
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ sum = -1;
+ continue;
+ }else{
+ last = (last + sum1) / 2;
+ }
+ } while (sum < 0);
+
+ sum = (sum - last)*AXIS_STEPS_PER_MM;
+ if(sum<0)
+ {
+ sum=-sum;
+ Com::printFLN(Com::tZProbeAverage,sum);
+ hradius = (sum / Printer::radius0)*2;
+ Com::printFLN(Com::tZProbeAverage,hradius);
+ Printer::radius0 = Printer::radius0 - hradius;
+ }
+ else
+ {
+ hradius = (sum / Printer::radius0)*2;
+ Printer::radius0 = Printer::radius0 + hradius;
+ }
+ if(Printer::radius0 / defaultRadius > 1.1 || Printer::radius0 / defaultRadius < 0.9){
+ Printer::radius0 = defaultRadius;
+ Com::printFLN(PSTR("Calculated Radius is bad :"), Printer::radius0 );
+ }else{
+ Com::printFLN(PSTR("Old Radius: "), oldRadius );
+ Com::printFLN(PSTR("New Radius: "), Printer::radius0 );
+ }
+ Printer::feedrate = oldFeedrate;
+ //Printer::setAutolevelActive(oldAutolevel);
+ Printer::setAutolevelActive(true);
+ Printer::updateCurrentPosition(true);
+ printCurrentPosition(PSTR("hehe"));
+ GCode::executeFString(Com::tZProbeEndScript);
+ Printer::feedrate = oldFeedrate;
+ Printer::homeAxis(true,true,true);
+ EEPROM::storeDataIntoEEPROM();
+#endif // DISTORTION_CORRECTION
+ }
+ break;
+ case 69: // G69 3 points, probe endstop offsets
+ {
+#if DISTORTION_CORRECTION
+ float oldFeedrate = Printer::feedrate;
+ Printer::measureDistortion();
+ Printer::feedrate = oldFeedrate;
+#else
+ //bool oldAutolevel = Printer::isAutolevelActive();
+ float sum1 = 0, sum = 0, last,oldFeedrate = Printer::feedrate;
+ int32_t probeSensitivity = Z_PROBE_SENSITIVITY;
+ do{
+ if(com->hasS() && com->S == 2){ // only wipe values if saving the new values
+ EEPROM::setDeltaTowerXOffsetSteps(0); // set X offset to 0
+ EEPROM::setDeltaTowerYOffsetSteps(0); // set Y offset to 0
+ EEPROM::setDeltaTowerZOffsetSteps(0); // set Z offset to 0
+ EEPROM::storeDataIntoEEPROM(); // store offsets to 0 before doing anything
+ EEPROM::readDataFromEEPROM();
+ }
+ Printer::homeAxis(true,true,true);
+ GCode::executeFString(Com::tZProbeStartScript);
+ Printer::setAutolevelActive(false);
+ Printer::moveTo(EEPROM::zProbeX1(),EEPROM::zProbeY1(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+
+ sum1 = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //First tap
+ sum = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); //Second tap
+ if ((sum1 - sum) > Z_PROBE_TOLERANCE || (sum1 - sum) < - Z_PROBE_TOLERANCE){ //tap reports distance, if more or less than .1mm, it will re-run
+ Com::printFLN(PSTR("Z probe failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ if ( com->hasS() )
+ {
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }
+ }
+ sum = -1;
+ continue;
+ }else{
+ sum = (sum + sum1) / 2;
+ }
+
+ int32_t offsetX = ((sum * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsX = EEPROM::deltaTowerXOffsetSteps();
+ Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+
+ sum1 = Printer::runZProbe(false,false); //First tap Y tower
+ last = Printer::runZProbe(false,false); //Second tap Y tower
+ if ((sum1 - last) > Z_PROBE_TOLERANCE || (sum1 - last) < - Z_PROBE_TOLERANCE){
+ Com::printFLN(PSTR("Z probe failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }
+ last = -1; // fail flag to stop probe
+ continue;
+ }else{
+ last = (last + sum1) / 2;
+ }
+
+ int32_t offsetY = ((last * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsY = EEPROM::deltaTowerYOffsetSteps();
+ Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed());
+
+ sum1 = Printer::runZProbe(false,true); //First tap Z tower
+ last = Printer::runZProbe(false,true); //Second tap Z tower
+ if((sum1 - last) > Z_PROBE_TOLERANCE || (sum1 - last) < - Z_PROBE_TOLERANCE){
+ Com::printFLN(PSTR("Z probe failed on sensitivity: "), probeSensitivity );
+ if(probeSensitivity < Z_PROBE_MAX_SENSITIVITY){
+ accelerometer_recv(0x32);
+ probeSensitivity+=2;
+ Com::printFLN(PSTR("Setting Probe Sensitivity To:"), probeSensitivity );
+ accelerometer_write(0x32,uint8_t(probeSensitivity)); //INT1 THRESHOLD
+ accelerometer_write(0x3A,uint8_t(probeSensitivity)); //CLICK THRESHOLD
+ accelerometer_recv(0x32);
+ }
+ last = -1; // fail flag to stop probe
+ continue;
+ }else{
+ last = (last + sum1) / 2;
+ }
+
+ int32_t offsetZ = ((last * AXIS_STEPS_PER_MM) - (Z_PROBE_BED_DISTANCE * AXIS_STEPS_PER_MM)), offsetStepsZ = EEPROM::deltaTowerZOffsetSteps();
+
+ if(com->hasS() && com->S)
+ {
+ Printer::updateCurrentPosition();
+ Printer::updateDerivedParameter();
+ //Printer::homeAxis(true,true,true); added to end of main g69
+ Com::printInfoFLN(Com::tZProbeZReset);
+ Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength);
+ }
+ Printer::feedrate = oldFeedrate;
+ //Printer::setAutolevelActive(oldAutolevel);
+ Printer::setAutolevelActive(true);
+
+ if(com->hasS() && com->S == 2)
+ {
+ if(offsetX < offsetY && offsetX < offsetZ)
+ {
+ offsetY = offsetStepsY + (offsetY - offsetX);
+ offsetZ = offsetStepsZ + (offsetZ - offsetX);
+ offsetX = 0;
+ EEPROM::setDeltaTowerYOffsetSteps(offsetY);
+ EEPROM::setDeltaTowerZOffsetSteps(offsetZ);
+ }
+ else if(offsetY < offsetX && offsetY < offsetZ)
+ {
+ offsetX = offsetStepsX + (offsetX - offsetY);
+ offsetZ = offsetStepsZ + (offsetZ - offsetY);
+ EEPROM::setDeltaTowerXOffsetSteps(offsetX);
+ EEPROM::setDeltaTowerZOffsetSteps(offsetZ);
+ offsetY = 0;
+ }
+ else if(offsetZ < offsetX && offsetZ < offsetY)
+ {
+ offsetX = offsetStepsX + (offsetX - offsetZ);
+ offsetY = offsetStepsY + (offsetY - offsetZ);
+ EEPROM::setDeltaTowerXOffsetSteps(offsetX);
+ EEPROM::setDeltaTowerYOffsetSteps(offsetY);
+ offsetZ = 0;
+ }
+ if(offsetX > 400 || offsetY > 400 || offsetZ > 400){
+ sum = -1;
+ Com::printFLN(PSTR("OFFSETS OFF BY TOO MUCH _ TRYING AGAIN: "), probeSensitivity);
+ Com::printFLN(PSTR("X: "), offsetX);
+ Com::printFLN(PSTR("Y: "), offsetY);
+ Com::printFLN(PSTR("Z: "), offsetZ);
+ }else{
+ EEPROM::storeDataIntoEEPROM();
+ }
+ }
+ }while(sum < 0 || last < 0);
+ Printer::updateCurrentPosition(true);
+ printCurrentPosition(PSTR("G69 "));
+ GCode::executeFString(Com::tZProbeEndScript);
+ Printer::feedrate = oldFeedrate;
+ Printer::homeAxis(true,true,true);
+#endif // DISTORTION_CORRECTION
+ }
+ break;
+#endif
+ case 90: // G90
+ Printer::relativeCoordinateMode = false;
+ if(com->internalCommand)
+ Com::printInfoFLN(PSTR("Absolute positioning"));
+ break;
+ case 91: // G91
+ Printer::relativeCoordinateMode = true;
+ if(com->internalCommand)
+ Com::printInfoFLN(PSTR("Relative positioning"));
+ break;
+ case 92: // G92
+ {
+ float xOff = Printer::coordinateOffset[X_AXIS];
+ float yOff = Printer::coordinateOffset[Y_AXIS];
+ float zOff = Printer::coordinateOffset[Z_AXIS];
+ if(com->hasX()) xOff = Printer::convertToMM(com->X) - Printer::currentPosition[X_AXIS];
+ if(com->hasY()) yOff = Printer::convertToMM(com->Y) - Printer::currentPosition[Y_AXIS];
+ if(com->hasZ()) zOff = Printer::convertToMM(com->Z) - Printer::currentPosition[Z_AXIS];
+ Printer::setOrigin(xOff, yOff, zOff);
+ if(com->hasE())
+ {
+ Printer::currentPositionSteps[E_AXIS] = Printer::convertToMM(com->E) * Printer::axisStepsPerMM[E_AXIS];
+ }
+ }
+ break;
+#if DRIVE_SYSTEM == DELTA
+ case 100: // G100 Calibrate floor or rod radius
+ {
+ // Using manual control, adjust hot end to contact floor.
+ // G100 No action. Avoid accidental floor reset.
+ // G100 [X] [Y] [Z] set floor for argument passed in. Number ignored and may be absent.
+ // G100 R with X Y or Z flag error, sets only floor or radius, not both.
+ // G100 R[n] Add n to radius. Adjust to be above floor if necessary
+ // G100 R[0] set radius based on current z measurement. Moves to (0,0,0)
+ float currentZmm = Printer::currentPosition[Z_AXIS];
+ if (currentZmm/Printer::zLength > 0.1)
+ {
+ Com::printErrorFLN(PSTR("Calibration code is limited to bottom 10% of Z height"));
+ break;
+ }
+ if (com->hasR())
+ {
+ if (com->hasX() || com->hasY() || com->hasZ())
+ Com::printErrorFLN(PSTR("Cannot set radius and floor at same time."));
+ else if (com->R != 0)
+ {
+ //add r to radius
+ if (abs(com->R) <= 10) EEPROM::incrementRodRadius(com->R);
+ else Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm."));
+ }
+ else
+ {
+ // auto set radius. Head must be at 0,0 and touching
+ // Z offset will be corrected for.
+ if (Printer::currentPosition[X_AXIS] == 0
+ && Printer::currentPosition[Y_AXIS] == 0)
+ {
+ if(Printer::isLargeMachine())
+ {
+ // calculate radius assuming we are at surface
+ // If Z is greater than 0 it will get calculated out for correct radius
+ // Use either A or B tower as they acnhor x cartesian axis and always have
+ // Radius distance to center in simplest set up.
+ float h = Printer::deltaDiagonalStepsSquaredB.f;
+ unsigned long bSteps = Printer::currentDeltaPositionSteps[B_TOWER];
+ // The correct Rod Radius would put us here at z==0 and B height is
+ // square root (rod length squared minus rod radius squared)
+ // Reverse that to get calculated Rod Radius given B height
+ h -= RMath::sqr((float)bSteps);
+ h = sqrt(h);
+ EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]);
+ }
+ else
+ {
+ // calculate radius assuming we are at surface
+ // If Z is greater than 0 it will get calculated out for correct radius
+ // Use either A or B tower as they acnhor x cartesian axis and always have
+ // Radius distance to center in simplest set up.
+ unsigned long h = Printer::deltaDiagonalStepsSquaredB.l;
+ unsigned long bSteps = Printer::currentDeltaPositionSteps[B_TOWER];
+ // The correct Rod Radius would put us here at z==0 and B height is
+ // square root (rod length squared minus rod radius squared)
+ // Reverse that to get calculated Rod Radius given B height
+ h -= RMath::sqr(bSteps);
+ h = SQRT(h);
+ EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]);
+ }
+ }
+ else
+ Com::printErrorFLN(PSTR("First move to touch at x,y=0,0 to auto-set radius."));
+ }
+ }
+ else
+ {
+ bool tooBig = false;
+ if (com->hasX())
+ {
+ if (abs(com->X) <= 10)
+ EEPROM::setTowerXFloor(com->X + currentZmm + Printer::xMin);
+ else tooBig = true;
+ }
+ if (com->hasY())
+ {
+ if (abs(com->Y) <= 10)
+ EEPROM::setTowerYFloor(com->Y + currentZmm + Printer::yMin);
+ else tooBig = true;
+ }
+ if (com->hasZ())
+ {
+ if (abs(com->Z) <= 10)
+ EEPROM::setTowerZFloor(com->Z + currentZmm + Printer::zMin);
+ else tooBig = true;
+ }
+ if (tooBig)
+ Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm."));
+ }
+ // after adjusting zero, physical position is out of sync with memory position
+ // this could cause jerky movement or push head into print surface.
+ // moving gets back into safe zero'ed position with respect to newle set floor or Radius.
+ Printer::moveTo(IGNORE_COORDINATE,IGNORE_COORDINATE,12.0,IGNORE_COORDINATE,IGNORE_COORDINATE);
+ break;
+ }
+ case 131: // G131 Remove offset
+ {
+ float cx,cy,cz;
+ Printer::realPosition(cx,cy,cz);
+ float oldfeedrate = Printer::feedrate;
+ Printer::offsetX = 0;
+ Printer::offsetY = 0;
+ Printer::moveToReal(cx,cy,cz,IGNORE_COORDINATE,Printer::homingFeedrate[X_AXIS]);
+ Printer::feedrate = oldfeedrate;
+ Printer::updateCurrentPosition();
+ }
+ break;
+ case 132: // G132 Calibrate endstop offsets
+ {
+// This has the probably unintended side effect of turning off leveling.
+ Printer::setAutolevelActive(false); // don't let transformations change result!
+ Printer::coordinateOffset[X_AXIS] = 0;
+ Printer::coordinateOffset[Y_AXIS] = 0;
+ Printer::coordinateOffset[Z_AXIS] = 0;
+// I think this is coded incorrectly, as it depends on the biginning position of the
+// of the hot end, and so should first move to x,y,z= 0,0,0, but as that may not
+// be possible if the printer is not in the homes/zeroed state, the printer
+// cannot safely move to 0 z coordinate without crashong into the print surface.
+// so other than commenting, I'm not meddling.
+// but you will always get different counts from different positions.
+ Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]);
+ int32_t m = RMath::max(Printer::stepsRemainingAtXHit,RMath::max(Printer::stepsRemainingAtYHit,Printer::stepsRemainingAtZHit));
+ int32_t offx = m-Printer::stepsRemainingAtXHit;
+ int32_t offy = m-Printer::stepsRemainingAtYHit;
+ int32_t offz = m-Printer::stepsRemainingAtZHit;
+ Com::printFLN(Com::tTower1,offx);
+ Com::printFLN(Com::tTower2,offy);
+ Com::printFLN(Com::tTower3,offz);
+#if EEPROM_MODE != 0
+ if(com->hasS() && com->S > 0)
+ {
+ EEPROM::setDeltaTowerXOffsetSteps(offx);
+ EEPROM::setDeltaTowerYOffsetSteps(offy);
+ EEPROM::setDeltaTowerZOffsetSteps(offz);
+ }
+#endif
+ Printer::homeAxis(true,true,true);
+ }
+ break;
+ case 133: // G133 Measure steps to top
+ {
+ bool oldAuto = Printer::isAutolevelActive();
+ Printer::setAutolevelActive(false); // don't let transformations change result!
+ Printer::currentPositionSteps[X_AXIS] = 0;
+ Printer::currentPositionSteps[Y_AXIS] = 0;
+ Printer::currentPositionSteps[Z_AXIS] = 0;
+ Printer::coordinateOffset[X_AXIS] = 0;
+ Printer::coordinateOffset[Y_AXIS] = 0;
+ Printer::coordinateOffset[Z_AXIS] = 0;
+ Printer::currentDeltaPositionSteps[A_TOWER] = 0;
+ Printer::currentDeltaPositionSteps[B_TOWER] = 0;
+ Printer::currentDeltaPositionSteps[C_TOWER] = 0;
+// similar to comment above, this will get a different answer from any different starting point
+// so it is unclear how this is helpful. It must start at a well defined point.
+ Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]);
+ int32_t offx = HOME_DISTANCE_STEPS-Printer::stepsRemainingAtXHit;
+ int32_t offy = HOME_DISTANCE_STEPS-Printer::stepsRemainingAtYHit;
+ int32_t offz = HOME_DISTANCE_STEPS-Printer::stepsRemainingAtZHit;
+ Com::printFLN(Com::tTower1,offx);
+ Com::printFLN(Com::tTower2,offy);
+ Com::printFLN(Com::tTower3,offz);
+ Printer::setAutolevelActive(oldAuto);
+ Printer::homeAxis(true,true,true);
+ }
+ break;
+ case 134: // G134
+ Com::printF(PSTR("CompDelta:"),Printer::currentDeltaPositionSteps[A_TOWER]);
+ Com::printF(Com::tComma,Printer::currentDeltaPositionSteps[B_TOWER]);
+ Com::printFLN(Com::tComma,Printer::currentDeltaPositionSteps[C_TOWER]);
+#ifdef DEBUG_REAL_POSITION
+ Com::printF(PSTR("RealDelta:"),Printer::realDeltaPositionSteps[A_TOWER]);
+ Com::printF(Com::tComma,Printer::realDeltaPositionSteps[B_TOWER]);
+ Com::printFLN(Com::tComma,Printer::realDeltaPositionSteps[C_TOWER]);
+#endif
+ Printer::updateCurrentPosition();
+ Com::printF(PSTR("PosFromSteps:"));
+ printCurrentPosition(PSTR("G134 "));
+ break;
+
+#endif // DRIVE_SYSTEM
+ default:
+ if(Printer::debugErrors())
+ {
+ Com::printF(Com::tUnknownCommand);
+ com->printCommand();
+ }
+ }
+ previousMillisCmd = HAL::timeInMilliseconds();
+}
+/**
+ \brief Execute the G command stored in com.
+*/
+void Commands::processMCode(GCode *com)
+{
+ uint32_t codenum; //throw away variable
+ switch( com->M )
+ {
+ case 17: //M17 Enable all Steppers
+ Printer::enableXStepper();
+ Printer::enableYStepper();
+ Printer::enableZStepper();
+ break;
+ case 18: //M18 Disable all Steppers
+ Printer::disableXStepper();
+ Printer::disableYStepper();
+ Printer::disableZStepper();
+ break;
+#if SDSUPPORT
+
+ case 20: // M20 - list SD card
+ sd.ls();
+ break;
+ case 21: // M21 - init SD card
+ sd.mount();
+ break;
+ case 22: //M22 - release SD card
+ sd.unmount();
+ break;
+ case 23: //M23 - Select file
+ if(com->hasString())
+ {
+ sd.fat.chdir();
+ sd.selectFile(com->text);
+ }
+ break;
+ case 24: //M24 - Start SD print
+ sd.startPrint();
+ break;
+ case 25: //M25 - Pause SD print
+ sd.pausePrint();
+ break;
+ case 26: //M26 - Set SD index
+ if(com->hasS())
+ sd.setIndex(com->S);
+ break;
+ case 27: //M27 - Get SD status
+ sd.printStatus();
+ break;
+ case 28: //M28 - Start SD write
+ if(com->hasString())
+ sd.startWrite(com->text);
+ break;
+ case 29: //M29 - Stop SD write
+ //processed in write to file routine above
+ //savetosd = false;
+ break;
+ case 30: // M30 filename - Delete file
+ if(com->hasString())
+ {
+ sd.fat.chdir();
+ sd.deleteFile(com->text);
+ }
+ break;
+ case 32: // M32 directoryname
+ if(com->hasString())
+ {
+ sd.fat.chdir();
+ sd.makeDirectory(com->text);
+ }
+ break;
+#endif
+ case 42: //M42 -Change pin status via gcode
+ if (com->hasP())
+ {
+ int pin_number = com->P;
+ for(uint8_t i = 0; i < (uint8_t)sizeof(sensitive_pins); i++)
+ {
+ if (pgm_read_byte(&sensitive_pins[i]) == pin_number)
+ {
+ pin_number = -1;
+ break;
+ }
+ }
+ if (pin_number > -1)
+ {
+ if(com->hasS())
+ {
+ if(com->S >= 0 && com->S <= 255)
+ {
+ pinMode(pin_number, OUTPUT);
+ digitalWrite(pin_number, com->S);
+ analogWrite(pin_number, com->S);
+ Com::printF(Com::tSetOutputSpace, pin_number);
+ Com::printFLN(Com::tSpaceToSpace,(int)com->S);
+ }
+ else
+ Com::printErrorFLN(PSTR("Illegal S value for M42"));
+ }
+ else
+ {
+ pinMode(pin_number, INPUT_PULLUP);
+ Com::printF(Com::tSpaceToSpace, pin_number);
+ Com::printFLN(Com::tSpaceIsSpace, digitalRead(pin_number));
+ }
+ }
+ else
+ {
+ Com::printErrorFLN(PSTR("Pin can not be set by M42, may in invalid or in use. "));
+ }
+ }
+ break;
+
+ case 80: // M80 - ATX Power On
+#if PS_ON_PIN>-1
+ Commands::waitUntilEndOfAllMoves();
+ previousMillisCmd = HAL::timeInMilliseconds();
+ SET_OUTPUT(PS_ON_PIN); //GND
+ Printer::setPowerOn(true);
+ WRITE(PS_ON_PIN, (POWER_INVERTING ? HIGH : LOW));
+#endif
+ break;
+ case 81: // M81 - ATX Power Off
+#if PS_ON_PIN>-1
+ Commands::waitUntilEndOfAllMoves();
+ SET_OUTPUT(PS_ON_PIN); //GND
+ Printer::setPowerOn(false);
+ WRITE(PS_ON_PIN,(POWER_INVERTING ? LOW : HIGH));
+#endif
+ break;
+ case 82: // M82
+ Printer::relativeExtruderCoordinateMode = false;
+ break;
+ case 83: // M83
+ Printer::relativeExtruderCoordinateMode = true;
+ break;
+ case 84: // M84
+ if(com->hasS())
+ {
+ stepperInactiveTime = com->S * 1000;
+ }
+ else
+ {
+ if(com->hasP()){
+ switch(com->P){
+ case 0:
+ Printer::disableXStepper();
+ break;
+
+ case 1:
+ Printer::disableYStepper();
+ break;
+
+ case 2:
+ Printer::disableZStepper();
+ break;
+
+ case 3:
+ Extruder::disableCurrentExtruderMotor();
+ break;
+
+ case 4:
+ Extruder::disableAllExtruderMotors();
+ break;
+ }
+ }else{
+ Commands::waitUntilEndOfAllMoves();
+ Printer::kill(true);
+ }
+ }
+ break;
+ case 85: // M85
+ if(com->hasS())
+ maxInactiveTime = (int32_t)com->S * 1000;
+ else
+ maxInactiveTime = 0;
+ break;
+ case 92: // M92
+ if(com->hasX()) Printer::axisStepsPerMM[X_AXIS] = com->X;
+ if(com->hasY()) Printer::axisStepsPerMM[Y_AXIS] = com->Y;
+ if(com->hasZ()) Printer::axisStepsPerMM[Z_AXIS] = com->Z;
+ Printer::updateDerivedParameter();
+ if(com->hasE())
+ {
+ Extruder::current->stepsPerMM = com->E;
+ Extruder::selectExtruderById(Extruder::current->id);
+ }
+ break;
+ case 99: // M99 S