Merge pull request #1 from MarlinFirmware/Development

dev pull
This commit is contained in:
Wurstnase 2015-03-05 17:51:33 +01:00
commit 92ebb7f4c8
58 changed files with 6546 additions and 5638 deletions

View file

@ -33,7 +33,7 @@ rambo.build.variant=rambo
######################################## ########################################
sanguino.name=Sanguino sanguino.name=Sanguino
sanguino.upload.tool=ardunio:avrdude sanguino.upload.tool=arduino:avrdude
sanguino.upload.protocol=stk500 sanguino.upload.protocol=stk500
sanguino.upload.maximum_size=131072 sanguino.upload.maximum_size=131072
sanguino.upload.speed=57600 sanguino.upload.speed=57600

View file

@ -5,16 +5,9 @@
#include "Marlin.h" #include "Marlin.h"
#ifdef BLINKM #ifdef BLINKM
#if (ARDUINO >= 100)
# include "Arduino.h"
#else
# include "WProgram.h"
#endif
#include "BlinkM.h" #include "BlinkM.h"
void SendColors(byte red, byte grn, byte blu) void SendColors(byte red, byte grn, byte blu) {
{
Wire.begin(); Wire.begin();
Wire.beginTransmission(0x09); Wire.beginTransmission(0x09);
Wire.write('o'); //to disable ongoing script, only needs to be used once Wire.write('o'); //to disable ongoing script, only needs to be used once

View file

@ -2,13 +2,12 @@
BlinkM.h BlinkM.h
Library header file for BlinkM library Library header file for BlinkM library
*/ */
#if (ARDUINO >= 100) #if ARDUINO >= 100
# include "Arduino.h" #include "Arduino.h"
#else #else
# include "WProgram.h" #include "WProgram.h"
#endif #endif
#include "Wire.h" #include "Wire.h"
void SendColors(byte red, byte grn, byte blu); void SendColors(byte red, byte grn, byte blu);

View file

@ -118,10 +118,15 @@ Here are some standard links for getting your machine calibrated:
// 1010 is Pt1000 with 1k pullup (non standard) // 1010 is Pt1000 with 1k pullup (non standard)
// 147 is Pt100 with 4k7 pullup // 147 is Pt100 with 4k7 pullup
// 110 is Pt100 with 1k pullup (non standard) // 110 is Pt100 with 1k pullup (non standard)
// 998 and 999 are Dummy Tables. They will ALWAYS read 25°C or the temperature defined below.
// Use it for Testing or Development purposes. NEVER for production machine.
// #define DUMMY_THERMISTOR_998_VALUE 25
// #define DUMMY_THERMISTOR_999_VALUE 100
#define TEMP_SENSOR_0 -1 #define TEMP_SENSOR_0 -1
#define TEMP_SENSOR_1 -1 #define TEMP_SENSOR_1 -1
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 0 #define TEMP_SENSOR_BED 0
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -139,6 +144,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -147,6 +153,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -323,11 +330,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define DISABLE_MAX_ENDSTOPS //#define DISABLE_MAX_ENDSTOPS
//#define DISABLE_MIN_ENDSTOPS //#define DISABLE_MIN_ENDSTOPS
// Disable max endstops for compatibility with endstop checking routine
#if defined(COREXY) && !defined(DISABLE_MAX_ENDSTOPS)
#define DISABLE_MAX_ENDSTOPS
#endif
// For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1 // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1
#define X_ENABLE_ON 0 #define X_ENABLE_ON 0
#define Y_ENABLE_ON 0 #define Y_ENABLE_ON 0
@ -341,12 +343,13 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DISABLE_E false // For all extruders #define DISABLE_E false // For all extruders
#define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled #define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled
#define INVERT_X_DIR true // for Mendel set to false, for Orca set to true #define INVERT_X_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -425,9 +428,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
// these are the offsets to the probe relative to the extruder tip (Hotend - Probe) // these are the offsets to the probe relative to the extruder tip (Hotend - Probe)
// X and Y offsets must be integers // X and Y offsets must be integers
#define X_PROBE_OFFSET_FROM_EXTRUDER -25 #define X_PROBE_OFFSET_FROM_EXTRUDER -25 // -left +right
#define Y_PROBE_OFFSET_FROM_EXTRUDER -29 #define Y_PROBE_OFFSET_FROM_EXTRUDER -29 // -front +behind
#define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35 #define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35 // -below (always!)
#define Z_RAISE_BEFORE_HOMING 4 // (in mm) Raise Z before homing (G28) for Probe Clearance. #define Z_RAISE_BEFORE_HOMING 4 // (in mm) Raise Z before homing (G28) for Probe Clearance.
// Be sure you have this distance over your Z_MAX_POS in case // Be sure you have this distance over your Z_MAX_POS in case
@ -582,10 +585,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -619,6 +632,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -1,5 +1,5 @@
#ifndef CONFIG_STORE_H #ifndef CONFIGURATIONSTORE_H
#define CONFIG_STORE_H #define CONFIGURATIONSTORE_H
#include "Configuration.h" #include "Configuration.h"
@ -19,4 +19,4 @@ void Config_ResetDefault();
FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); } FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
#endif #endif
#endif // __CONFIG_STORE_H #endif //CONFIGURATIONSTORE_H

View file

@ -284,6 +284,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again

View file

@ -14,10 +14,14 @@
// it is a Russian alphabet translation // it is a Russian alphabet translation
// except 0401 --> 0xa2 = ╗, 0451 --> 0xb5 // except 0401 --> 0xa2 = ╗, 0451 --> 0xb5
const PROGMEM uint8_t utf_recode[] = const PROGMEM uint8_t utf_recode[] =
{ 0x41,0xa0,0x42,0xa1,0xe0,0x45,0xa3,0xa4,0xa5,0xa6,0x4b,0xa7,0x4d,0x48,0x4f, { 0x41,0xa0,0x42,0xa1,0xe0,0x45,0xa3,0xa4,
0xa8,0x50,0x43,0x54,0xa9,0xaa,0x58,0xe1,0xab,0xac,0xe2,0xad,0xae,0x62,0xaf,0xb0,0xb1, 0xa5,0xa6,0x4b,0xa7,0x4d,0x48,0x4f,0xa8,
0x61,0xb2,0xb3,0xb4,0xe3,0x65,0xb6,0xb7,0xb8,0xb9,0xba,0xbb,0xbc,0xbd,0x6f, 0x50,0x43,0x54,0xa9,0xaa,0x58,0xe1,0xab,
0xbe,0x70,0x63,0xbf,0x79,0xe4,0x78,0xe5,0xc0,0xc1,0xe6,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7 0xac,0xe2,0xad,0xae,0x62,0xaf,0xb0,0xb1,
0x61,0xb2,0xb3,0xb4,0xe3,0x65,0xb6,0xb7,
0xb8,0xb9,0xba,0xbb,0xbc,0xbd,0x6f,0xbe,
0x70,0x63,0xbf,0x79,0xe4,0x78,0xe5,0xc0,
0xc1,0xe6,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7
}; };
// When the display powers up, it is configured as follows: // When the display powers up, it is configured as follows:

View file

@ -180,8 +180,8 @@ void manage_inactivity(bool ignore_stepper_queue=false);
#define disable_e3() /* nothing */ #define disable_e3() /* nothing */
#endif #endif
enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5}; enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
//X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
void FlushSerialRequestResend(); void FlushSerialRequestResend();
void ClearToSend(); void ClearToSend();
@ -201,8 +201,9 @@ void Stop();
bool IsStopped(); bool IsStopped();
void enquecommand(const char *cmd); //put an ASCII command at the end of the current buffer. bool enquecommand(const char *cmd); //put a single ASCII command at the end of the current buffer or return false when it is full
void enquecommand_P(const char *cmd); //put an ASCII command at the end of the current buffer, read from flash void enquecommands_P(const char *cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
void prepare_arc_move(char isclockwise); void prepare_arc_move(char isclockwise);
void clamp_to_software_endstops(float target[3]); void clamp_to_software_endstops(float target[3]);

View file

@ -30,7 +30,10 @@
#include "Marlin.h" #include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h" #if Z_MIN_PIN == -1
#error "You must have a Z_MIN endstop to enable Auto Bed Leveling feature. Z_MIN_PIN must point to a valid hardware pin."
#endif
#include "vector_3.h"
#ifdef AUTO_BED_LEVELING_GRID #ifdef AUTO_BED_LEVELING_GRID
#include "qr_solve.h" #include "qr_solve.h"
#endif #endif
@ -124,6 +127,8 @@
// M115 - Capabilities string // M115 - Capabilities string
// M117 - display message // M117 - display message
// M119 - Output Endstop status to serial port // M119 - Output Endstop status to serial port
// M120 - Enable endstop detection
// M121 - Disable endstop detection
// M126 - Solenoid Air Valve Open (BariCUDA support by jmil) // M126 - Solenoid Air Valve Open (BariCUDA support by jmil)
// M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil) // M127 - Solenoid Air Valve Closed (BariCUDA vent to atmospheric pressure by jmil)
// M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil) // M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
@ -154,6 +159,8 @@
// M302 - Allow cold extrudes, or set the minimum extrude S<temperature>. // M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C) // M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
// M304 - Set bed PID parameters P I and D // M304 - Set bed PID parameters P I and D
// M380 - Activate solenoid on active extruder
// M381 - Disable all solenoids
// M400 - Finish all moves // M400 - Finish all moves
// M401 - Lower z-probe if present // M401 - Lower z-probe if present
// M402 - Raise z-probe if present // M402 - Raise z-probe if present
@ -201,9 +208,9 @@ int extruder_multiply[EXTRUDERS] = { 100
, 100 , 100
#if EXTRUDERS > 2 #if EXTRUDERS > 2
, 100 , 100
#if EXTRUDERS > 3 #if EXTRUDERS > 3
, 100 , 100
#endif #endif
#endif #endif
#endif #endif
}; };
@ -285,8 +292,8 @@ int fanSpeed = 0;
#if EXTRUDERS > 2 #if EXTRUDERS > 2
, false , false
#if EXTRUDERS > 3 #if EXTRUDERS > 3
, false , false
#endif #endif
#endif #endif
#endif #endif
}; };
@ -296,8 +303,8 @@ int fanSpeed = 0;
#if EXTRUDERS > 2 #if EXTRUDERS > 2
, false , false
#if EXTRUDERS > 3 #if EXTRUDERS > 3
, false , false
#endif #endif
#endif #endif
#endif #endif
}; };
@ -317,7 +324,7 @@ int fanSpeed = 0;
#ifdef PS_DEFAULT_OFF #ifdef PS_DEFAULT_OFF
false false
#else #else
true true
#endif #endif
; ;
#endif #endif
@ -329,9 +336,9 @@ int fanSpeed = 0;
// these are the default values, can be overriden with M665 // these are the default values, can be overriden with M665
float delta_radius = DELTA_RADIUS; float delta_radius = DELTA_RADIUS;
float delta_tower1_x = -SIN_60 * delta_radius; // front left tower float delta_tower1_x = -SIN_60 * delta_radius; // front left tower
float delta_tower1_y = -COS_60 * delta_radius; float delta_tower1_y = -COS_60 * delta_radius;
float delta_tower2_x = SIN_60 * delta_radius; // front right tower float delta_tower2_x = SIN_60 * delta_radius; // front right tower
float delta_tower2_y = -COS_60 * delta_radius; float delta_tower2_y = -COS_60 * delta_radius;
float delta_tower3_x = 0; // back middle tower float delta_tower3_x = 0; // back middle tower
float delta_tower3_y = delta_radius; float delta_tower3_y = delta_radius;
float delta_diagonal_rod = DELTA_DIAGONAL_ROD; float delta_diagonal_rod = DELTA_DIAGONAL_ROD;
@ -341,7 +348,7 @@ int fanSpeed = 0;
#ifdef SCARA #ifdef SCARA
float axis_scaling[3] = { 1, 1, 1 }; // Build size scaling, default to 1 float axis_scaling[3] = { 1, 1, 1 }; // Build size scaling, default to 1
#endif #endif
bool cancel_heatup = false; bool cancel_heatup = false;
@ -385,6 +392,8 @@ static int serial_count = 0;
static boolean comment_mode = false; static boolean comment_mode = false;
static char *strchr_pointer; ///< A pointer to find chars in the command string (X, Y, Z, E, etc.) static char *strchr_pointer; ///< A pointer to find chars in the command string (X, Y, Z, E, etc.)
const char* queued_commands_P= NULL; /* pointer to the current line in the active sequence of commands, or NULL when none */
const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42 const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42
// Inactivity shutdown // Inactivity shutdown
@ -448,39 +457,64 @@ void serial_echopair_P(const char *s_P, unsigned long v)
} }
#endif //!SDSUPPORT #endif //!SDSUPPORT
//adds an command to the main command buffer //Injects the next command from the pending sequence of commands, when possible
//thats really done in a non-safe way. //Return false if and only if no command was pending
//needs overworking someday static bool drain_queued_commands_P()
void enquecommand(const char *cmd)
{ {
if(buflen < BUFSIZE) char cmd[30];
if(!queued_commands_P)
return false;
// Get the next 30 chars from the sequence of gcodes to run
strncpy_P(cmd, queued_commands_P, sizeof(cmd)-1);
cmd[sizeof(cmd)-1]= 0;
// Look for the end of line, or the end of sequence
size_t i= 0;
char c;
while( (c= cmd[i]) && c!='\n' )
++i; // look for the end of this gcode command
cmd[i]= 0;
if(enquecommand(cmd)) // buffer was not full (else we will retry later)
{ {
//this is dangerous if a mixing of serial and this happens if(c)
strcpy(&(cmdbuffer[bufindw][0]),cmd); queued_commands_P+= i+1; // move to next command
SERIAL_ECHO_START; else
SERIAL_ECHOPGM(MSG_Enqueing); queued_commands_P= NULL; // will have no more commands in the sequence
SERIAL_ECHO(cmdbuffer[bufindw]);
SERIAL_ECHOLNPGM("\"");
bufindw= (bufindw + 1)%BUFSIZE;
buflen += 1;
} }
return true;
} }
void enquecommand_P(const char *cmd) //Record one or many commands to run from program memory.
//Aborts the current queue, if any.
//Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards
void enquecommands_P(const char* pgcode)
{ {
if(buflen < BUFSIZE) queued_commands_P= pgcode;
{ drain_queued_commands_P(); // first command exectuted asap (when possible)
//this is dangerous if a mixing of serial and this happens
strcpy_P(&(cmdbuffer[bufindw][0]),cmd);
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_Enqueing);
SERIAL_ECHO(cmdbuffer[bufindw]);
SERIAL_ECHOLNPGM("\"");
bufindw= (bufindw + 1)%BUFSIZE;
buflen += 1;
}
} }
//adds a single command to the main command buffer, from RAM
//that is really done in a non-safe way.
//needs overworking someday
//Returns false if it failed to do so
bool enquecommand(const char *cmd)
{
if(*cmd==';')
return false;
if(buflen >= BUFSIZE)
return false;
//this is dangerous if a mixing of serial and this happens
strcpy(&(cmdbuffer[bufindw][0]),cmd);
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_Enqueing);
SERIAL_ECHO(cmdbuffer[bufindw]);
SERIAL_ECHOLNPGM("\"");
bufindw= (bufindw + 1)%BUFSIZE;
buflen += 1;
return true;
}
void setup_killpin() void setup_killpin()
{ {
#if defined(KILL_PIN) && KILL_PIN > -1 #if defined(KILL_PIN) && KILL_PIN > -1
@ -502,32 +536,28 @@ void setup_homepin(void)
void setup_photpin() void setup_photpin()
{ {
#if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1 #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
SET_OUTPUT(PHOTOGRAPH_PIN); OUT_WRITE(PHOTOGRAPH_PIN, LOW);
WRITE(PHOTOGRAPH_PIN, LOW);
#endif #endif
} }
void setup_powerhold() void setup_powerhold()
{ {
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1 #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
SET_OUTPUT(SUICIDE_PIN); OUT_WRITE(SUICIDE_PIN, HIGH);
WRITE(SUICIDE_PIN, HIGH);
#endif #endif
#if defined(PS_ON_PIN) && PS_ON_PIN > -1 #if defined(PS_ON_PIN) && PS_ON_PIN > -1
SET_OUTPUT(PS_ON_PIN); #if defined(PS_DEFAULT_OFF)
#if defined(PS_DEFAULT_OFF) OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
WRITE(PS_ON_PIN, PS_ON_ASLEEP);
#else #else
WRITE(PS_ON_PIN, PS_ON_AWAKE); OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE);
#endif #endif
#endif #endif
} }
void suicide() void suicide()
{ {
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1 #if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
SET_OUTPUT(SUICIDE_PIN); OUT_WRITE(SUICIDE_PIN, LOW);
WRITE(SUICIDE_PIN, LOW);
#endif #endif
} }
@ -618,7 +648,7 @@ void setup()
lcd_init(); lcd_init();
_delay_ms(1000); // wait 1sec to display the splash screen _delay_ms(1000); // wait 1sec to display the splash screen
#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1 #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
@ -632,6 +662,15 @@ void setup()
digitalWrite(SERVO0_PIN, LOW); // turn it off digitalWrite(SERVO0_PIN, LOW); // turn it off
#endif // Z_PROBE_SLED #endif // Z_PROBE_SLED
setup_homepin(); setup_homepin();
#ifdef STAT_LED_RED
pinMode(STAT_LED_RED, OUTPUT);
digitalWrite(STAT_LED_RED, LOW); // turn it off
#endif
#ifdef STAT_LED_BLUE
pinMode(STAT_LED_BLUE, OUTPUT);
digitalWrite(STAT_LED_BLUE, LOW); // turn it off
#endif
} }
@ -684,6 +723,9 @@ void loop()
void get_command() void get_command()
{ {
if(drain_queued_commands_P()) // priority is given to non-serial commands
return;
while( MYSERIAL.available() > 0 && buflen < BUFSIZE) { while( MYSERIAL.available() > 0 && buflen < BUFSIZE) {
serial_char = MYSERIAL.read(); serial_char = MYSERIAL.read();
if(serial_char == '\n' || if(serial_char == '\n' ||
@ -702,7 +744,7 @@ void get_command()
if(strchr(cmdbuffer[bufindw], 'N') != NULL) if(strchr(cmdbuffer[bufindw], 'N') != NULL)
{ {
strchr_pointer = strchr(cmdbuffer[bufindw], 'N'); strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10)); gcode_N = (strtol(strchr_pointer + 1, NULL, 10));
if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) { if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORPGM(MSG_ERR_LINE_NO); SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
@ -720,7 +762,7 @@ void get_command()
while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++]; while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
strchr_pointer = strchr(cmdbuffer[bufindw], '*'); strchr_pointer = strchr(cmdbuffer[bufindw], '*');
if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) { if( (int)(strtod(strchr_pointer + 1, NULL)) != checksum) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH); SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
SERIAL_ERRORLN(gcode_LastN); SERIAL_ERRORLN(gcode_LastN);
@ -756,7 +798,7 @@ void get_command()
} }
if((strchr(cmdbuffer[bufindw], 'G') != NULL)){ if((strchr(cmdbuffer[bufindw], 'G') != NULL)){
strchr_pointer = strchr(cmdbuffer[bufindw], 'G'); strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){ switch((int)((strtod(strchr_pointer + 1, NULL)))){
case 0: case 0:
case 1: case 1:
case 2: case 2:
@ -855,12 +897,12 @@ void get_command()
float code_value() float code_value()
{ {
return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); return (strtod(strchr_pointer + 1, NULL));
} }
long code_value_long() long code_value_long()
{ {
return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); return (strtol(strchr_pointer + 1, NULL, 10));
} }
bool code_seen(char code) bool code_seen(char code)
@ -958,7 +1000,7 @@ static void axis_is_at_home(int axis) {
{ {
homeposition[i] = base_home_pos(i); homeposition[i] = base_home_pos(i);
} }
// SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]); // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
// SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]); // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
// Works out real Homeposition angles using inverse kinematics, // Works out real Homeposition angles using inverse kinematics,
// and calculates homing offset using forward kinematics // and calculates homing offset using forward kinematics
@ -973,7 +1015,7 @@ static void axis_is_at_home(int axis) {
} }
// SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(add_homing[X_AXIS]); // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(add_homing[X_AXIS]);
// SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(add_homing[Y_AXIS]); // SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(add_homing[Y_AXIS]);
// SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]); // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
// SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
@ -1161,22 +1203,24 @@ static void retract_z_probe() {
#endif #endif
} }
enum ProbeAction { ProbeStay, ProbeEngage, ProbeRetract, ProbeEngageRetract };
/// Probe bed height at position (x,y), returns the measured z value /// Probe bed height at position (x,y), returns the measured z value
static float probe_pt(float x, float y, float z_before, int retract_action=0) { static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageRetract) {
// move to right place // move to right place
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before); do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]); do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
#ifndef Z_PROBE_SLED #ifndef Z_PROBE_SLED
if ((retract_action==0) || (retract_action==1)) if (retract_action & ProbeEngage) engage_z_probe();
engage_z_probe(); // Engage Z Servo endstop if available #endif
#endif // Z_PROBE_SLED
run_z_probe(); run_z_probe();
float measured_z = current_position[Z_AXIS]; float measured_z = current_position[Z_AXIS];
#ifndef Z_PROBE_SLED
if ((retract_action==0) || (retract_action==3)) #ifndef Z_PROBE_SLED
retract_z_probe(); if (retract_action & ProbeRetract) retract_z_probe();
#endif // Z_PROBE_SLED #endif
SERIAL_PROTOCOLPGM(MSG_BED); SERIAL_PROTOCOLPGM(MSG_BED);
SERIAL_PROTOCOLPGM(" x: "); SERIAL_PROTOCOLPGM(" x: ");
@ -1216,7 +1260,7 @@ static void homeaxis(int axis) {
if (axis==Z_AXIS) { if (axis==Z_AXIS) {
engage_z_probe(); engage_z_probe();
} }
else else
#endif #endif
if (servo_endstops[axis] > -1) { if (servo_endstops[axis] > -1) {
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]); servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
@ -1337,6 +1381,11 @@ void refresh_cmd_timeout(void)
#endif //FWRETRACT #endif //FWRETRACT
#ifdef Z_PROBE_SLED #ifdef Z_PROBE_SLED
#ifndef SLED_DOCKING_OFFSET
#define SLED_DOCKING_OFFSET 0
#endif
// //
// Method to dock/undock a sled designed by Charles Bell. // Method to dock/undock a sled designed by Charles Bell.
// //
@ -1372,157 +1421,159 @@ static void dock_sled(bool dock, int offset=0) {
} }
#endif #endif
void process_commands() /**
{ *
unsigned long codenum; //throw away variable * G-Code Handler functions
char *starpos = NULL; *
#ifdef ENABLE_AUTO_BED_LEVELING */
float x_tmp, y_tmp, z_tmp, real_z;
#endif /**
if(code_seen('G')) * G0, G1: Coordinated movement of X Y Z E axes
{ */
switch((int)code_value()) inline void gcode_G0_G1() {
{ if (!Stopped) {
case 0: // G0 -> G1 get_coordinates(); // For X Y Z E F
case 1: // G1 #ifdef FWRETRACT
if(Stopped == false) { if (autoretract_enabled)
get_coordinates(); // For X Y Z E F if (!(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
#ifdef FWRETRACT float echange = destination[E_AXIS] - current_position[E_AXIS];
if(autoretract_enabled) // Is this move an attempt to retract or recover?
if( !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) { if ((echange < -MIN_RETRACT && !retracted[active_extruder]) || (echange > MIN_RETRACT && retracted[active_extruder])) {
float echange=destination[E_AXIS]-current_position[E_AXIS]; current_position[E_AXIS] = destination[E_AXIS]; // hide the slicer-generated retract/recover from calculations
if((echange<-MIN_RETRACT && !retracted) || (echange>MIN_RETRACT && retracted)) { //move appears to be an attempt to retract or recover plan_set_e_position(current_position[E_AXIS]); // AND from the planner
current_position[E_AXIS] = destination[E_AXIS]; //hide the slicer-generated retract/recover from calculations retract(!retracted[active_extruder]);
plan_set_e_position(current_position[E_AXIS]); //AND from the planner return;
retract(!retracted); }
return;
}
}
#endif //FWRETRACT
prepare_move();
//ClearToSend();
} }
break; #endif //FWRETRACT
#ifndef SCARA //disable arc support prepare_move();
case 2: // G2 - CW ARC //ClearToSend();
if(Stopped == false) { }
get_arc_coordinates(); }
prepare_arc_move(true);
/**
* G2: Clockwise Arc
* G3: Counterclockwise Arc
*/
inline void gcode_G2_G3(bool clockwise) {
if (!Stopped) {
get_arc_coordinates();
prepare_arc_move(clockwise);
}
}
/**
* G4: Dwell S<seconds> or P<milliseconds>
*/
inline void gcode_G4() {
unsigned long codenum;
LCD_MESSAGEPGM(MSG_DWELL);
if (code_seen('P')) codenum = code_value_long(); // milliseconds to wait
if (code_seen('S')) codenum = code_value_long() * 1000; // seconds to wait
st_synchronize();
previous_millis_cmd = millis();
codenum += previous_millis_cmd; // keep track of when we started waiting
while(millis() < codenum) {
manage_heater();
manage_inactivity();
lcd_update();
}
}
#ifdef FWRETRACT
/**
* G10 - Retract filament according to settings of M207
* G11 - Recover filament according to settings of M208
*/
inline void gcode_G10_G11(bool doRetract=false) {
#if EXTRUDERS > 1
if (doRetract) {
retracted_swap[active_extruder] = (code_seen('S') && code_value_long() == 1); // checks for swap retract argument
} }
break; #endif
case 3: // G3 - CCW ARC retract(doRetract
if(Stopped == false) { #if EXTRUDERS > 1
get_arc_coordinates(); , retracted_swap[active_extruder]
prepare_arc_move(false); #endif
} );
break; }
#endif
case 4: // G4 dwell
LCD_MESSAGEPGM(MSG_DWELL);
codenum = 0;
if(code_seen('P')) codenum = code_value(); // milliseconds to wait
if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
st_synchronize(); #endif //FWRETRACT
codenum += millis(); // keep track of when we started waiting
previous_millis_cmd = millis();
while(millis() < codenum) {
manage_heater();
manage_inactivity();
lcd_update();
}
break;
#ifdef FWRETRACT
case 10: // G10 retract
#if EXTRUDERS > 1
retracted_swap[active_extruder]=(code_seen('S') && code_value_long() == 1); // checks for swap retract argument
retract(true,retracted_swap[active_extruder]);
#else
retract(true);
#endif
break;
case 11: // G11 retract_recover
#if EXTRUDERS > 1
retract(false,retracted_swap[active_extruder]);
#else
retract(false);
#endif
break;
#endif //FWRETRACT
case 28: //G28 Home all Axis one at a time
#ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#endif //ENABLE_AUTO_BED_LEVELING
saved_feedrate = feedrate; /**
saved_feedmultiply = feedmultiply; * G28: Home all axes, one at a time
feedmultiply = 100; */
previous_millis_cmd = millis(); inline void gcode_G28() {
#ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#endif
enable_endstops(true); saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
previous_millis_cmd = millis();
for(int8_t i=0; i < NUM_AXIS; i++) { enable_endstops(true);
destination[i] = current_position[i];
}
feedrate = 0.0;
#ifdef DELTA for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = current_position[i];
// A delta can only safely home all axis at the same time
// all axis have to home at the same time
// Move all carriages up together until the first endstop is hit. feedrate = 0.0;
current_position[X_AXIS] = 0;
current_position[Y_AXIS] = 0;
current_position[Z_AXIS] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
destination[X_AXIS] = 3 * Z_MAX_LENGTH; #ifdef DELTA
destination[Y_AXIS] = 3 * Z_MAX_LENGTH; // A delta can only safely home all axis at the same time
destination[Z_AXIS] = 3 * Z_MAX_LENGTH; // all axis have to home at the same time
feedrate = 1.732 * homing_feedrate[X_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
endstops_hit_on_purpose();
current_position[X_AXIS] = destination[X_AXIS]; // Move all carriages up together until the first endstop is hit.
current_position[Y_AXIS] = destination[Y_AXIS]; for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = 0;
current_position[Z_AXIS] = destination[Z_AXIS]; plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
// take care of back off and rehome now we are all at the top for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH;
HOMEAXIS(X); feedrate = 1.732 * homing_feedrate[X_AXIS];
HOMEAXIS(Y); plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
HOMEAXIS(Z); st_synchronize();
endstops_hit_on_purpose();
calculate_delta(current_position); // Destination reached
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = destination[i];
#else // NOT DELTA // take care of back off and rehome now we are all at the top
HOMEAXIS(X);
HOMEAXIS(Y);
HOMEAXIS(Z);
home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))); calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#if Z_HOME_DIR > 0 // If homing away from BED do Z first #else // NOT DELTA
if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
home_all_axis = !(code_seen(axis_codes[X_AXIS]) || code_seen(axis_codes[Y_AXIS]) || code_seen(axis_codes[Z_AXIS]));
#if Z_HOME_DIR > 0 // If homing away from BED do Z first
if (home_all_axis || code_seen(axis_codes[Z_AXIS])) {
HOMEAXIS(Z); HOMEAXIS(Z);
} }
#endif #endif
#ifdef QUICK_HOME #ifdef QUICK_HOME
if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) ) //first diagonal move if (home_all_axis || code_seen(axis_codes[X_AXIS] && code_seen(axis_codes[Y_AXIS]))) { //first diagonal move
{ current_position[X_AXIS] = current_position[Y_AXIS] = 0;
current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
#ifndef DUAL_X_CARRIAGE #ifndef DUAL_X_CARRIAGE
int x_axis_home_dir = home_dir(X_AXIS); int x_axis_home_dir = home_dir(X_AXIS);
#else #else
int x_axis_home_dir = x_home_dir(active_extruder); int x_axis_home_dir = x_home_dir(active_extruder);
extruder_duplication_enabled = false; extruder_duplication_enabled = false;
#endif #endif
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS); destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;
destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS);
feedrate = homing_feedrate[X_AXIS]; feedrate = homing_feedrate[X_AXIS];
if(homing_feedrate[Y_AXIS]<feedrate) if (homing_feedrate[Y_AXIS] < feedrate) feedrate = homing_feedrate[Y_AXIS];
feedrate = homing_feedrate[Y_AXIS];
if (max_length(X_AXIS) > max_length(Y_AXIS)) { if (max_length(X_AXIS) > max_length(Y_AXIS)) {
feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1); feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1);
} else { } else {
@ -1543,14 +1594,13 @@ void process_commands()
current_position[X_AXIS] = destination[X_AXIS]; current_position[X_AXIS] = destination[X_AXIS];
current_position[Y_AXIS] = destination[Y_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS];
#ifndef SCARA #ifndef SCARA
current_position[Z_AXIS] = destination[Z_AXIS]; current_position[Z_AXIS] = destination[Z_AXIS];
#endif #endif
} }
#endif #endif //QUICK_HOME
if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) if ((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) {
{
#ifdef DUAL_X_CARRIAGE #ifdef DUAL_X_CARRIAGE
int tmp_extruder = active_extruder; int tmp_extruder = active_extruder;
extruder_duplication_enabled = false; extruder_duplication_enabled = false;
@ -1566,2398 +1616,3140 @@ void process_commands()
#else #else
HOMEAXIS(X); HOMEAXIS(X);
#endif #endif
} }
if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) { if (home_all_axis || code_seen(axis_codes[Y_AXIS])) HOMEAXIS(Y);
HOMEAXIS(Y);
}
if(code_seen(axis_codes[X_AXIS])) if (code_seen(axis_codes[X_AXIS])) {
{ if (code_value_long() != 0) {
if(code_value_long() != 0) { current_position[X_AXIS] = code_value()
#ifdef SCARA #ifndef SCARA
current_position[X_AXIS]=code_value(); + add_homing[X_AXIS]
#else
current_position[X_AXIS]=code_value()+add_homing[X_AXIS];
#endif
}
}
if(code_seen(axis_codes[Y_AXIS])) {
if(code_value_long() != 0) {
#ifdef SCARA
current_position[Y_AXIS]=code_value();
#else
current_position[Y_AXIS]=code_value()+add_homing[Y_AXIS];
#endif
}
}
#if Z_HOME_DIR < 0 // If homing towards BED do Z last
#ifndef Z_SAFE_HOMING
if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
#if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
st_synchronize();
#endif #endif
HOMEAXIS(Z); ;
} }
#else // Z Safe mode activated. }
if(home_all_axis) {
destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed
feedrate = XY_TRAVEL_SPEED/60;
current_position[Z_AXIS] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); if (code_seen(axis_codes[Y_AXIS]) && code_value_long() != 0) {
current_position[Y_AXIS] = code_value()
#ifndef SCARA
+ add_homing[Y_AXIS]
#endif
;
}
#if Z_HOME_DIR < 0 // If homing towards BED do Z last
#ifndef Z_SAFE_HOMING
if (home_all_axis || code_seen(axis_codes[Z_AXIS])) {
#if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
st_synchronize(); st_synchronize();
current_position[X_AXIS] = destination[X_AXIS]; #endif
current_position[Y_AXIS] = destination[Y_AXIS]; HOMEAXIS(Z);
}
HOMEAXIS(Z); #else // Z_SAFE_HOMING
}
// Let's see if X and Y are homed and probe is inside bed area.
if(code_seen(axis_codes[Z_AXIS])) {
if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \
&& (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \
&& (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \
&& (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \
&& (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) {
if (home_all_axis) {
destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = XY_TRAVEL_SPEED / 60;
current_position[Z_AXIS] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
st_synchronize();
current_position[X_AXIS] = destination[X_AXIS];
current_position[Y_AXIS] = destination[Y_AXIS];
HOMEAXIS(Z);
}
// Let's see if X and Y are homed and probe is inside bed area.
if (code_seen(axis_codes[Z_AXIS])) {
if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) {
float cpx = current_position[X_AXIS], cpy = current_position[Y_AXIS];
if ( cpx >= X_MIN_POS - X_PROBE_OFFSET_FROM_EXTRUDER
&& cpx <= X_MAX_POS - X_PROBE_OFFSET_FROM_EXTRUDER
&& cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER
&& cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) {
current_position[Z_AXIS] = 0; current_position[Z_AXIS] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS]; feedrate = max_feedrate[Z_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
st_synchronize(); st_synchronize();
HOMEAXIS(Z); HOMEAXIS(Z);
} else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) { }
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN); else {
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
} else {
LCD_MESSAGEPGM(MSG_ZPROBE_OUT); LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT); SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
} }
} }
#endif else {
#endif LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
}
if(code_seen(axis_codes[Z_AXIS])) {
if(code_value_long() != 0) {
current_position[Z_AXIS]=code_value()+add_homing[Z_AXIS];
} }
}
#ifdef ENABLE_AUTO_BED_LEVELING
if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
current_position[Z_AXIS] += zprobe_zoffset; //Add Z_Probe offset (the distance is negative)
}
#endif
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif // else DELTA
#ifdef SCARA #endif // Z_SAFE_HOMING
calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#endif // SCARA
#ifdef ENDSTOPS_ONLY_FOR_HOMING #endif // Z_HOME_DIR < 0
enable_endstops(false);
#endif
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply; if (code_seen(axis_codes[Z_AXIS]) && code_value_long() != 0)
previous_millis_cmd = millis(); current_position[Z_AXIS] = code_value() + add_homing[Z_AXIS];
endstops_hit_on_purpose();
break; #ifdef ENABLE_AUTO_BED_LEVELING
if (home_all_axis || code_seen(axis_codes[Z_AXIS]))
current_position[Z_AXIS] += zprobe_zoffset; //Add Z_Probe offset (the distance is negative)
#endif
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif // else DELTA
#ifdef SCARA
calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#endif
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
#endif
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply;
previous_millis_cmd = millis();
endstops_hit_on_purpose();
}
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
{ /**
#if Z_MIN_PIN == -1 * G29: Detailed Z-Probe, probes the bed at 3 or more points.
#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin." * Will fail if the printer has not been homed with G28.
*
* Enhanced G29 Auto Bed Leveling Probe Routine
*
* Parameters With AUTO_BED_LEVELING_GRID:
*
* P Set the size of the grid that will be probed (P x P points).
* Example: "G29 P4"
*
* V Set the verbose level (0-4). Example: "G29 V3"
*
* T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
* This is useful for manual bed leveling and finding flaws in the bed (to
* assist with part placement).
*
* F Set the Front limit of the probing grid
* B Set the Back limit of the probing grid
* L Set the Left limit of the probing grid
* R Set the Right limit of the probing grid
*
* Global Parameters:
*
* E/e By default G29 engages / disengages the probe for each point.
* Include "E" to engage and disengage the probe just once.
* There's no extra effect if you have a fixed probe.
* Usage: "G29 E" or "G29 e"
*
*/
// Use one of these defines to specify the origin
// for a topographical map to be printed for your bed.
enum { OriginBackLeft, OriginFrontLeft, OriginBackRight, OriginFrontRight };
#define TOPO_ORIGIN OriginFrontLeft
inline void gcode_G29() {
float x_tmp, y_tmp, z_tmp, real_z;
// Prevent user from running a G29 without first homing in X and Y
if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
return;
}
bool enhanced_g29 = code_seen('E') || code_seen('e');
#ifdef AUTO_BED_LEVELING_GRID
// Example Syntax: G29 N4 V2 E T
int verbose_level = 1;
bool topo_flag = code_seen('T') || code_seen('t');
if (code_seen('V') || code_seen('v')) {
verbose_level = code_value();
if (verbose_level < 0 || verbose_level > 4) {
SERIAL_PROTOCOLPGM("?(V)erbose Level is implausible (0-4).\n");
return;
}
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("G29 Enhanced Auto Bed Leveling Code V1.25:\n");
SERIAL_PROTOCOLPGM("Full support at: http://3dprintboard.com/forum.php\n");
if (verbose_level > 2) topo_flag = true;
}
}
int auto_bed_leveling_grid_points = code_seen('P') ? code_value_long() : AUTO_BED_LEVELING_GRID_POINTS;
if (auto_bed_leveling_grid_points < 2 || auto_bed_leveling_grid_points > AUTO_BED_LEVELING_GRID_POINTS) {
SERIAL_PROTOCOLPGM("?Number of probed (P)oints is implausible (2 minimum).\n");
return;
}
// Define the possible boundaries for probing based on the set limits.
// Code above (in G28) might have these limits wrong, or I am wrong here.
#define MIN_PROBE_EDGE 10 // Edges of the probe square can be no less
const int min_probe_x = max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER),
max_probe_x = min(X_MAX_POS, X_MAX_POS + X_PROBE_OFFSET_FROM_EXTRUDER),
min_probe_y = max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER),
max_probe_y = min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER);
int left_probe_bed_position = code_seen('L') ? code_value_long() : LEFT_PROBE_BED_POSITION,
right_probe_bed_position = code_seen('R') ? code_value_long() : RIGHT_PROBE_BED_POSITION,
front_probe_bed_position = code_seen('F') ? code_value_long() : FRONT_PROBE_BED_POSITION,
back_probe_bed_position = code_seen('B') ? code_value_long() : BACK_PROBE_BED_POSITION;
bool left_out_l = left_probe_bed_position < min_probe_x,
left_out_r = left_probe_bed_position > right_probe_bed_position - MIN_PROBE_EDGE,
left_out = left_out_l || left_out_r,
right_out_r = right_probe_bed_position > max_probe_x,
right_out_l =right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE,
right_out = right_out_l || right_out_r,
front_out_f = front_probe_bed_position < min_probe_y,
front_out_b = front_probe_bed_position > back_probe_bed_position - MIN_PROBE_EDGE,
front_out = front_out_f || front_out_b,
back_out_b = back_probe_bed_position > max_probe_y,
back_out_f = back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE,
back_out = back_out_f || back_out_b;
if (left_out || right_out || front_out || back_out) {
if (left_out) {
SERIAL_PROTOCOLPGM("?Probe (L)eft position out of range.\n");
left_probe_bed_position = left_out_l ? min_probe_x : right_probe_bed_position - MIN_PROBE_EDGE;
}
if (right_out) {
SERIAL_PROTOCOLPGM("?Probe (R)ight position out of range.\n");
right_probe_bed_position = right_out_r ? max_probe_x : left_probe_bed_position + MIN_PROBE_EDGE;
}
if (front_out) {
SERIAL_PROTOCOLPGM("?Probe (F)ront position out of range.\n");
front_probe_bed_position = front_out_f ? min_probe_y : back_probe_bed_position - MIN_PROBE_EDGE;
}
if (back_out) {
SERIAL_PROTOCOLPGM("?Probe (B)ack position out of range.\n");
back_probe_bed_position = back_out_b ? max_probe_y : front_probe_bed_position + MIN_PROBE_EDGE;
}
return;
}
#endif // AUTO_BED_LEVELING_GRID
#ifdef Z_PROBE_SLED
dock_sled(false); // engage (un-dock) the probe
#endif
st_synchronize();
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_get_position();
//uncorrected_position.debug("position durring G29");
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
#ifdef AUTO_BED_LEVELING_GRID
// probe at the points of a lattice grid
int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
// solve the plane equation ax + by + d = z
// A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
eqnBVector[abl2], // "B" vector of Z points
mean = 0.0;
int probePointCounter = 0;
bool zig = true;
for (int yProbe = front_probe_bed_position; yProbe <= back_probe_bed_position; yProbe += yGridSpacing) {
int xProbe, xInc;
if (zig)
xProbe = left_probe_bed_position, xInc = xGridSpacing;
else
xProbe = right_probe_bed_position, xInc = -xGridSpacing;
// If topo_flag is set then don't zig-zag. Just scan in one direction.
// This gets the probe points in more readable order.
if (!topo_flag) zig = !zig;
for (int xCount = 0; xCount < auto_bed_leveling_grid_points; xCount++) {
// raise extruder
float measured_z,
z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
// Enhanced G29 - Do not retract servo between probes
ProbeAction act;
if (enhanced_g29) {
if (yProbe == front_probe_bed_position && xCount == 0)
act = ProbeEngage;
else if (yProbe == front_probe_bed_position + (yGridSpacing * (auto_bed_leveling_grid_points - 1)) && xCount == auto_bed_leveling_grid_points - 1)
act = ProbeRetract;
else
act = ProbeStay;
}
else
act = ProbeEngageRetract;
measured_z = probe_pt(xProbe, yProbe, z_before, act);
mean += measured_z;
eqnBVector[probePointCounter] = measured_z;
eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
eqnAMatrix[probePointCounter + 2 * abl2] = 1;
probePointCounter++;
xProbe += xInc;
} //xProbe
} //yProbe
clean_up_after_endstop_move();
// solve lsq problem
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
mean /= abl2;
if (verbose_level) {
SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
SERIAL_PROTOCOLPGM(" b: ");
SERIAL_PROTOCOL(plane_equation_coefficients[1]);
SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Mean of sampled points: ");
SERIAL_PROTOCOL_F(mean, 6);
SERIAL_PROTOCOLPGM(" \n");
}
}
if (topo_flag) {
int xx, yy;
SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n");
#if TOPO_ORIGIN == OriginFrontLeft
for (yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--)
#else
for (yy = 0; yy < auto_bed_leveling_grid_points; yy++)
#endif
{
#if TOPO_ORIGIN == OriginBackRight
for (xx = auto_bed_leveling_grid_points - 1; xx >= 0; xx--)
#else
for (xx = 0; xx < auto_bed_leveling_grid_points; xx++)
#endif #endif
// Prevent user from running a G29 without first homing in X and Y
if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
{
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
break; // abort G29, since we don't know where we are
}
#ifdef Z_PROBE_SLED
dock_sled(false);
#endif // Z_PROBE_SLED
st_synchronize();
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_get_position();
//uncorrected_position.debug("position durring G29");
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
#ifdef AUTO_BED_LEVELING_GRID
// probe at the points of a lattice grid
int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
// solve the plane equation ax + by + d = z
// A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
// "A" matrix of the linear system of equations
double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3];
// "B" vector of Z points
double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS];
int probePointCounter = 0;
bool zig = true;
for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
{
int xProbe, xInc;
if (zig)
{ {
xProbe = LEFT_PROBE_BED_POSITION; int ind =
//xEnd = RIGHT_PROBE_BED_POSITION; #if TOPO_ORIGIN == OriginBackRight || TOPO_ORIGIN == OriginFrontLeft
xInc = xGridSpacing; yy * auto_bed_leveling_grid_points + xx
zig = false; #elif TOPO_ORIGIN == OriginBackLeft
} else // zag xx * auto_bed_leveling_grid_points + yy
{ #elif TOPO_ORIGIN == OriginFrontRight
xProbe = RIGHT_PROBE_BED_POSITION; abl2 - xx * auto_bed_leveling_grid_points - yy - 1
//xEnd = LEFT_PROBE_BED_POSITION; #endif
xInc = -xGridSpacing; ;
zig = true; float diff = eqnBVector[ind] - mean;
} if (diff >= 0.0)
SERIAL_PROTOCOLPGM(" +"); // Watch column alignment in Pronterface
else
SERIAL_PROTOCOLPGM(" -");
SERIAL_PROTOCOL_F(diff, 5);
} // xx
SERIAL_PROTOCOLPGM("\n");
} // yy
SERIAL_PROTOCOLPGM("\n");
for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++) } //topo_flag
{
float z_before;
if (probePointCounter == 0)
{
// raise before probing
z_before = Z_RAISE_BEFORE_PROBING;
} else
{
// raise extruder
z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
}
float measured_z;
//Enhanced G29 - Do not retract servo between probes
if (code_seen('E') || code_seen('e') )
{
if ((yProbe==FRONT_PROBE_BED_POSITION) && (xCount==0))
{
measured_z = probe_pt(xProbe, yProbe, z_before,1);
} else if ((yProbe==FRONT_PROBE_BED_POSITION + (yGridSpacing * (AUTO_BED_LEVELING_GRID_POINTS-1))) && (xCount == AUTO_BED_LEVELING_GRID_POINTS-1))
{
measured_z = probe_pt(xProbe, yProbe, z_before,3);
} else {
measured_z = probe_pt(xProbe, yProbe, z_before,2);
}
} else {
measured_z = probe_pt(xProbe, yProbe, z_before);
}
eqnBVector[probePointCounter] = measured_z;
eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe;
eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe;
eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1;
probePointCounter++;
xProbe += xInc;
}
}
clean_up_after_endstop_move();
// solve lsq problem
double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector);
SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
SERIAL_PROTOCOLPGM(" b: ");
SERIAL_PROTOCOL(plane_equation_coefficients[1]);
SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
set_bed_level_equation_lsq(plane_equation_coefficients); set_bed_level_equation_lsq(plane_equation_coefficients);
free(plane_equation_coefficients);
free(plane_equation_coefficients); #else // !AUTO_BED_LEVELING_GRID
#else // AUTO_BED_LEVELING_GRID not defined // Probe at 3 arbitrary points
float z_at_pt_1, z_at_pt_2, z_at_pt_3;
// Probe at 3 arbitrary points if (enhanced_g29) {
// Enhanced G29 // Basic Enhanced G29
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, ProbeEngage);
float z_at_pt_1,z_at_pt_2,z_at_pt_3; z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeStay);
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeRetract);
if (code_seen('E') || code_seen('e') ) }
{ else {
// probe 1 z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING,1); z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
// probe 2 z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,2); }
// probe 3 clean_up_after_endstop_move();
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,3); set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
}
else
{
// probe 1
float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
// probe 2 #endif // !AUTO_BED_LEVELING_GRID
float z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
// probe 3 st_synchronize();
float z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
}
clean_up_after_endstop_move();
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); if (verbose_level > 0)
plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
// The following code correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected.
real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS];
#endif // AUTO_BED_LEVELING_GRID apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
st_synchronize(); current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
// The following code correct the Z height difference from z-probe position and hotend tip position. #ifdef Z_PROBE_SLED
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
// When the bed is uneven, this height must be corrected. #endif
real_z = float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) }
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS];
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset #ifndef Z_PROBE_SLED
current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); inline void gcode_G30() {
#ifdef Z_PROBE_SLED engage_z_probe(); // Engage Z Servo endstop if available
dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel. st_synchronize();
#endif // Z_PROBE_SLED // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
} setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
run_z_probe();
SERIAL_PROTOCOLPGM(MSG_BED);
SERIAL_PROTOCOLPGM(" X: ");
SERIAL_PROTOCOL(current_position[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL(current_position[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
clean_up_after_endstop_move();
retract_z_probe(); // Retract Z Servo endstop if available
}
#endif //!Z_PROBE_SLED
#endif //ENABLE_AUTO_BED_LEVELING
/**
* G92: Set current position to given X Y Z E
*/
inline void gcode_G92() {
if (!code_seen(axis_codes[E_AXIS]))
st_synchronize();
for (int i=0;i<NUM_AXIS;i++) {
if (code_seen(axis_codes[i])) {
if (i == E_AXIS) {
current_position[i] = code_value();
plan_set_e_position(current_position[E_AXIS]);
}
else {
current_position[i] = code_value() +
#ifdef SCARA
((i != X_AXIS && i != Y_AXIS) ? add_homing[i] : 0)
#else
add_homing[i]
#endif
;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
}
}
}
#ifdef ULTIPANEL
/**
* M0: // M0 - Unconditional stop - Wait for user button press on LCD
* M1: // M1 - Conditional stop - Wait for user button press on LCD
*/
inline void gcode_M0_M1() {
char *src = strchr_pointer + 2;
unsigned long codenum = 0;
bool hasP = false, hasS = false;
if (code_seen('P')) {
codenum = code_value(); // milliseconds to wait
hasP = codenum > 0;
}
if (code_seen('S')) {
codenum = code_value() * 1000; // seconds to wait
hasS = codenum > 0;
}
char* starpos = strchr(src, '*');
if (starpos != NULL) *(starpos) = '\0';
while (*src == ' ') ++src;
if (!hasP && !hasS && *src != '\0')
lcd_setstatus(src);
else
LCD_MESSAGEPGM(MSG_USERWAIT);
lcd_ignore_click();
st_synchronize();
previous_millis_cmd = millis();
if (codenum > 0) {
codenum += previous_millis_cmd; // keep track of when we started waiting
while(millis() < codenum && !lcd_clicked()) {
manage_heater();
manage_inactivity();
lcd_update();
}
lcd_ignore_click(false);
}
else {
if (!lcd_detected()) return;
while (!lcd_clicked()) {
manage_heater();
manage_inactivity();
lcd_update();
}
}
if (IS_SD_PRINTING)
LCD_MESSAGEPGM(MSG_RESUMING);
else
LCD_MESSAGEPGM(WELCOME_MSG);
}
#endif // ULTIPANEL
/**
* M17: Enable power on all stepper motors
*/
inline void gcode_M17() {
LCD_MESSAGEPGM(MSG_NO_MOVE);
enable_x();
enable_y();
enable_z();
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
#ifdef SDSUPPORT
/**
* M20: List SD card to serial output
*/
inline void gcode_M20() {
SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
card.ls();
SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
}
/**
* M21: Init SD Card
*/
inline void gcode_M21() {
card.initsd();
}
/**
* M22: Release SD Card
*/
inline void gcode_M22() {
card.release();
}
/**
* M23: Select a file
*/
inline void gcode_M23() {
char* codepos = strchr_pointer + 4;
char* starpos = strchr(codepos, '*');
if (starpos) *starpos = '\0';
card.openFile(codepos, true);
}
/**
* M24: Start SD Print
*/
inline void gcode_M24() {
card.startFileprint();
starttime = millis();
}
/**
* M25: Pause SD Print
*/
inline void gcode_M25() {
card.pauseSDPrint();
}
/**
* M26: Set SD Card file index
*/
inline void gcode_M26() {
if (card.cardOK && code_seen('S'))
card.setIndex(code_value_long());
}
/**
* M27: Get SD Card status
*/
inline void gcode_M27() {
card.getStatus();
}
/**
* M28: Start SD Write
*/
inline void gcode_M28() {
char* codepos = strchr_pointer + 4;
char* starpos = strchr(strchr_pointer + 4, '*');
if (starpos) {
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos, ' ') + 1;
*(starpos) = '\0';
}
card.openFile(strchr_pointer + 4, false);
}
/**
* M29: Stop SD Write
* Processed in write to file routine above
*/
inline void gcode_M29() {
// card.saving = false;
}
/**
* M30 <filename>: Delete SD Card file
*/
inline void gcode_M30() {
if (card.cardOK) {
card.closefile();
char* starpos = strchr(strchr_pointer + 4, '*');
if (starpos) {
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos, ' ') + 1;
*(starpos) = '\0';
}
card.removeFile(strchr_pointer + 4);
}
}
#endif
/**
* M31: Get the time since the start of SD Print (or last M109)
*/
inline void gcode_M31() {
stoptime = millis();
unsigned long t = (stoptime - starttime) / 1000;
int min = t / 60, sec = t % 60;
char time[30];
sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
SERIAL_ECHO_START;
SERIAL_ECHOLN(time);
lcd_setstatus(time);
autotempShutdown();
}
#ifdef SDSUPPORT
/**
* M32: Select file and start SD Print
*/
inline void gcode_M32() {
if (card.sdprinting)
st_synchronize();
char* codepos = strchr_pointer + 4;
char* namestartpos = strchr(codepos, '!'); //find ! to indicate filename string start.
if (! namestartpos)
namestartpos = codepos; //default name position, 4 letters after the M
else
namestartpos++; //to skip the '!'
char* starpos = strchr(codepos, '*');
if (starpos) *(starpos) = '\0';
bool call_procedure = code_seen('P') && (strchr_pointer < namestartpos);
if (card.cardOK) {
card.openFile(namestartpos, true, !call_procedure);
if (code_seen('S') && strchr_pointer < namestartpos) // "S" (must occur _before_ the filename!)
card.setIndex(code_value_long());
card.startFileprint();
if (!call_procedure)
starttime = millis(); //procedure calls count as normal print time.
}
}
/**
* M928: Start SD Write
*/
inline void gcode_M928() {
char* starpos = strchr(strchr_pointer + 5, '*');
if (starpos) {
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos, ' ') + 1;
*(starpos) = '\0';
}
card.openLogFile(strchr_pointer + 5);
}
#endif // SDSUPPORT
/**
* M42: Change pin status via GCode
*/
inline void gcode_M42() {
if (code_seen('S')) {
int pin_status = code_value(),
pin_number = LED_PIN;
if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
pin_number = code_value();
for (int8_t i = 0; i < (int8_t)(sizeof(sensitive_pins) / sizeof(*sensitive_pins)); i++) {
if (sensitive_pins[i] == pin_number) {
pin_number = -1;
break; break;
#ifndef Z_PROBE_SLED }
case 30: // G30 Single Z Probe }
#if defined(FAN_PIN) && FAN_PIN > -1
if (pin_number == FAN_PIN) fanSpeed = pin_status;
#endif
if (pin_number > -1) {
pinMode(pin_number, OUTPUT);
digitalWrite(pin_number, pin_status);
analogWrite(pin_number, pin_status);
}
} // code_seen('S')
}
#if defined(ENABLE_AUTO_BED_LEVELING) && defined(Z_PROBE_REPEATABILITY_TEST)
#if Z_MIN_PIN == -1
#error "You must have a Z_MIN endstop in order to enable calculation of Z-Probe repeatability."
#endif
/**
* M48: Z-Probe repeatability measurement function.
*
* Usage:
* M48 <n#> <X#> <Y#> <V#> <E> <L#>
* n = Number of samples (4-50, default 10)
* X = Sample X position
* Y = Sample Y position
* V = Verbose level (0-4, default=1)
* E = Engage probe for each reading
* L = Number of legs of movement before probe
*
* This function assumes the bed has been homed. Specificaly, that a G28 command
* as been issued prior to invoking the M48 Z-Probe repeatability measurement function.
* Any information generated by a prior G29 Bed leveling command will be lost and need to be
* regenerated.
*
* The number of samples will default to 10 if not specified. You can use upper or lower case
* letters for any of the options EXCEPT n. n must be in lower case because Marlin uses a capital
* N for its communication protocol and will get horribly confused if you send it a capital N.
*/
inline void gcode_M48() {
double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50];
int verbose_level = 1, n = 0, j, n_samples = 10, n_legs = 0, engage_probe_for_each_reading = 0;
double X_current, Y_current, Z_current;
double X_probe_location, Y_probe_location, Z_start_location, ext_position;
if (code_seen('V') || code_seen('v')) {
verbose_level = code_value();
if (verbose_level < 0 || verbose_level > 4 ) {
SERIAL_PROTOCOLPGM("?Verbose Level not plausible (0-4).\n");
return;
}
}
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("M48 Z-Probe Repeatability test. Version 2.00\n");
SERIAL_PROTOCOLPGM("Full support at: http://3dprintboard.com/forum.php\n");
}
if (code_seen('n')) {
n_samples = code_value();
if (n_samples < 4 || n_samples > 50) {
SERIAL_PROTOCOLPGM("?Specified sample size not plausible (4-50).\n");
return;
}
}
X_current = X_probe_location = st_get_position_mm(X_AXIS);
Y_current = Y_probe_location = st_get_position_mm(Y_AXIS);
Z_current = st_get_position_mm(Z_AXIS);
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
ext_position = st_get_position_mm(E_AXIS);
if (code_seen('E') || code_seen('e'))
engage_probe_for_each_reading++;
if (code_seen('X') || code_seen('x')) {
X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER;
if (X_probe_location < X_MIN_POS || X_probe_location > X_MAX_POS) {
SERIAL_PROTOCOLPGM("?Specified X position out of range.\n");
return;
}
}
if (code_seen('Y') || code_seen('y')) {
Y_probe_location = code_value() - Y_PROBE_OFFSET_FROM_EXTRUDER;
if (Y_probe_location < Y_MIN_POS || Y_probe_location > Y_MAX_POS) {
SERIAL_PROTOCOLPGM("?Specified Y position out of range.\n");
return;
}
}
if (code_seen('L') || code_seen('l')) {
n_legs = code_value();
if (n_legs == 1) n_legs = 2;
if (n_legs < 0 || n_legs > 15) {
SERIAL_PROTOCOLPGM("?Specified number of legs in movement not plausible (0-15).\n");
return;
}
}
//
// Do all the preliminary setup work. First raise the probe.
//
st_synchronize();
plan_bed_level_matrix.set_to_identity();
plan_buffer_line(X_current, Y_current, Z_start_location,
ext_position,
homing_feedrate[Z_AXIS] / 60,
active_extruder);
st_synchronize();
//
// Now get everything to the specified probe point So we can safely do a probe to
// get us close to the bed. If the Z-Axis is far from the bed, we don't want to
// use that as a starting point for each probe.
//
if (verbose_level > 2)
SERIAL_PROTOCOL("Positioning probe for the test.\n");
plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
ext_position,
homing_feedrate[X_AXIS]/60,
active_extruder);
st_synchronize();
current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS);
current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS);
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
current_position[E_AXIS] = ext_position = st_get_position_mm(E_AXIS);
//
// OK, do the inital probe to get us close to the bed.
// Then retrace the right amount and use that in subsequent probes
//
engage_z_probe();
setup_for_endstop_move();
run_z_probe();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
ext_position,
homing_feedrate[X_AXIS]/60,
active_extruder);
st_synchronize();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
if (engage_probe_for_each_reading) retract_z_probe();
for (n=0; n < n_samples; n++) {
do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
if (n_legs) {
double radius=0.0, theta=0.0, x_sweep, y_sweep;
int l;
int rotational_direction = (unsigned long) millis() & 0x0001; // clockwise or counter clockwise
radius = (unsigned long)millis() % (long)(X_MAX_LENGTH / 4); // limit how far out to go
theta = (float)((unsigned long)millis() % 360L) / (360. / (2 * 3.1415926)); // turn into radians
//SERIAL_ECHOPAIR("starting radius: ",radius);
//SERIAL_ECHOPAIR(" theta: ",theta);
//SERIAL_ECHOPAIR(" direction: ",rotational_direction);
//SERIAL_PROTOCOLLNPGM("");
float dir = rotational_direction ? 1 : -1;
for (l = 0; l < n_legs - 1; l++) {
theta += dir * (float)((unsigned long)millis() % 20L) / (360.0/(2*3.1415926)); // turn into radians
radius += (float)(((long)((unsigned long) millis() % 10L)) - 5L);
if (radius < 0.0) radius = -radius;
X_current = X_probe_location + cos(theta) * radius;
Y_current = Y_probe_location + sin(theta) * radius;
// Make sure our X & Y are sane
X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
if (verbose_level > 3) {
SERIAL_ECHOPAIR("x: ", X_current);
SERIAL_ECHOPAIR("y: ", Y_current);
SERIAL_PROTOCOLLNPGM("");
}
do_blocking_move_to( X_current, Y_current, Z_current );
}
do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
}
if (engage_probe_for_each_reading) {
engage_z_probe();
delay(1000);
}
setup_for_endstop_move();
run_z_probe();
sample_set[n] = current_position[Z_AXIS];
//
// Get the current mean for the data points we have so far
//
sum = 0.0;
for (j=0; j<=n; j++) sum += sample_set[j];
mean = sum / (double (n+1));
//
// Now, use that mean to calculate the standard deviation for the
// data points we have so far
//
sum = 0.0;
for (j=0; j<=n; j++) sum += (sample_set[j]-mean) * (sample_set[j]-mean);
sigma = sqrt( sum / (double (n+1)) );
if (verbose_level > 1) {
SERIAL_PROTOCOL(n+1);
SERIAL_PROTOCOL(" of ");
SERIAL_PROTOCOL(n_samples);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6);
}
if (verbose_level > 2) {
SERIAL_PROTOCOL(" mean: ");
SERIAL_PROTOCOL_F(mean,6);
SERIAL_PROTOCOL(" sigma: ");
SERIAL_PROTOCOL_F(sigma,6);
}
if (verbose_level > 0)
SERIAL_PROTOCOLPGM("\n");
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
st_synchronize();
if (engage_probe_for_each_reading) {
retract_z_probe();
delay(1000);
}
}
retract_z_probe();
delay(1000);
clean_up_after_endstop_move();
// enable_endstops(true);
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("Mean: ");
SERIAL_PROTOCOL_F(mean, 6);
SERIAL_PROTOCOLPGM("\n");
}
SERIAL_PROTOCOLPGM("Standard Deviation: ");
SERIAL_PROTOCOL_F(sigma, 6);
SERIAL_PROTOCOLPGM("\n\n");
}
#endif // ENABLE_AUTO_BED_LEVELING && Z_PROBE_REPEATABILITY_TEST
/**
* M104: Set hot end temperature
*/
inline void gcode_M104() {
if (setTargetedHotend(104)) return;
if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
#endif
setWatch();
}
/**
* M105: Read hot end and bed temperature
*/
inline void gcode_M105() {
if (setTargetedHotend(105)) return;
#if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
SERIAL_PROTOCOLPGM("ok T:");
SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
SERIAL_PROTOCOLPGM(" B:");
SERIAL_PROTOCOL_F(degBed(),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetBed(),1);
#endif //TEMP_BED_PIN
for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
SERIAL_PROTOCOLPGM(" T");
SERIAL_PROTOCOL(cur_extruder);
SERIAL_PROTOCOLPGM(":");
SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetHotend(cur_extruder),1);
}
#else
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
#endif
SERIAL_PROTOCOLPGM(" @:");
#ifdef EXTRUDER_WATTS
SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(tmp_extruder))/127);
SERIAL_PROTOCOLPGM("W");
#else
SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
#endif
SERIAL_PROTOCOLPGM(" B@:");
#ifdef BED_WATTS
SERIAL_PROTOCOL((BED_WATTS * getHeaterPower(-1))/127);
SERIAL_PROTOCOLPGM("W");
#else
SERIAL_PROTOCOL(getHeaterPower(-1));
#endif
#ifdef SHOW_TEMP_ADC_VALUES
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
SERIAL_PROTOCOLPGM(" ADC B:");
SERIAL_PROTOCOL_F(degBed(),1);
SERIAL_PROTOCOLPGM("C->");
SERIAL_PROTOCOL_F(rawBedTemp()/OVERSAMPLENR,0);
#endif
for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
SERIAL_PROTOCOLPGM(" T");
SERIAL_PROTOCOL(cur_extruder);
SERIAL_PROTOCOLPGM(":");
SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
SERIAL_PROTOCOLPGM("C->");
SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
}
#endif
SERIAL_PROTOCOLLN("");
}
#if defined(FAN_PIN) && FAN_PIN > -1
/**
* M106: Set Fan Speed
*/
inline void gcode_M106() { fanSpeed = code_seen('S') ? constrain(code_value(), 0, 255) : 255; }
/**
* M107: Fan Off
*/
inline void gcode_M107() { fanSpeed = 0; }
#endif //FAN_PIN
/**
* M109: Wait for extruder(s) to reach temperature
*/
inline void gcode_M109() {
if (setTargetedHotend(109)) return;
LCD_MESSAGEPGM(MSG_HEATING);
CooldownNoWait = code_seen('S');
if (CooldownNoWait || code_seen('R')) {
setTargetHotend(code_value(), tmp_extruder);
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
#endif
}
#ifdef AUTOTEMP
autotemp_enabled = code_seen('F');
if (autotemp_enabled) autotemp_factor = code_value();
if (code_seen('S')) autotemp_min = code_value();
if (code_seen('B')) autotemp_max = code_value();
#endif
setWatch();
unsigned long timetemp = millis();
/* See if we are heating up or cooling down */
target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
cancel_heatup = false;
#ifdef TEMP_RESIDENCY_TIME
long residencyStart = -1;
/* continue to loop until we have reached the target temp
_and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
while((!cancel_heatup)&&((residencyStart == -1) ||
(residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
#else
while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) )
#endif //TEMP_RESIDENCY_TIME
{ // while loop
if (millis() > timetemp + 1000UL) { //Print temp & remaining time every 1s while waiting
SERIAL_PROTOCOLPGM("T:");
SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL((int)tmp_extruder);
#ifdef TEMP_RESIDENCY_TIME
SERIAL_PROTOCOLPGM(" W:");
if (residencyStart > -1) {
timetemp = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
SERIAL_PROTOCOLLN( timetemp );
}
else {
SERIAL_PROTOCOLLN( "?" );
}
#else
SERIAL_PROTOCOLLN("");
#endif
timetemp = millis();
}
manage_heater();
manage_inactivity();
lcd_update();
#ifdef TEMP_RESIDENCY_TIME
// start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
// or when current temp falls outside the hysteresis after target temp was reached
if ((residencyStart == -1 && target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
(residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
(residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
{ {
engage_z_probe(); // Engage Z Servo endstop if available residencyStart = millis();
st_synchronize();
// TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
run_z_probe();
SERIAL_PROTOCOLPGM(MSG_BED);
SERIAL_PROTOCOLPGM(" X: ");
SERIAL_PROTOCOL(current_position[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL(current_position[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
clean_up_after_endstop_move();
retract_z_probe(); // Retract Z Servo endstop if available
} }
#endif //TEMP_RESIDENCY_TIME
}
LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
starttime = previous_millis_cmd = millis();
}
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
/**
* M190: Sxxx Wait for bed current temp to reach target temp. Waits only when heating
* Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
*/
inline void gcode_M190() {
LCD_MESSAGEPGM(MSG_BED_HEATING);
CooldownNoWait = code_seen('S');
if (CooldownNoWait || code_seen('R'))
setTargetBed(code_value());
unsigned long timetemp = millis();
cancel_heatup = false;
target_direction = isHeatingBed(); // true if heating, false if cooling
while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) {
unsigned long ms = millis();
if (ms > timetemp + 1000UL) { //Print Temp Reading every 1 second while heating up.
timetemp = ms;
float tt = degHotend(active_extruder);
SERIAL_PROTOCOLPGM("T:");
SERIAL_PROTOCOL(tt);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL((int)active_extruder);
SERIAL_PROTOCOLPGM(" B:");
SERIAL_PROTOCOL_F(degBed(), 1);
SERIAL_PROTOCOLLN("");
}
manage_heater();
manage_inactivity();
lcd_update();
}
LCD_MESSAGEPGM(MSG_BED_DONE);
previous_millis_cmd = millis();
}
#endif // TEMP_BED_PIN > -1
/**
* M112: Emergency Stop
*/
inline void gcode_M112() {
kill();
}
#ifdef BARICUDA
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
/**
* M126: Heater 1 valve open
*/
inline void gcode_M126() { ValvePressure = code_seen('S') ? constrain(code_value(), 0, 255) : 255; }
/**
* M127: Heater 1 valve close
*/
inline void gcode_M127() { ValvePressure = 0; }
#endif
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
/**
* M128: Heater 2 valve open
*/
inline void gcode_M128() { EtoPPressure = code_seen('S') ? constrain(code_value(), 0, 255) : 255; }
/**
* M129: Heater 2 valve close
*/
inline void gcode_M129() { EtoPPressure = 0; }
#endif
#endif //BARICUDA
/**
* M140: Set bed temperature
*/
inline void gcode_M140() {
if (code_seen('S')) setTargetBed(code_value());
}
#if defined(PS_ON_PIN) && PS_ON_PIN > -1
/**
* M80: Turn on Power Supply
*/
inline void gcode_M80() {
OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE); //GND
// If you have a switch on suicide pin, this is useful
// if you want to start another print with suicide feature after
// a print without suicide...
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
OUT_WRITE(SUICIDE_PIN, HIGH);
#endif
#ifdef ULTIPANEL
powersupply = true;
LCD_MESSAGEPGM(WELCOME_MSG);
lcd_update();
#endif
}
#endif // PS_ON_PIN
/**
* M81: Turn off Power Supply
*/
inline void gcode_M81() {
disable_heater();
st_synchronize();
disable_e0();
disable_e1();
disable_e2();
disable_e3();
finishAndDisableSteppers();
fanSpeed = 0;
delay(1000); // Wait 1 second before switching off
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
st_synchronize();
suicide();
#elif defined(PS_ON_PIN) && PS_ON_PIN > -1
OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
#endif
#ifdef ULTIPANEL
powersupply = false;
LCD_MESSAGEPGM(MACHINE_NAME " " MSG_OFF ".");
lcd_update();
#endif
}
/**
* M82: Set E codes absolute (default)
*/
inline void gcode_M82() { axis_relative_modes[E_AXIS] = false; }
/**
* M82: Set E codes relative while in Absolute Coordinates (G90) mode
*/
inline void gcode_M83() { axis_relative_modes[E_AXIS] = true; }
/**
* M18, M84: Disable all stepper motors
*/
inline void gcode_M18_M84() {
if (code_seen('S')) {
stepper_inactive_time = code_value() * 1000;
}
else {
bool all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))|| (code_seen(axis_codes[E_AXIS])));
if (all_axis) {
st_synchronize();
disable_e0();
disable_e1();
disable_e2();
disable_e3();
finishAndDisableSteppers();
}
else {
st_synchronize();
if (code_seen('X')) disable_x();
if (code_seen('Y')) disable_y();
if (code_seen('Z')) disable_z();
#if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
if (code_seen('E')) {
disable_e0();
disable_e1();
disable_e2();
disable_e3();
}
#endif
}
}
}
/**
* M85: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
*/
inline void gcode_M85() {
if (code_seen('S')) max_inactive_time = code_value() * 1000;
}
/**
* M92: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
*/
inline void gcode_M92() {
for(int8_t i=0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) {
if (i == E_AXIS) {
float value = code_value();
if (value < 20.0) {
float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
max_e_jerk *= factor;
max_feedrate[i] *= factor;
axis_steps_per_sqr_second[i] *= factor;
}
axis_steps_per_unit[i] = value;
}
else {
axis_steps_per_unit[i] = code_value();
}
}
}
}
/**
* M114: Output current position to serial port
*/
inline void gcode_M114() {
SERIAL_PROTOCOLPGM("X:");
SERIAL_PROTOCOL(current_position[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y:");
SERIAL_PROTOCOL(current_position[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z:");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL(current_position[E_AXIS]);
SERIAL_PROTOCOLPGM(MSG_COUNT_X);
SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y:");
SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z:");
SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
SERIAL_PROTOCOLLN("");
#ifdef SCARA
SERIAL_PROTOCOLPGM("SCARA Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta:");
SERIAL_PROTOCOL(delta[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]+add_homing[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta (90):");
SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+add_homing[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta:");
SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLLN("");
#endif
}
/**
* M115: Capabilities string
*/
inline void gcode_M115() {
SERIAL_PROTOCOLPGM(MSG_M115_REPORT);
}
/**
* M117: Set LCD Status Message
*/
inline void gcode_M117() {
char* codepos = strchr_pointer + 5;
char* starpos = strchr(codepos, '*');
if (starpos) *starpos = '\0';
lcd_setstatus(codepos);
}
/**
* M119: Output endstop states to serial output
*/
inline void gcode_M119() {
SERIAL_PROTOCOLLN(MSG_M119_REPORT);
#if defined(X_MIN_PIN) && X_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_X_MIN);
SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(X_MAX_PIN) && X_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_X_MAX);
SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Y_MIN);
SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Y_MAX);
SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Z_MIN);
SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Z_MAX);
SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
}
/**
* M120: Enable endstops
*/
inline void gcode_M120() { enable_endstops(false); }
/**
* M121: Disable endstops
*/
inline void gcode_M121() { enable_endstops(true); }
#ifdef BLINKM
/**
* M150: Set Status LED Color - Use R-U-B for R-G-B
*/
inline void gcode_M150() {
SendColors(
code_seen('R') ? (byte)code_value() : 0,
code_seen('U') ? (byte)code_value() : 0,
code_seen('B') ? (byte)code_value() : 0
);
}
#endif // BLINKM
/**
* M200: Set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
* T<extruder>
* D<millimeters>
*/
inline void gcode_M200() {
tmp_extruder = active_extruder;
if (code_seen('T')) {
tmp_extruder = code_value();
if (tmp_extruder >= EXTRUDERS) {
SERIAL_ECHO_START;
SERIAL_ECHO(MSG_M200_INVALID_EXTRUDER);
return;
}
}
float area = .0;
if (code_seen('D')) {
float diameter = code_value();
// setting any extruder filament size disables volumetric on the assumption that
// slicers either generate in extruder values as cubic mm or as as filament feeds
// for all extruders
volumetric_enabled = (diameter != 0.0);
if (volumetric_enabled) {
filament_size[tmp_extruder] = diameter;
// make sure all extruders have some sane value for the filament size
for (int i=0; i<EXTRUDERS; i++)
if (! filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
}
}
else {
//reserved for setting filament diameter via UFID or filament measuring device
return;
}
calculate_volumetric_multipliers();
}
/**
* M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
*/
inline void gcode_M201() {
for (int8_t i=0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) {
max_acceleration_units_per_sq_second[i] = code_value();
}
}
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
}
#if 0 // Not used for Sprinter/grbl gen6
inline void gcode_M202() {
for(int8_t i=0; i < NUM_AXIS; i++) {
if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
}
}
#endif
/**
* M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
*/
inline void gcode_M203() {
for (int8_t i=0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) {
max_feedrate[i] = code_value();
}
}
}
/**
* M204: Set Default Acceleration and/or Default Filament Acceleration in mm/sec^2 (M204 S3000 T7000)
*
* S = normal moves
* T = filament only moves
*
* Also sets minimum segment time in ms (B20000) to prevent buffer under-runs and M20 minimum feedrate
*/
inline void gcode_M204() {
if (code_seen('S')) acceleration = code_value();
if (code_seen('T')) retract_acceleration = code_value();
}
/**
* M205: Set Advanced Settings
*
* S = Min Feed Rate (mm/s)
* T = Min Travel Feed Rate (mm/s)
* B = Min Segment Time (µs)
* X = Max XY Jerk (mm/s/s)
* Z = Max Z Jerk (mm/s/s)
* E = Max E Jerk (mm/s/s)
*/
inline void gcode_M205() {
if (code_seen('S')) minimumfeedrate = code_value();
if (code_seen('T')) mintravelfeedrate = code_value();
if (code_seen('B')) minsegmenttime = code_value();
if (code_seen('X')) max_xy_jerk = code_value();
if (code_seen('Z')) max_z_jerk = code_value();
if (code_seen('E')) max_e_jerk = code_value();
}
/**
* M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
*/
inline void gcode_M206() {
for (int8_t i=X_AXIS; i <= Z_AXIS; i++) {
if (code_seen(axis_codes[i])) {
add_homing[i] = code_value();
}
}
#ifdef SCARA
if (code_seen('T')) add_homing[X_AXIS] = code_value(); // Theta
if (code_seen('P')) add_homing[Y_AXIS] = code_value(); // Psi
#endif
}
#ifdef DELTA
/**
* M665: Set delta configurations
*
* L = diagonal rod
* R = delta radius
* S = segments per second
*/
inline void gcode_M665() {
if (code_seen('L')) delta_diagonal_rod = code_value();
if (code_seen('R')) delta_radius = code_value();
if (code_seen('S')) delta_segments_per_second = code_value();
recalc_delta_settings(delta_radius, delta_diagonal_rod);
}
/**
* M666: Set delta endstop adjustment
*/
inline void gcode_M666() {
for (int8_t i = 0; i < 3; i++) {
if (code_seen(axis_codes[i])) {
endstop_adj[i] = code_value();
}
}
}
#endif // DELTA
#ifdef FWRETRACT
/**
* M207: Set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
*/
inline void gcode_M207() {
if (code_seen('S')) retract_length = code_value();
if (code_seen('F')) retract_feedrate = code_value() / 60;
if (code_seen('Z')) retract_zlift = code_value();
}
/**
* M208: Set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
*/
inline void gcode_M208() {
if (code_seen('S')) retract_recover_length = code_value();
if (code_seen('F')) retract_recover_feedrate = code_value() / 60;
}
/**
* M209: Enable automatic retract (M209 S1)
* detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
*/
inline void gcode_M209() {
if (code_seen('S')) {
int t = code_value();
switch(t) {
case 0:
autoretract_enabled = false;
break;
case 1:
autoretract_enabled = true;
break;
default:
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
SERIAL_ECHO(cmdbuffer[bufindr]);
SERIAL_ECHOLNPGM("\"");
return;
}
for (int i=0; i<EXTRUDERS; i++) retracted[i] = false;
}
}
#endif // FWRETRACT
#if EXTRUDERS > 1
/**
* M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
*/
inline void gcode_M218() {
if (setTargetedHotend(218)) return;
if (code_seen('X')) extruder_offset[X_AXIS][tmp_extruder] = code_value();
if (code_seen('Y')) extruder_offset[Y_AXIS][tmp_extruder] = code_value();
#ifdef DUAL_X_CARRIAGE
if (code_seen('Z')) extruder_offset[Z_AXIS][tmp_extruder] = code_value();
#endif
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
for (tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++) {
SERIAL_ECHO(" ");
SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
#ifdef DUAL_X_CARRIAGE
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
#endif
}
SERIAL_EOL;
}
#endif // EXTRUDERS > 1
/**
* M220: Set speed percentage factor, aka "Feed Rate" (M220 S95)
*/
inline void gcode_M220() {
if (code_seen('S')) feedmultiply = code_value();
}
/**
* M221: Set extrusion percentage (M221 T0 S95)
*/
inline void gcode_M221() {
if (code_seen('S')) {
int sval = code_value();
if (code_seen('T')) {
if (setTargetedHotend(221)) return;
extruder_multiply[tmp_extruder] = sval;
}
else {
extrudemultiply = sval;
}
}
}
/**
* M226: Wait until the specified pin reaches the state required (M226 P<pin> S<state>)
*/
inline void gcode_M226() {
if (code_seen('P')) {
int pin_number = code_value();
int pin_state = code_seen('S') ? code_value() : -1; // required pin state - default is inverted
if (pin_state >= -1 && pin_state <= 1) {
for (int8_t i = 0; i < (int8_t)(sizeof(sensitive_pins)/sizeof(*sensitive_pins)); i++) {
if (sensitive_pins[i] == pin_number) {
pin_number = -1;
break;
}
}
if (pin_number > -1) {
int target = LOW;
st_synchronize();
pinMode(pin_number, INPUT);
switch(pin_state){
case 1:
target = HIGH;
break;
case 0:
target = LOW;
break;
case -1:
target = !digitalRead(pin_number);
break;
}
while(digitalRead(pin_number) != target) {
manage_heater();
manage_inactivity();
lcd_update();
}
} // pin_number > -1
} // pin_state -1 0 1
} // code_seen('P')
}
#if NUM_SERVOS > 0
/**
* M280: Set servo position absolute. P: servo index, S: angle or microseconds
*/
inline void gcode_M280() {
int servo_index = code_seen('P') ? code_value() : -1;
int servo_position = 0;
if (code_seen('S')) {
servo_position = code_value();
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
#if defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
servos[servo_index].attach(0);
#endif
servos[servo_index].write(servo_position);
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_index].detach();
#endif
}
else {
SERIAL_ECHO_START;
SERIAL_ECHO("Servo ");
SERIAL_ECHO(servo_index);
SERIAL_ECHOLN(" out of range");
}
}
else if (servo_index >= 0) {
SERIAL_PROTOCOL(MSG_OK);
SERIAL_PROTOCOL(" Servo ");
SERIAL_PROTOCOL(servo_index);
SERIAL_PROTOCOL(": ");
SERIAL_PROTOCOL(servos[servo_index].read());
SERIAL_PROTOCOLLN("");
}
}
#endif // NUM_SERVOS > 0
#if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
/**
* M300: Play beep sound S<frequency Hz> P<duration ms>
*/
inline void gcode_M300() {
int beepS = code_seen('S') ? code_value() : 110;
int beepP = code_seen('P') ? code_value() : 1000;
if (beepS > 0) {
#if BEEPER > 0
tone(BEEPER, beepS);
delay(beepP);
noTone(BEEPER);
#elif defined(ULTRALCD)
lcd_buzz(beepS, beepP);
#elif defined(LCD_USE_I2C_BUZZER)
lcd_buzz(beepP, beepS);
#endif
}
else {
delay(beepP);
}
}
#endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
#ifdef PIDTEMP
/**
* M301: Set PID parameters P I D (and optionally C)
*/
inline void gcode_M301() {
// multi-extruder PID patch: M301 updates or prints a single extruder's PID values
// default behaviour (omitting E parameter) is to update for extruder 0 only
int e = code_seen('E') ? code_value() : 0; // extruder being updated
if (e < EXTRUDERS) { // catch bad input value
if (code_seen('P')) PID_PARAM(Kp, e) = code_value();
if (code_seen('I')) PID_PARAM(Ki, e) = scalePID_i(code_value());
if (code_seen('D')) PID_PARAM(Kd, e) = scalePID_d(code_value());
#ifdef PID_ADD_EXTRUSION_RATE
if (code_seen('C')) PID_PARAM(Kc, e) = code_value();
#endif
updatePID();
SERIAL_PROTOCOL(MSG_OK);
#ifdef PID_PARAMS_PER_EXTRUDER
SERIAL_PROTOCOL(" e:"); // specify extruder in serial output
SERIAL_PROTOCOL(e);
#endif // PID_PARAMS_PER_EXTRUDER
SERIAL_PROTOCOL(" p:");
SERIAL_PROTOCOL(PID_PARAM(Kp, e));
SERIAL_PROTOCOL(" i:");
SERIAL_PROTOCOL(unscalePID_i(PID_PARAM(Ki, e)));
SERIAL_PROTOCOL(" d:");
SERIAL_PROTOCOL(unscalePID_d(PID_PARAM(Kd, e)));
#ifdef PID_ADD_EXTRUSION_RATE
SERIAL_PROTOCOL(" c:");
//Kc does not have scaling applied above, or in resetting defaults
SERIAL_PROTOCOL(PID_PARAM(Kc, e));
#endif
SERIAL_PROTOCOLLN("");
}
else {
SERIAL_ECHO_START;
SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
}
}
#endif // PIDTEMP
#ifdef PIDTEMPBED
inline void gcode_M304() {
if (code_seen('P')) bedKp = code_value();
if (code_seen('I')) bedKi = scalePID_i(code_value());
if (code_seen('D')) bedKd = scalePID_d(code_value());
updatePID();
SERIAL_PROTOCOL(MSG_OK);
SERIAL_PROTOCOL(" p:");
SERIAL_PROTOCOL(bedKp);
SERIAL_PROTOCOL(" i:");
SERIAL_PROTOCOL(unscalePID_i(bedKi));
SERIAL_PROTOCOL(" d:");
SERIAL_PROTOCOL(unscalePID_d(bedKd));
SERIAL_PROTOCOLLN("");
}
#endif // PIDTEMPBED
#if defined(CHDK) || (defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1)
/**
* M240: Trigger a camera by emulating a Canon RC-1
* See http://www.doc-diy.net/photo/rc-1_hacked/
*/
inline void gcode_M240() {
#ifdef CHDK
OUT_WRITE(CHDK, HIGH);
chdkHigh = millis();
chdkActive = true;
#elif defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
const uint8_t NUM_PULSES = 16;
const float PULSE_LENGTH = 0.01524;
for (int i = 0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
delay(7.33);
for (int i = 0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
#endif // !CHDK && PHOTOGRAPH_PIN > -1
}
#endif // CHDK || PHOTOGRAPH_PIN
#ifdef DOGLCD
/**
* M250: Read and optionally set the LCD contrast
*/
inline void gcode_M250() {
if (code_seen('C')) lcd_setcontrast(code_value_long() & 0x3F);
SERIAL_PROTOCOLPGM("lcd contrast value: ");
SERIAL_PROTOCOL(lcd_contrast);
SERIAL_PROTOCOLLN("");
}
#endif // DOGLCD
#ifdef PREVENT_DANGEROUS_EXTRUDE
/**
* M302: Allow cold extrudes, or set the minimum extrude S<temperature>.
*/
inline void gcode_M302() {
set_extrude_min_temp(code_seen('S') ? code_value() : 0);
}
#endif // PREVENT_DANGEROUS_EXTRUDE
/**
* M303: PID relay autotune
* S<temperature> sets the target temperature. (default target temperature = 150C)
* E<extruder> (-1 for the bed)
* C<cycles>
*/
inline void gcode_M303() {
int e = code_seen('E') ? code_value_long() : 0;
int c = code_seen('C') ? code_value_long() : 5;
float temp = code_seen('S') ? code_value() : (e < 0 ? 70.0 : 150.0);
PID_autotune(temp, e, c);
}
#ifdef SCARA
/**
* M360: SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
*/
inline bool gcode_M360() {
SERIAL_ECHOLN(" Cal: Theta 0 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if (! Stopped) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 0;
delta[Y_AXIS] = 120;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return true;
}
return false;
}
/**
* M361: SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
*/
inline bool gcode_M361() {
SERIAL_ECHOLN(" Cal: Theta 90 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if (! Stopped) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 90;
delta[Y_AXIS] = 130;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return true;
}
return false;
}
/**
* M362: SCARA calibration: Move to cal-position PsiA (0 deg calibration)
*/
inline bool gcode_M362() {
SERIAL_ECHOLN(" Cal: Psi 0 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if (! Stopped) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 60;
delta[Y_AXIS] = 180;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return true;
}
return false;
}
/**
* M363: SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
*/
inline bool gcode_M363() {
SERIAL_ECHOLN(" Cal: Psi 90 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if (! Stopped) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 50;
delta[Y_AXIS] = 90;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return true;
}
return false;
}
/**
* M364: SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
*/
inline bool gcode_M364() {
SERIAL_ECHOLN(" Cal: Theta-Psi 90 ");
// SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if (! Stopped) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 45;
delta[Y_AXIS] = 135;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return true;
}
return false;
}
/**
* M365: SCARA calibration: Scaling factor, X, Y, Z axis
*/
inline void gcode_M365() {
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
if (code_seen(axis_codes[i])) {
axis_scaling[i] = code_value();
}
}
}
#endif // SCARA
#ifdef EXT_SOLENOID
void enable_solenoid(uint8_t num) {
switch(num) {
case 0:
OUT_WRITE(SOL0_PIN, HIGH);
break; break;
#else #if defined(SOL1_PIN) && SOL1_PIN > -1
case 31: // dock the sled case 1:
dock_sled(true); OUT_WRITE(SOL1_PIN, HIGH);
break;
#endif
#if defined(SOL2_PIN) && SOL2_PIN > -1
case 2:
OUT_WRITE(SOL2_PIN, HIGH);
break;
#endif
#if defined(SOL3_PIN) && SOL3_PIN > -1
case 3:
OUT_WRITE(SOL3_PIN, HIGH);
break;
#endif
default:
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_INVALID_SOLENOID);
break; break;
case 32: // undock the sled }
dock_sled(false); }
void enable_solenoid_on_active_extruder() { enable_solenoid(active_extruder); }
void disable_all_solenoids() {
OUT_WRITE(SOL0_PIN, LOW);
OUT_WRITE(SOL1_PIN, LOW);
OUT_WRITE(SOL2_PIN, LOW);
OUT_WRITE(SOL3_PIN, LOW);
}
/**
* M380: Enable solenoid on the active extruder
*/
inline void gcode_M380() { enable_solenoid_on_active_extruder(); }
/**
* M381: Disable all solenoids
*/
inline void gcode_M381() { disable_all_solenoids(); }
#endif // EXT_SOLENOID
/**
* M400: Finish all moves
*/
inline void gcode_M400() { st_synchronize(); }
#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS) && not defined(Z_PROBE_SLED)
/**
* M401: Engage Z Servo endstop if available
*/
inline void gcode_M401() { engage_z_probe(); }
/**
* M402: Retract Z Servo endstop if enabled
*/
inline void gcode_M402() { retract_z_probe(); }
#endif
#ifdef FILAMENT_SENSOR
/**
* M404: Display or set the nominal filament width (3mm, 1.75mm ) N<3.0>
*/
inline void gcode_M404() {
#if FILWIDTH_PIN > -1
if (code_seen('N')) {
filament_width_nominal = code_value();
}
else {
SERIAL_PROTOCOLPGM("Filament dia (nominal mm):");
SERIAL_PROTOCOLLN(filament_width_nominal);
}
#endif
}
/**
* M405: Turn on filament sensor for control
*/
inline void gcode_M405() {
if (code_seen('D')) meas_delay_cm = code_value();
if (meas_delay_cm > MAX_MEASUREMENT_DELAY) meas_delay_cm = MAX_MEASUREMENT_DELAY;
if (delay_index2 == -1) { //initialize the ring buffer if it has not been done since startup
int temp_ratio = widthFil_to_size_ratio();
for (delay_index1 = 0; delay_index1 < MAX_MEASUREMENT_DELAY + 1; ++delay_index1)
measurement_delay[delay_index1] = temp_ratio - 100; //subtract 100 to scale within a signed byte
delay_index1 = delay_index2 = 0;
}
filament_sensor = true;
//SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
//SERIAL_PROTOCOL(filament_width_meas);
//SERIAL_PROTOCOLPGM("Extrusion ratio(%):");
//SERIAL_PROTOCOL(extrudemultiply);
}
/**
* M406: Turn off filament sensor for control
*/
inline void gcode_M406() { filament_sensor = false; }
/**
* M407: Get measured filament diameter on serial output
*/
inline void gcode_M407() {
SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
SERIAL_PROTOCOLLN(filament_width_meas);
}
#endif // FILAMENT_SENSOR
/**
* M500: Store settings in EEPROM
*/
inline void gcode_M500() {
Config_StoreSettings();
}
/**
* M501: Read settings from EEPROM
*/
inline void gcode_M501() {
Config_RetrieveSettings();
}
/**
* M502: Revert to default settings
*/
inline void gcode_M502() {
Config_ResetDefault();
}
/**
* M503: print settings currently in memory
*/
inline void gcode_M503() {
Config_PrintSettings(code_seen('S') && code_value == 0);
}
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
/**
* M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
*/
inline void gcode_M540() {
if (code_seen('S')) abort_on_endstop_hit = (code_value() > 0);
}
#endif // ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
#ifdef CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
inline void gcode_SET_Z_PROBE_OFFSET() {
float value;
if (code_seen('Z')) {
value = code_value();
if (Z_PROBE_OFFSET_RANGE_MIN <= value && value <= Z_PROBE_OFFSET_RANGE_MAX) {
zprobe_zoffset = -value; // compare w/ line 278 of ConfigurationStore.cpp
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " " MSG_OK);
SERIAL_PROTOCOLLN("");
}
else {
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_ZPROBE_ZOFFSET);
SERIAL_ECHOPGM(MSG_Z_MIN);
SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MIN);
SERIAL_ECHOPGM(MSG_Z_MAX);
SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MAX);
SERIAL_PROTOCOLLN("");
}
}
else {
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " : ");
SERIAL_ECHO(-zprobe_zoffset);
SERIAL_PROTOCOLLN("");
}
}
#endif // CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
#ifdef FILAMENTCHANGEENABLE
/**
* M600: Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
*/
inline void gcode_M600() {
float target[NUM_AXIS], lastpos[NUM_AXIS], fr60 = feedrate / 60;
for (int i=0; i<NUM_AXIS; i++)
target[i] = lastpos[i] = current_position[i];
#define BASICPLAN plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr60, active_extruder);
#ifdef DELTA
#define RUNPLAN calculate_delta(target); BASICPLAN
#else
#define RUNPLAN BASICPLAN
#endif
//retract by E
if (code_seen('E')) target[E_AXIS] += code_value();
#ifdef FILAMENTCHANGE_FIRSTRETRACT
else target[E_AXIS] += FILAMENTCHANGE_FIRSTRETRACT;
#endif
RUNPLAN;
//lift Z
if (code_seen('Z')) target[Z_AXIS] += code_value();
#ifdef FILAMENTCHANGE_ZADD
else target[Z_AXIS] += FILAMENTCHANGE_ZADD;
#endif
RUNPLAN;
//move xy
if (code_seen('X')) target[X_AXIS] = code_value();
#ifdef FILAMENTCHANGE_XPOS
else target[X_AXIS] = FILAMENTCHANGE_XPOS;
#endif
if (code_seen('Y')) target[Y_AXIS] = code_value();
#ifdef FILAMENTCHANGE_YPOS
else target[Y_AXIS] = FILAMENTCHANGE_YPOS;
#endif
RUNPLAN;
if (code_seen('L')) target[E_AXIS] += code_value();
#ifdef FILAMENTCHANGE_FINALRETRACT
else target[E_AXIS] += FILAMENTCHANGE_FINALRETRACT;
#endif
RUNPLAN;
//finish moves
st_synchronize();
//disable extruder steppers so filament can be removed
disable_e0();
disable_e1();
disable_e2();
disable_e3();
delay(100);
LCD_ALERTMESSAGEPGM(MSG_FILAMENTCHANGE);
uint8_t cnt = 0;
while (!lcd_clicked()) {
cnt++;
manage_heater();
manage_inactivity(true);
lcd_update();
if (cnt == 0) {
#if BEEPER > 0
OUT_WRITE(BEEPER,HIGH);
delay(3);
WRITE(BEEPER,LOW);
delay(3);
#else
#if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
lcd_buzz(1000/6, 100);
#else
lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
#endif
#endif
}
} // while(!lcd_clicked)
//return to normal
if (code_seen('L')) target[E_AXIS] -= code_value();
#ifdef FILAMENTCHANGE_FINALRETRACT
else target[E_AXIS] -= FILAMENTCHANGE_FINALRETRACT;
#endif
current_position[E_AXIS] = target[E_AXIS]; //the long retract of L is compensated by manual filament feeding
plan_set_e_position(current_position[E_AXIS]);
RUNPLAN; //should do nothing
lcd_reset_alert_level();
#ifdef DELTA
calculate_delta(lastpos);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move xyz back
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], fr60, active_extruder); //final untretract
#else
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move xy back
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move z back
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], fr60, active_extruder); //final untretract
#endif
}
#endif // FILAMENTCHANGEENABLE
#ifdef DUAL_X_CARRIAGE
/**
* M605: Set dual x-carriage movement mode
*
* M605 S0: Full control mode. The slicer has full control over x-carriage movement
* M605 S1: Auto-park mode. The inactive head will auto park/unpark without slicer involvement
* M605 S2 [Xnnn] [Rmmm]: Duplication mode. The second extruder will duplicate the first with nnn
* millimeters x-offset and an optional differential hotend temperature of
* mmm degrees. E.g., with "M605 S2 X100 R2" the second extruder will duplicate
* the first with a spacing of 100mm in the x direction and 2 degrees hotter.
*
* Note: the X axis should be homed after changing dual x-carriage mode.
*/
inline void gcode_M605() {
st_synchronize();
if (code_seen('S')) dual_x_carriage_mode = code_value();
switch(dual_x_carriage_mode) {
case DXC_DUPLICATION_MODE:
if (code_seen('X')) duplicate_extruder_x_offset = max(code_value(), X2_MIN_POS - x_home_pos(0));
if (code_seen('R')) duplicate_extruder_temp_offset = code_value();
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
SERIAL_ECHO(" ");
SERIAL_ECHO(extruder_offset[X_AXIS][0]);
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
SERIAL_ECHO(" ");
SERIAL_ECHO(duplicate_extruder_x_offset);
SERIAL_ECHO(",");
SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
break; break;
#endif // Z_PROBE_SLED case DXC_FULL_CONTROL_MODE:
#endif // ENABLE_AUTO_BED_LEVELING case DXC_AUTO_PARK_MODE:
break;
default:
dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
break;
}
active_extruder_parked = false;
extruder_duplication_enabled = false;
delayed_move_time = 0;
}
#endif // DUAL_X_CARRIAGE
/**
* M907: Set digital trimpot motor current using axis codes X, Y, Z, E, B, S
*/
inline void gcode_M907() {
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
for (int i=0;i<NUM_AXIS;i++)
if (code_seen(axis_codes[i])) digipot_current(i, code_value());
if (code_seen('B')) digipot_current(4, code_value());
if (code_seen('S')) for (int i=0; i<=4; i++) digipot_current(i, code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_XY_PIN
if (code_seen('X')) digipot_current(0, code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_Z_PIN
if (code_seen('Z')) digipot_current(1, code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_E_PIN
if (code_seen('E')) digipot_current(2, code_value());
#endif
#ifdef DIGIPOT_I2C
// this one uses actual amps in floating point
for (int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value());
// for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
for (int i=NUM_AXIS;i<DIGIPOT_I2C_NUM_CHANNELS;i++) if(code_seen('B'+i-NUM_AXIS)) digipot_i2c_set_current(i, code_value());
#endif
}
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
/**
* M908: Control digital trimpot directly (M908 P<pin> S<current>)
*/
inline void gcode_M908() {
digitalPotWrite(
code_seen('P') ? code_value() : 0,
code_seen('S') ? code_value() : 0
);
}
#endif // DIGIPOTSS_PIN
// M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
inline void gcode_M350() {
#if defined(X_MS1_PIN) && X_MS1_PIN > -1
if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
if(code_seen('B')) microstep_mode(4,code_value());
microstep_readings();
#endif
}
/**
* M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
* S# determines MS1 or MS2, X# sets the pin high/low.
*/
inline void gcode_M351() {
#if defined(X_MS1_PIN) && X_MS1_PIN > -1
if (code_seen('S')) switch((int)code_value()) {
case 1:
for(int i=0;i<NUM_AXIS;i++) if (code_seen(axis_codes[i])) microstep_ms(i, code_value(), -1);
if (code_seen('B')) microstep_ms(4, code_value(), -1);
break;
case 2:
for(int i=0;i<NUM_AXIS;i++) if (code_seen(axis_codes[i])) microstep_ms(i, -1, code_value());
if (code_seen('B')) microstep_ms(4, -1, code_value());
break;
}
microstep_readings();
#endif
}
/**
* M999: Restart after being stopped
*/
inline void gcode_M999() {
Stopped = false;
lcd_reset_alert_level();
gcode_LastN = Stopped_gcode_LastN;
FlushSerialRequestResend();
}
inline void gcode_T() {
tmp_extruder = code_value();
if (tmp_extruder >= EXTRUDERS) {
SERIAL_ECHO_START;
SERIAL_ECHO("T");
SERIAL_ECHO(tmp_extruder);
SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
}
else {
boolean make_move = false;
if (code_seen('F')) {
make_move = true;
next_feedrate = code_value();
if (next_feedrate > 0.0) feedrate = next_feedrate;
}
#if EXTRUDERS > 1
if (tmp_extruder != active_extruder) {
// Save current position to return to after applying extruder offset
memcpy(destination, current_position, sizeof(destination));
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && Stopped == false &&
(delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder))) {
// Park old head: 1) raise 2) move to park position 3) lower
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[X_AXIS], active_extruder);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS],
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
st_synchronize();
}
// apply Y & Z extruder offset (x offset is already used in determining home pos)
current_position[Y_AXIS] = current_position[Y_AXIS] -
extruder_offset[Y_AXIS][active_extruder] +
extruder_offset[Y_AXIS][tmp_extruder];
current_position[Z_AXIS] = current_position[Z_AXIS] -
extruder_offset[Z_AXIS][active_extruder] +
extruder_offset[Z_AXIS][tmp_extruder];
active_extruder = tmp_extruder;
// This function resets the max/min values - the current position may be overwritten below.
axis_is_at_home(X_AXIS);
if (dual_x_carriage_mode == DXC_FULL_CONTROL_MODE) {
current_position[X_AXIS] = inactive_extruder_x_pos;
inactive_extruder_x_pos = destination[X_AXIS];
}
else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
active_extruder_parked = (active_extruder == 0); // this triggers the second extruder to move into the duplication position
if (active_extruder == 0 || active_extruder_parked)
current_position[X_AXIS] = inactive_extruder_x_pos;
else
current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset;
inactive_extruder_x_pos = destination[X_AXIS];
extruder_duplication_enabled = false;
}
else {
// record raised toolhead position for use by unpark
memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
raised_parked_position[Z_AXIS] += TOOLCHANGE_UNPARK_ZLIFT;
active_extruder_parked = true;
delayed_move_time = 0;
}
#else // !DUAL_X_CARRIAGE
// Offset extruder (only by XY)
for (int i=X_AXIS; i<=Y_AXIS; i++)
current_position[i] += extruder_offset[i][tmp_extruder] - extruder_offset[i][active_extruder];
// Set the new active extruder and position
active_extruder = tmp_extruder;
#endif // !DUAL_X_CARRIAGE
#ifdef DELTA
calculate_delta(current_position); // change cartesian kinematic to delta kinematic;
//sent position to plan_set_position();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]);
#else
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif
// Move to the old position if 'F' was in the parameters
if (make_move && !Stopped) prepare_move();
}
#ifdef EXT_SOLENOID
st_synchronize();
disable_all_solenoids();
enable_solenoid_on_active_extruder();
#endif // EXT_SOLENOID
#endif // EXTRUDERS > 1
SERIAL_ECHO_START;
SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
SERIAL_PROTOCOLLN((int)active_extruder);
}
}
/**
* Process Commands and dispatch them to handlers
*/
void process_commands() {
if (code_seen('G')) {
int gCode = code_value_long();
switch(gCode) {
// G0, G1
case 0:
case 1:
gcode_G0_G1();
break;
// G2, G3
#ifndef SCARA
case 2: // G2 - CW ARC
case 3: // G3 - CCW ARC
gcode_G2_G3(gCode == 2);
break;
#endif
// G4 Dwell
case 4:
gcode_G4();
break;
#ifdef FWRETRACT
case 10: // G10: retract
case 11: // G11: retract_recover
gcode_G10_G11(gCode == 10);
break;
#endif //FWRETRACT
case 28: // G28: Home all axes, one at a time
gcode_G28();
break;
#ifdef ENABLE_AUTO_BED_LEVELING
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
gcode_G29();
break;
#ifndef Z_PROBE_SLED
case 30: // G30 Single Z Probe
gcode_G30();
break;
#else // Z_PROBE_SLED
case 31: // G31: dock the sled
case 32: // G32: undock the sled
dock_sled(gCode == 31);
break;
#endif // Z_PROBE_SLED
#endif // ENABLE_AUTO_BED_LEVELING
case 90: // G90 case 90: // G90
relative_mode = false; relative_mode = false;
break; break;
case 91: // G91 case 91: // G91
relative_mode = true; relative_mode = true;
break; break;
case 92: // G92 case 92: // G92
if(!code_seen(axis_codes[E_AXIS])) gcode_G92();
st_synchronize();
for(int8_t i=0; i < NUM_AXIS; i++) {
if(code_seen(axis_codes[i])) {
if(i == E_AXIS) {
current_position[i] = code_value();
plan_set_e_position(current_position[E_AXIS]);
}
else {
#ifdef SCARA
if (i == X_AXIS || i == Y_AXIS) {
current_position[i] = code_value();
}
else {
current_position[i] = code_value()+add_homing[i];
}
#else
current_position[i] = code_value()+add_homing[i];
#endif
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
}
}
break; break;
} }
} }
else if(code_seen('M')) else if (code_seen('M')) {
{ switch( (int)code_value() ) {
switch( (int)code_value() ) #ifdef ULTIPANEL
{ case 0: // M0 - Unconditional stop - Wait for user button press on LCD
#ifdef ULTIPANEL case 1: // M1 - Conditional stop - Wait for user button press on LCD
case 0: // M0 - Unconditional stop - Wait for user button press on LCD gcode_M0_M1();
case 1: // M1 - Conditional stop - Wait for user button press on LCD break;
{ #endif // ULTIPANEL
char *src = strchr_pointer + 2;
codenum = 0; case 17:
gcode_M17();
break;
bool hasP = false, hasS = false; #ifdef SDSUPPORT
if (code_seen('P')) {
codenum = code_value(); // milliseconds to wait
hasP = codenum > 0;
}
if (code_seen('S')) {
codenum = code_value() * 1000; // seconds to wait
hasS = codenum > 0;
}
starpos = strchr(src, '*');
if (starpos != NULL) *(starpos) = '\0';
while (*src == ' ') ++src;
if (!hasP && !hasS && *src != '\0') {
lcd_setstatus(src);
} else {
LCD_MESSAGEPGM(MSG_USERWAIT);
}
lcd_ignore_click(); case 20: // M20 - list SD card
st_synchronize(); gcode_M20(); break;
previous_millis_cmd = millis(); case 21: // M21 - init SD card
if (codenum > 0){ gcode_M21(); break;
codenum += millis(); // keep track of when we started waiting case 22: //M22 - release SD card
while(millis() < codenum && !lcd_clicked()){ gcode_M22(); break;
manage_heater(); case 23: //M23 - Select file
manage_inactivity(); gcode_M23(); break;
lcd_update(); case 24: //M24 - Start SD print
} gcode_M24(); break;
lcd_ignore_click(false); case 25: //M25 - Pause SD print
}else{ gcode_M25(); break;
if (!lcd_detected()) case 26: //M26 - Set SD index
break; gcode_M26(); break;
while(!lcd_clicked()){ case 27: //M27 - Get SD status
manage_heater(); gcode_M27(); break;
manage_inactivity(); case 28: //M28 - Start SD write
lcd_update(); gcode_M28(); break;
} case 29: //M29 - Stop SD write
} gcode_M29(); break;
if (IS_SD_PRINTING) case 30: //M30 <filename> Delete File
LCD_MESSAGEPGM(MSG_RESUMING); gcode_M30(); break;
else case 32: //M32 - Select file and start SD print
LCD_MESSAGEPGM(WELCOME_MSG); gcode_M32(); break;
} case 928: //M928 - Start SD write
break; gcode_M928(); break;
#endif
case 17:
LCD_MESSAGEPGM(MSG_NO_MOVE);
enable_x();
enable_y();
enable_z();
enable_e0();
enable_e1();
enable_e2();
break;
#ifdef SDSUPPORT #endif //SDSUPPORT
case 20: // M20 - list SD card
SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
card.ls();
SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
break;
case 21: // M21 - init SD card
card.initsd(); case 31: //M31 take time since the start of the SD print or an M109 command
gcode_M31();
break;
break; case 42: //M42 -Change pin status via gcode
case 22: //M22 - release SD card gcode_M42();
card.release(); break;
break; #if defined(ENABLE_AUTO_BED_LEVELING) && defined(Z_PROBE_REPEATABILITY_TEST)
case 23: //M23 - Select file case 48: // M48 Z-Probe repeatability
starpos = (strchr(strchr_pointer + 4,'*')); gcode_M48();
if(starpos!=NULL) break;
*(starpos)='\0'; #endif // ENABLE_AUTO_BED_LEVELING && Z_PROBE_REPEATABILITY_TEST
card.openFile(strchr_pointer + 4,true);
break;
case 24: //M24 - Start SD print
card.startFileprint();
starttime=millis();
break;
case 25: //M25 - Pause SD print
card.pauseSDPrint();
break;
case 26: //M26 - Set SD index
if(card.cardOK && code_seen('S')) {
card.setIndex(code_value_long());
}
break;
case 27: //M27 - Get SD status
card.getStatus();
break;
case 28: //M28 - Start SD write
starpos = (strchr(strchr_pointer + 4,'*'));
if(starpos != NULL){
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos,' ') + 1;
*(starpos) = '\0';
}
card.openFile(strchr_pointer+4,false);
break;
case 29: //M29 - Stop SD write
//processed in write to file routine above
//card,saving = false;
break;
case 30: //M30 <filename> Delete File
if (card.cardOK){
card.closefile();
starpos = (strchr(strchr_pointer + 4,'*'));
if(starpos != NULL){
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos,' ') + 1;
*(starpos) = '\0';
}
card.removeFile(strchr_pointer + 4);
}
break;
case 32: //M32 - Select file and start SD print
{
if(card.sdprinting) {
st_synchronize();
} case 104: // M104
starpos = (strchr(strchr_pointer + 4,'*')); gcode_M104();
break;
char* namestartpos = (strchr(strchr_pointer + 4,'!')); //find ! to indicate filename string start. case 112: // M112 Emergency Stop
if(namestartpos==NULL) gcode_M112();
{ break;
namestartpos=strchr_pointer + 4; //default name position, 4 letters after the M
}
else
namestartpos++; //to skip the '!'
if(starpos!=NULL) case 140: // M140 Set bed temp
*(starpos)='\0'; gcode_M140();
break;
bool call_procedure=(code_seen('P')); case 105: // M105 Read current temperature
gcode_M105();
return;
break;
if(strchr_pointer>namestartpos) case 109: // M109 Wait for temperature
call_procedure=false; //false alert, 'P' found within filename gcode_M109();
break;
if( card.cardOK ) #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
{ case 190: // M190 - Wait for bed heater to reach target.
card.openFile(namestartpos,true,!call_procedure); gcode_M190();
if(code_seen('S')) break;
if(strchr_pointer<namestartpos) //only if "S" is occuring _before_ the filename #endif //TEMP_BED_PIN
card.setIndex(code_value_long());
card.startFileprint();
if(!call_procedure)
starttime=millis(); //procedure calls count as normal print time.
}
} break;
case 928: //M928 - Start SD write
starpos = (strchr(strchr_pointer + 5,'*'));
if(starpos != NULL){
char* npos = strchr(cmdbuffer[bufindr], 'N');
strchr_pointer = strchr(npos,' ') + 1;
*(starpos) = '\0';
}
card.openLogFile(strchr_pointer+5);
break;
#endif //SDSUPPORT
case 31: //M31 take time since the start of the SD print or an M109 command
{
stoptime=millis();
char time[30];
unsigned long t=(stoptime-starttime)/1000;
int sec,min;
min=t/60;
sec=t%60;
sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
SERIAL_ECHO_START;
SERIAL_ECHOLN(time);
lcd_setstatus(time);
autotempShutdown();
}
break;
case 42: //M42 -Change pin status via gcode
if (code_seen('S'))
{
int pin_status = code_value();
int pin_number = LED_PIN;
if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
pin_number = code_value();
for(int8_t i = 0; i < (int8_t)(sizeof(sensitive_pins)/sizeof(int)); i++)
{
if (sensitive_pins[i] == pin_number)
{
pin_number = -1;
break;
}
}
#if defined(FAN_PIN) && FAN_PIN > -1 #if defined(FAN_PIN) && FAN_PIN > -1
if (pin_number == FAN_PIN) case 106: //M106 Fan On
fanSpeed = pin_status; gcode_M106();
#endif
if (pin_number > -1)
{
pinMode(pin_number, OUTPUT);
digitalWrite(pin_number, pin_status);
analogWrite(pin_number, pin_status);
}
}
break;
// M48 Z-Probe repeatability measurement function.
//
// Usage: M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <Engage_probe_for_each_reading> <L legs_of_movement_prior_to_doing_probe>
//
// This function assumes the bed has been homed. Specificaly, that a G28 command
// as been issued prior to invoking the M48 Z-Probe repeatability measurement function.
// Any information generated by a prior G29 Bed leveling command will be lost and need to be
// regenerated.
//
// The number of samples will default to 10 if not specified. You can use upper or lower case
// letters for any of the options EXCEPT n. n must be in lower case because Marlin uses a capital
// N for its communication protocol and will get horribly confused if you send it a capital N.
//
#ifdef ENABLE_AUTO_BED_LEVELING
#ifdef Z_PROBE_REPEATABILITY_TEST
case 48: // M48 Z-Probe repeatability
{
#if Z_MIN_PIN == -1
#error "You must have a Z_MIN endstop in order to enable calculation of Z-Probe repeatability."
#endif
double sum=0.0;
double mean=0.0;
double sigma=0.0;
double sample_set[50];
int verbose_level=1, n=0, j, n_samples = 10, n_legs=0, engage_probe_for_each_reading=0 ;
double X_current, Y_current, Z_current;
double X_probe_location, Y_probe_location, Z_start_location, ext_position;
if (code_seen('V') || code_seen('v')) {
verbose_level = code_value();
if (verbose_level<0 || verbose_level>4 ) {
SERIAL_PROTOCOLPGM("?Verbose Level not plausable.\n");
goto Sigma_Exit;
}
}
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("M48 Z-Probe Repeatability test. Version 2.00\n");
SERIAL_PROTOCOLPGM("Full support at: http://3dprintboard.com/forum.php\n");
}
if (code_seen('n')) {
n_samples = code_value();
if (n_samples<4 || n_samples>50 ) {
SERIAL_PROTOCOLPGM("?Specified sample size not plausable.\n");
goto Sigma_Exit;
}
}
X_current = X_probe_location = st_get_position_mm(X_AXIS);
Y_current = Y_probe_location = st_get_position_mm(Y_AXIS);
Z_current = st_get_position_mm(Z_AXIS);
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
ext_position = st_get_position_mm(E_AXIS);
if (code_seen('E') || code_seen('e') )
engage_probe_for_each_reading++;
if (code_seen('X') || code_seen('x') ) {
X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER;
if (X_probe_location<X_MIN_POS || X_probe_location>X_MAX_POS ) {
SERIAL_PROTOCOLPGM("?Specified X position out of range.\n");
goto Sigma_Exit;
}
}
if (code_seen('Y') || code_seen('y') ) {
Y_probe_location = code_value() - Y_PROBE_OFFSET_FROM_EXTRUDER;
if (Y_probe_location<Y_MIN_POS || Y_probe_location>Y_MAX_POS ) {
SERIAL_PROTOCOLPGM("?Specified Y position out of range.\n");
goto Sigma_Exit;
}
}
if (code_seen('L') || code_seen('l') ) {
n_legs = code_value();
if ( n_legs==1 )
n_legs = 2;
if ( n_legs<0 || n_legs>15 ) {
SERIAL_PROTOCOLPGM("?Specified number of legs in movement not plausable.\n");
goto Sigma_Exit;
}
}
//
// Do all the preliminary setup work. First raise the probe.
//
st_synchronize();
plan_bed_level_matrix.set_to_identity();
plan_buffer_line( X_current, Y_current, Z_start_location,
ext_position,
homing_feedrate[Z_AXIS]/60,
active_extruder);
st_synchronize();
//
// Now get everything to the specified probe point So we can safely do a probe to
// get us close to the bed. If the Z-Axis is far from the bed, we don't want to
// use that as a starting point for each probe.
//
if (verbose_level > 2)
SERIAL_PROTOCOL("Positioning probe for the test.\n");
plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
ext_position,
homing_feedrate[X_AXIS]/60,
active_extruder);
st_synchronize();
current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS);
current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS);
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
current_position[E_AXIS] = ext_position = st_get_position_mm(E_AXIS);
//
// OK, do the inital probe to get us close to the bed.
// Then retrace the right amount and use that in subsequent probes
//
engage_z_probe();
setup_for_endstop_move();
run_z_probe();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
ext_position,
homing_feedrate[X_AXIS]/60,
active_extruder);
st_synchronize();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
if (engage_probe_for_each_reading)
retract_z_probe();
for( n=0; n<n_samples; n++) {
do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
if ( n_legs) {
double radius=0.0, theta=0.0, x_sweep, y_sweep;
int rotational_direction, l;
rotational_direction = (unsigned long) millis() & 0x0001; // clockwise or counter clockwise
radius = (unsigned long) millis() % (long) (X_MAX_LENGTH/4); // limit how far out to go
theta = (float) ((unsigned long) millis() % (long) 360) / (360./(2*3.1415926)); // turn into radians
//SERIAL_ECHOPAIR("starting radius: ",radius);
//SERIAL_ECHOPAIR(" theta: ",theta);
//SERIAL_ECHOPAIR(" direction: ",rotational_direction);
//SERIAL_PROTOCOLLNPGM("");
for( l=0; l<n_legs-1; l++) {
if (rotational_direction==1)
theta += (float) ((unsigned long) millis() % (long) 20) / (360.0/(2*3.1415926)); // turn into radians
else
theta -= (float) ((unsigned long) millis() % (long) 20) / (360.0/(2*3.1415926)); // turn into radians
radius += (float) ( ((long) ((unsigned long) millis() % (long) 10)) - 5);
if ( radius<0.0 )
radius = -radius;
X_current = X_probe_location + cos(theta) * radius;
Y_current = Y_probe_location + sin(theta) * radius;
if ( X_current<X_MIN_POS) // Make sure our X & Y are sane
X_current = X_MIN_POS;
if ( X_current>X_MAX_POS)
X_current = X_MAX_POS;
if ( Y_current<Y_MIN_POS) // Make sure our X & Y are sane
Y_current = Y_MIN_POS;
if ( Y_current>Y_MAX_POS)
Y_current = Y_MAX_POS;
if (verbose_level>3 ) {
SERIAL_ECHOPAIR("x: ", X_current);
SERIAL_ECHOPAIR("y: ", Y_current);
SERIAL_PROTOCOLLNPGM("");
}
do_blocking_move_to( X_current, Y_current, Z_current );
}
do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
}
if (engage_probe_for_each_reading) {
engage_z_probe();
delay(1000);
}
setup_for_endstop_move();
run_z_probe();
sample_set[n] = current_position[Z_AXIS];
//
// Get the current mean for the data points we have so far
//
sum=0.0;
for( j=0; j<=n; j++) {
sum = sum + sample_set[j];
}
mean = sum / (double (n+1));
//
// Now, use that mean to calculate the standard deviation for the
// data points we have so far
//
sum=0.0;
for( j=0; j<=n; j++) {
sum = sum + (sample_set[j]-mean) * (sample_set[j]-mean);
}
sigma = sqrt( sum / (double (n+1)) );
if (verbose_level > 1) {
SERIAL_PROTOCOL(n+1);
SERIAL_PROTOCOL(" of ");
SERIAL_PROTOCOL(n_samples);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6);
}
if (verbose_level > 2) {
SERIAL_PROTOCOL(" mean: ");
SERIAL_PROTOCOL_F(mean,6);
SERIAL_PROTOCOL(" sigma: ");
SERIAL_PROTOCOL_F(sigma,6);
}
if (verbose_level > 0)
SERIAL_PROTOCOLPGM("\n");
plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location,
current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
st_synchronize();
if (engage_probe_for_each_reading) {
retract_z_probe();
delay(1000);
}
}
retract_z_probe();
delay(1000);
clean_up_after_endstop_move();
// enable_endstops(true);
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("Mean: ");
SERIAL_PROTOCOL_F(mean, 6);
SERIAL_PROTOCOLPGM("\n");
}
SERIAL_PROTOCOLPGM("Standard Deviation: ");
SERIAL_PROTOCOL_F(sigma, 6);
SERIAL_PROTOCOLPGM("\n\n");
Sigma_Exit:
break;
}
#endif // Z_PROBE_REPEATABILITY_TEST
#endif // ENABLE_AUTO_BED_LEVELING
case 104: // M104
if(setTargetedHotend(104)){
break;
}
if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
#endif
setWatch();
break;
case 112: // M112 -Emergency Stop
kill();
break;
case 140: // M140 set bed temp
if (code_seen('S')) setTargetBed(code_value());
break;
case 105 : // M105
if(setTargetedHotend(105)){
break;
}
#if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
SERIAL_PROTOCOLPGM("ok T:");
SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
SERIAL_PROTOCOLPGM(" B:");
SERIAL_PROTOCOL_F(degBed(),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetBed(),1);
#endif //TEMP_BED_PIN
for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
SERIAL_PROTOCOLPGM(" T");
SERIAL_PROTOCOL(cur_extruder);
SERIAL_PROTOCOLPGM(":");
SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
SERIAL_PROTOCOLPGM(" /");
SERIAL_PROTOCOL_F(degTargetHotend(cur_extruder),1);
}
#else
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
#endif
SERIAL_PROTOCOLPGM(" @:");
#ifdef EXTRUDER_WATTS
SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(tmp_extruder))/127);
SERIAL_PROTOCOLPGM("W");
#else
SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
#endif
SERIAL_PROTOCOLPGM(" B@:");
#ifdef BED_WATTS
SERIAL_PROTOCOL((BED_WATTS * getHeaterPower(-1))/127);
SERIAL_PROTOCOLPGM("W");
#else
SERIAL_PROTOCOL(getHeaterPower(-1));
#endif
#ifdef SHOW_TEMP_ADC_VALUES
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
SERIAL_PROTOCOLPGM(" ADC B:");
SERIAL_PROTOCOL_F(degBed(),1);
SERIAL_PROTOCOLPGM("C->");
SERIAL_PROTOCOL_F(rawBedTemp()/OVERSAMPLENR,0);
#endif
for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
SERIAL_PROTOCOLPGM(" T");
SERIAL_PROTOCOL(cur_extruder);
SERIAL_PROTOCOLPGM(":");
SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
SERIAL_PROTOCOLPGM("C->");
SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
}
#endif
SERIAL_PROTOCOLLN("");
return;
break;
case 109:
{// M109 - Wait for extruder heater to reach target.
if(setTargetedHotend(109)){
break;
}
LCD_MESSAGEPGM(MSG_HEATING);
#ifdef AUTOTEMP
autotemp_enabled=false;
#endif
if (code_seen('S')) {
setTargetHotend(code_value(), tmp_extruder);
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
#endif
CooldownNoWait = true;
} else if (code_seen('R')) {
setTargetHotend(code_value(), tmp_extruder);
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
#endif
CooldownNoWait = false;
}
#ifdef AUTOTEMP
if (code_seen('S')) autotemp_min=code_value();
if (code_seen('B')) autotemp_max=code_value();
if (code_seen('F'))
{
autotemp_factor=code_value();
autotemp_enabled=true;
}
#endif
setWatch();
codenum = millis();
/* See if we are heating up or cooling down */
target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
cancel_heatup = false;
#ifdef TEMP_RESIDENCY_TIME
long residencyStart;
residencyStart = -1;
/* continue to loop until we have reached the target temp
_and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
while((!cancel_heatup)&&((residencyStart == -1) ||
(residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) {
#else
while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
#endif //TEMP_RESIDENCY_TIME
if( (millis() - codenum) > 1000UL )
{ //Print Temp Reading and remaining time every 1 second while heating up/cooling down
SERIAL_PROTOCOLPGM("T:");
SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL((int)tmp_extruder);
#ifdef TEMP_RESIDENCY_TIME
SERIAL_PROTOCOLPGM(" W:");
if(residencyStart > -1)
{
codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
SERIAL_PROTOCOLLN( codenum );
}
else
{
SERIAL_PROTOCOLLN( "?" );
}
#else
SERIAL_PROTOCOLLN("");
#endif
codenum = millis();
}
manage_heater();
manage_inactivity();
lcd_update();
#ifdef TEMP_RESIDENCY_TIME
/* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
or when current temp falls outside the hysteresis after target temp was reached */
if ((residencyStart == -1 && target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
(residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
(residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
{
residencyStart = millis();
}
#endif //TEMP_RESIDENCY_TIME
}
LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
starttime=millis();
previous_millis_cmd = millis();
}
break;
case 190: // M190 - Wait for bed heater to reach target.
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
LCD_MESSAGEPGM(MSG_BED_HEATING);
if (code_seen('S')) {
setTargetBed(code_value());
CooldownNoWait = true;
} else if (code_seen('R')) {
setTargetBed(code_value());
CooldownNoWait = false;
}
codenum = millis();
cancel_heatup = false;
target_direction = isHeatingBed(); // true if heating, false if cooling
while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) )
{
if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
{
float tt=degHotend(active_extruder);
SERIAL_PROTOCOLPGM("T:");
SERIAL_PROTOCOL(tt);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL((int)active_extruder);
SERIAL_PROTOCOLPGM(" B:");
SERIAL_PROTOCOL_F(degBed(),1);
SERIAL_PROTOCOLLN("");
codenum = millis();
}
manage_heater();
manage_inactivity();
lcd_update();
}
LCD_MESSAGEPGM(MSG_BED_DONE);
previous_millis_cmd = millis();
#endif
break;
#if defined(FAN_PIN) && FAN_PIN > -1
case 106: //M106 Fan On
if (code_seen('S')){
fanSpeed=constrain(code_value(),0,255);
}
else {
fanSpeed=255;
}
break;
case 107: //M107 Fan Off
fanSpeed = 0;
break;
#endif //FAN_PIN
#ifdef BARICUDA
// PWM for HEATER_1_PIN
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
case 126: //M126 valve open
if (code_seen('S')){
ValvePressure=constrain(code_value(),0,255);
}
else {
ValvePressure=255;
}
break; break;
case 127: //M127 valve closed case 107: //M107 Fan Off
ValvePressure = 0; gcode_M107();
break; break;
#endif //HEATER_1_PIN #endif //FAN_PIN
// PWM for HEATER_2_PIN #ifdef BARICUDA
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 // PWM for HEATER_1_PIN
case 128: //M128 valve open #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
if (code_seen('S')){ case 126: // M126 valve open
EtoPPressure=constrain(code_value(),0,255); gcode_M126();
} break;
else { case 127: // M127 valve closed
EtoPPressure=255; gcode_M127();
} break;
#endif //HEATER_1_PIN
// PWM for HEATER_2_PIN
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
case 128: // M128 valve open
gcode_M128();
break;
case 129: // M129 valve closed
gcode_M129();
break;
#endif //HEATER_2_PIN
#endif //BARICUDA
#if defined(PS_ON_PIN) && PS_ON_PIN > -1
case 80: // M80 - Turn on Power Supply
gcode_M80();
break; break;
case 129: //M129 valve closed
EtoPPressure = 0;
break;
#endif //HEATER_2_PIN
#endif
#if defined(PS_ON_PIN) && PS_ON_PIN > -1 #endif // PS_ON_PIN
case 80: // M80 - Turn on Power Supply
SET_OUTPUT(PS_ON_PIN); //GND
WRITE(PS_ON_PIN, PS_ON_AWAKE);
// If you have a switch on suicide pin, this is useful
// if you want to start another print with suicide feature after
// a print without suicide...
#if defined SUICIDE_PIN && SUICIDE_PIN > -1
SET_OUTPUT(SUICIDE_PIN);
WRITE(SUICIDE_PIN, HIGH);
#endif
#ifdef ULTIPANEL
powersupply = true;
LCD_MESSAGEPGM(WELCOME_MSG);
lcd_update();
#endif
break;
#endif
case 81: // M81 - Turn off Power Supply case 81: // M81 - Turn off Power Supply
disable_heater(); gcode_M81();
st_synchronize();
disable_e0();
disable_e1();
disable_e2();
finishAndDisableSteppers();
fanSpeed = 0;
delay(1000); // Wait a little before to switch off
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
st_synchronize();
suicide();
#elif defined(PS_ON_PIN) && PS_ON_PIN > -1
SET_OUTPUT(PS_ON_PIN);
WRITE(PS_ON_PIN, PS_ON_ASLEEP);
#endif
#ifdef ULTIPANEL
powersupply = false;
LCD_MESSAGEPGM(MACHINE_NAME" "MSG_OFF".");
lcd_update();
#endif
break;
case 82:
axis_relative_modes[3] = false;
break;
case 83:
axis_relative_modes[3] = true;
break;
case 18: //compatibility
case 84: // M84
if(code_seen('S')){
stepper_inactive_time = code_value() * 1000;
}
else
{
bool all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))|| (code_seen(axis_codes[E_AXIS])));
if(all_axis)
{
st_synchronize();
disable_e0();
disable_e1();
disable_e2();
finishAndDisableSteppers();
}
else
{
st_synchronize();
if(code_seen('X')) disable_x();
if(code_seen('Y')) disable_y();
if(code_seen('Z')) disable_z();
#if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
if(code_seen('E')) {
disable_e0();
disable_e1();
disable_e2();
}
#endif
}
}
break;
case 85: // M85
if(code_seen('S')) {
max_inactive_time = code_value() * 1000;
}
break;
case 92: // M92
for(int8_t i=0; i < NUM_AXIS; i++)
{
if(code_seen(axis_codes[i]))
{
if(i == 3) { // E
float value = code_value();
if(value < 20.0) {
float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
max_e_jerk *= factor;
max_feedrate[i] *= factor;
axis_steps_per_sqr_second[i] *= factor;
}
axis_steps_per_unit[i] = value;
}
else {
axis_steps_per_unit[i] = code_value();
}
}
}
break;
case 115: // M115
SERIAL_PROTOCOLPGM(MSG_M115_REPORT);
break;
case 117: // M117 display message
starpos = (strchr(strchr_pointer + 5,'*'));
if(starpos!=NULL)
*(starpos)='\0';
lcd_setstatus(strchr_pointer + 5);
break;
case 114: // M114
SERIAL_PROTOCOLPGM("X:");
SERIAL_PROTOCOL(current_position[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y:");
SERIAL_PROTOCOL(current_position[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z:");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM(" E:");
SERIAL_PROTOCOL(current_position[E_AXIS]);
SERIAL_PROTOCOLPGM(MSG_COUNT_X);
SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y:");
SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z:");
SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
SERIAL_PROTOCOLLN("");
#ifdef SCARA
SERIAL_PROTOCOLPGM("SCARA Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta:");
SERIAL_PROTOCOL(delta[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]+add_homing[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta (90):");
SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+add_homing[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta:");
SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]);
SERIAL_PROTOCOLLN("");
SERIAL_PROTOCOLLN("");
#endif
break;
case 120: // M120
enable_endstops(false) ;
break;
case 121: // M121
enable_endstops(true) ;
break;
case 119: // M119
SERIAL_PROTOCOLLN(MSG_M119_REPORT);
#if defined(X_MIN_PIN) && X_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_X_MIN);
SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(X_MAX_PIN) && X_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_X_MAX);
SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Y_MIN);
SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Y_MAX);
SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Z_MIN);
SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
#if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
SERIAL_PROTOCOLPGM(MSG_Z_MAX);
SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
#endif
break;
//TODO: update for all axis, use for loop
#ifdef BLINKM
case 150: // M150
{
byte red;
byte grn;
byte blu;
if(code_seen('R')) red = code_value();
if(code_seen('U')) grn = code_value();
if(code_seen('B')) blu = code_value();
SendColors(red,grn,blu);
}
break;
#endif //BLINKM
case 200: // M200 D<millimeters> set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
{
tmp_extruder = active_extruder;
if(code_seen('T')) {
tmp_extruder = code_value();
if(tmp_extruder >= EXTRUDERS) {
SERIAL_ECHO_START;
SERIAL_ECHO(MSG_M200_INVALID_EXTRUDER);
break;
}
}
float area = .0;
if(code_seen('D')) {
float diameter = code_value();
// setting any extruder filament size disables volumetric on the assumption that
// slicers either generate in extruder values as cubic mm or as as filament feeds
// for all extruders
volumetric_enabled = (diameter != 0.0);
if (volumetric_enabled) {
filament_size[tmp_extruder] = diameter;
// make sure all extruders have some sane value for the filament size
for (int i=0; i<EXTRUDERS; i++)
if (! filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
}
} else {
//reserved for setting filament diameter via UFID or filament measuring device
break;
}
calculate_volumetric_multipliers();
}
break;
case 201: // M201
for(int8_t i=0; i < NUM_AXIS; i++)
{
if(code_seen(axis_codes[i]))
{
max_acceleration_units_per_sq_second[i] = code_value();
}
}
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
break;
#if 0 // Not used for Sprinter/grbl gen6
case 202: // M202
for(int8_t i=0; i < NUM_AXIS; i++) {
if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
}
break;
#endif
case 203: // M203 max feedrate mm/sec
for(int8_t i=0; i < NUM_AXIS; i++) {
if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
}
break;
case 204: // M204 acclereration S normal moves T filmanent only moves
{
if(code_seen('S')) acceleration = code_value() ;
if(code_seen('T')) retract_acceleration = code_value() ;
}
break;
case 205: //M205 advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
{
if(code_seen('S')) minimumfeedrate = code_value();
if(code_seen('T')) mintravelfeedrate = code_value();
if(code_seen('B')) minsegmenttime = code_value() ;
if(code_seen('X')) max_xy_jerk = code_value() ;
if(code_seen('Z')) max_z_jerk = code_value() ;
if(code_seen('E')) max_e_jerk = code_value() ;
}
break;
case 206: // M206 additional homing offset
for(int8_t i=0; i < 3; i++)
{
if(code_seen(axis_codes[i])) add_homing[i] = code_value();
}
#ifdef SCARA
if(code_seen('T')) // Theta
{
add_homing[X_AXIS] = code_value() ;
}
if(code_seen('P')) // Psi
{
add_homing[Y_AXIS] = code_value() ;
}
#endif
break;
#ifdef DELTA
case 665: // M665 set delta configurations L<diagonal_rod> R<delta_radius> S<segments_per_sec>
if(code_seen('L')) {
delta_diagonal_rod= code_value();
}
if(code_seen('R')) {
delta_radius= code_value();
}
if(code_seen('S')) {
delta_segments_per_second= code_value();
}
recalc_delta_settings(delta_radius, delta_diagonal_rod);
break;
case 666: // M666 set delta endstop adjustemnt
for(int8_t i=0; i < 3; i++)
{
if(code_seen(axis_codes[i])) endstop_adj[i] = code_value();
}
break;
#endif
#ifdef FWRETRACT
case 207: //M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
{
if(code_seen('S'))
{
retract_length = code_value() ;
}
if(code_seen('F'))
{
retract_feedrate = code_value()/60 ;
}
if(code_seen('Z'))
{
retract_zlift = code_value() ;
}
}break;
case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
{
if(code_seen('S'))
{
retract_recover_length = code_value() ;
}
if(code_seen('F'))
{
retract_recover_feedrate = code_value()/60 ;
}
}break;
case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
{
if(code_seen('S'))
{
int t= code_value() ;
switch(t)
{
case 0:
case 1:
{
autoretract_enabled = (t == 1);
for (int i=0; i<EXTRUDERS; i++) retracted[i] = false;
}break;
default:
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
SERIAL_ECHO(cmdbuffer[bufindr]);
SERIAL_ECHOLNPGM("\"");
}
}
}break;
#endif // FWRETRACT
#if EXTRUDERS > 1
case 218: // M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
{
if(setTargetedHotend(218)){
break; break;
}
if(code_seen('X'))
{
extruder_offset[X_AXIS][tmp_extruder] = code_value();
}
if(code_seen('Y'))
{
extruder_offset[Y_AXIS][tmp_extruder] = code_value();
}
#ifdef DUAL_X_CARRIAGE
if(code_seen('Z'))
{
extruder_offset[Z_AXIS][tmp_extruder] = code_value();
}
#endif
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
for(tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++)
{
SERIAL_ECHO(" ");
SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
#ifdef DUAL_X_CARRIAGE
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
#endif
}
SERIAL_ECHOLN("");
}break;
#endif
case 220: // M220 S<factor in percent>- set speed factor override percentage
{
if(code_seen('S'))
{
feedmultiply = code_value() ;
}
}
break;
case 221: // M221 S<factor in percent>- set extrude factor override percentage
{
if(code_seen('S'))
{
int tmp_code = code_value();
if (code_seen('T'))
{
if(setTargetedHotend(221)){
break;
}
extruder_multiply[tmp_extruder] = tmp_code;
}
else
{
extrudemultiply = tmp_code ;
}
}
}
break;
case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required case 82:
{ gcode_M82();
if(code_seen('P')){ break;
int pin_number = code_value(); // pin number case 83:
int pin_state = -1; // required pin state - default is inverted gcode_M83();
break;
case 18: //compatibility
case 84: // M84
gcode_M18_M84();
break;
case 85: // M85
gcode_M85();
break;
case 92: // M92
gcode_M92();
break;
case 115: // M115
gcode_M115();
break;
case 117: // M117 display message
gcode_M117();
break;
case 114: // M114
gcode_M114();
break;
case 120: // M120
gcode_M120();
break;
case 121: // M121
gcode_M121();
break;
case 119: // M119
gcode_M119();
break;
//TODO: update for all axis, use for loop
if(code_seen('S')) pin_state = code_value(); // required pin state #ifdef BLINKM
if(pin_state >= -1 && pin_state <= 1){ case 150: // M150
gcode_M150();
for(int8_t i = 0; i < (int8_t)(sizeof(sensitive_pins)/sizeof(int)); i++)
{
if (sensitive_pins[i] == pin_number)
{
pin_number = -1;
break;
}
}
if (pin_number > -1)
{
int target = LOW;
st_synchronize();
pinMode(pin_number, INPUT);
switch(pin_state){
case 1:
target = HIGH;
break;
case 0:
target = LOW;
break;
case -1:
target = !digitalRead(pin_number);
break;
}
while(digitalRead(pin_number) != target){
manage_heater();
manage_inactivity();
lcd_update();
}
}
}
}
}
break;
#if NUM_SERVOS > 0
case 280: // M280 - set servo position absolute. P: servo index, S: angle or microseconds
{
int servo_index = -1;
int servo_position = 0;
if (code_seen('P'))
servo_index = code_value();
if (code_seen('S')) {
servo_position = code_value();
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
servos[servo_index].attach(0);
#endif
servos[servo_index].write(servo_position);
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_index].detach();
#endif
}
else {
SERIAL_ECHO_START;
SERIAL_ECHO("Servo ");
SERIAL_ECHO(servo_index);
SERIAL_ECHOLN(" out of range");
}
}
else if (servo_index >= 0) {
SERIAL_PROTOCOL(MSG_OK);
SERIAL_PROTOCOL(" Servo ");
SERIAL_PROTOCOL(servo_index);
SERIAL_PROTOCOL(": ");
SERIAL_PROTOCOL(servos[servo_index].read());
SERIAL_PROTOCOLLN("");
}
}
break;
#endif // NUM_SERVOS > 0
#if (LARGE_FLASH == true && ( BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)))
case 300: // M300
{
int beepS = code_seen('S') ? code_value() : 110;
int beepP = code_seen('P') ? code_value() : 1000;
if (beepS > 0)
{
#if BEEPER > 0
tone(BEEPER, beepS);
delay(beepP);
noTone(BEEPER);
#elif defined(ULTRALCD)
lcd_buzz(beepS, beepP);
#elif defined(LCD_USE_I2C_BUZZER)
lcd_buzz(beepP, beepS);
#endif
}
else
{
delay(beepP);
}
}
break;
#endif // M300
#ifdef PIDTEMP
case 301: // M301
{
// multi-extruder PID patch: M301 updates or prints a single extruder's PID values
// default behaviour (omitting E parameter) is to update for extruder 0 only
int e = 0; // extruder being updated
if (code_seen('E'))
{
e = (int)code_value();
}
if (e < EXTRUDERS) // catch bad input value
{
if (code_seen('P')) PID_PARAM(Kp,e) = code_value();
if (code_seen('I')) PID_PARAM(Ki,e) = scalePID_i(code_value());
if (code_seen('D')) PID_PARAM(Kd,e) = scalePID_d(code_value());
#ifdef PID_ADD_EXTRUSION_RATE
if (code_seen('C')) PID_PARAM(Kc,e) = code_value();
#endif
updatePID();
SERIAL_PROTOCOL(MSG_OK);
#ifdef PID_PARAMS_PER_EXTRUDER
SERIAL_PROTOCOL(" e:"); // specify extruder in serial output
SERIAL_PROTOCOL(e);
#endif // PID_PARAMS_PER_EXTRUDER
SERIAL_PROTOCOL(" p:");
SERIAL_PROTOCOL(PID_PARAM(Kp,e));
SERIAL_PROTOCOL(" i:");
SERIAL_PROTOCOL(unscalePID_i(PID_PARAM(Ki,e)));
SERIAL_PROTOCOL(" d:");
SERIAL_PROTOCOL(unscalePID_d(PID_PARAM(Kd,e)));
#ifdef PID_ADD_EXTRUSION_RATE
SERIAL_PROTOCOL(" c:");
//Kc does not have scaling applied above, or in resetting defaults
SERIAL_PROTOCOL(PID_PARAM(Kc,e));
#endif
SERIAL_PROTOCOLLN("");
}
else
{
SERIAL_ECHO_START;
SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
}
}
break;
#endif //PIDTEMP
#ifdef PIDTEMPBED
case 304: // M304
{
if(code_seen('P')) bedKp = code_value();
if(code_seen('I')) bedKi = scalePID_i(code_value());
if(code_seen('D')) bedKd = scalePID_d(code_value());
updatePID();
SERIAL_PROTOCOL(MSG_OK);
SERIAL_PROTOCOL(" p:");
SERIAL_PROTOCOL(bedKp);
SERIAL_PROTOCOL(" i:");
SERIAL_PROTOCOL(unscalePID_i(bedKi));
SERIAL_PROTOCOL(" d:");
SERIAL_PROTOCOL(unscalePID_d(bedKd));
SERIAL_PROTOCOLLN("");
}
break;
#endif //PIDTEMP
case 240: // M240 Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
{
#ifdef CHDK
SET_OUTPUT(CHDK);
WRITE(CHDK, HIGH);
chdkHigh = millis();
chdkActive = true;
#else
#if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
const uint8_t NUM_PULSES=16;
const float PULSE_LENGTH=0.01524;
for(int i=0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
delay(7.33);
for(int i=0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
#endif
#endif //chdk end if
}
break;
#ifdef DOGLCD
case 250: // M250 Set LCD contrast value: C<value> (value 0..63)
{
if (code_seen('C')) {
lcd_setcontrast( ((int)code_value())&63 );
}
SERIAL_PROTOCOLPGM("lcd contrast value: ");
SERIAL_PROTOCOL(lcd_contrast);
SERIAL_PROTOCOLLN("");
}
break;
#endif
#ifdef PREVENT_DANGEROUS_EXTRUDE
case 302: // allow cold extrudes, or set the minimum extrude temperature
{
float temp = .0;
if (code_seen('S')) temp=code_value();
set_extrude_min_temp(temp);
}
break;
#endif
case 303: // M303 PID autotune
{
float temp = 150.0;
int e=0;
int c=5;
if (code_seen('E')) e=code_value();
if (e<0)
temp=70;
if (code_seen('S')) temp=code_value();
if (code_seen('C')) c=code_value();
PID_autotune(temp, e, c);
}
break;
#ifdef SCARA
case 360: // M360 SCARA Theta pos1
SERIAL_ECHOLN(" Cal: Theta 0 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if(Stopped == false) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 0;
delta[Y_AXIS] = 120;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return;
}
break;
case 361: // SCARA Theta pos2
SERIAL_ECHOLN(" Cal: Theta 90 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if(Stopped == false) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 90;
delta[Y_AXIS] = 130;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return;
}
break;
case 362: // SCARA Psi pos1
SERIAL_ECHOLN(" Cal: Psi 0 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if(Stopped == false) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 60;
delta[Y_AXIS] = 180;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return;
}
break;
case 363: // SCARA Psi pos2
SERIAL_ECHOLN(" Cal: Psi 90 ");
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if(Stopped == false) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 50;
delta[Y_AXIS] = 90;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return;
}
break;
case 364: // SCARA Psi pos3 (90 deg to Theta)
SERIAL_ECHOLN(" Cal: Theta-Psi 90 ");
// SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLN(" Soft endstops disabled ");
if(Stopped == false) {
//get_coordinates(); // For X Y Z E F
delta[X_AXIS] = 45;
delta[Y_AXIS] = 135;
calculate_SCARA_forward_Transform(delta);
destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
prepare_move();
//ClearToSend();
return;
}
break;
case 365: // M364 Set SCARA scaling for X Y Z
for(int8_t i=0; i < 3; i++)
{
if(code_seen(axis_codes[i]))
{
axis_scaling[i] = code_value();
}
}
break;
#endif
case 400: // M400 finish all moves
{
st_synchronize();
}
break;
#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS) && not defined(Z_PROBE_SLED)
case 401:
{
engage_z_probe(); // Engage Z Servo endstop if available
}
break;
case 402:
{
retract_z_probe(); // Retract Z Servo endstop if enabled
}
break;
#endif
#ifdef FILAMENT_SENSOR
case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width
{
#if (FILWIDTH_PIN > -1)
if(code_seen('N')) filament_width_nominal=code_value();
else{
SERIAL_PROTOCOLPGM("Filament dia (nominal mm):");
SERIAL_PROTOCOLLN(filament_width_nominal);
}
#endif
}
break;
case 405: //M405 Turn on filament sensor for control
{
if(code_seen('D')) meas_delay_cm=code_value();
if(meas_delay_cm> MAX_MEASUREMENT_DELAY)
meas_delay_cm = MAX_MEASUREMENT_DELAY;
if(delay_index2 == -1) //initialize the ring buffer if it has not been done since startup
{
int temp_ratio = widthFil_to_size_ratio();
for (delay_index1=0; delay_index1<(MAX_MEASUREMENT_DELAY+1); ++delay_index1 ){
measurement_delay[delay_index1]=temp_ratio-100; //subtract 100 to scale within a signed byte
}
delay_index1=0;
delay_index2=0;
}
filament_sensor = true ;
//SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
//SERIAL_PROTOCOL(filament_width_meas);
//SERIAL_PROTOCOLPGM("Extrusion ratio(%):");
//SERIAL_PROTOCOL(extrudemultiply);
}
break;
case 406: //M406 Turn off filament sensor for control
{
filament_sensor = false ;
}
break;
case 407: //M407 Display measured filament diameter
{
SERIAL_PROTOCOLPGM("Filament dia (measured mm):");
SERIAL_PROTOCOLLN(filament_width_meas);
}
break;
#endif
case 500: // M500 Store settings in EEPROM
{
Config_StoreSettings();
}
break;
case 501: // M501 Read settings from EEPROM
{
Config_RetrieveSettings();
}
break;
case 502: // M502 Revert to default settings
{
Config_ResetDefault();
}
break;
case 503: // M503 print settings currently in memory
{
Config_PrintSettings(code_seen('S') && code_value == 0);
}
break;
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
case 540:
{
if(code_seen('S')) abort_on_endstop_hit = code_value() > 0;
}
break;
#endif
#ifdef CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
case CUSTOM_M_CODE_SET_Z_PROBE_OFFSET:
{
float value;
if (code_seen('Z'))
{
value = code_value();
if ((Z_PROBE_OFFSET_RANGE_MIN <= value) && (value <= Z_PROBE_OFFSET_RANGE_MAX))
{
zprobe_zoffset = -value; // compare w/ line 278 of ConfigurationStore.cpp
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " " MSG_OK);
SERIAL_PROTOCOLLN("");
}
else
{
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_ZPROBE_ZOFFSET);
SERIAL_ECHOPGM(MSG_Z_MIN);
SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MIN);
SERIAL_ECHOPGM(MSG_Z_MAX);
SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MAX);
SERIAL_PROTOCOLLN("");
}
}
else
{
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ZPROBE_ZOFFSET " : ");
SERIAL_ECHO(-zprobe_zoffset);
SERIAL_PROTOCOLLN("");
}
break;
}
#endif // CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
#ifdef FILAMENTCHANGEENABLE
case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
{
float target[NUM_AXIS], lastpos[NUM_AXIS], fr60 = feedrate/60;
for (int i=0; i<NUM_AXIS; i++)
target[i] = lastpos[i] = current_position[i];
#define BASICPLAN plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr60, active_extruder);
#ifdef DELTA
#define RUNPLAN calculate_delta(target); BASICPLAN
#else
#define RUNPLAN BASICPLAN
#endif
//retract by E
if(code_seen('E'))
{
target[E_AXIS]+= code_value();
}
else
{
#ifdef FILAMENTCHANGE_FIRSTRETRACT
target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
#endif
}
RUNPLAN;
//lift Z
if(code_seen('Z'))
{
target[Z_AXIS]+= code_value();
}
else
{
#ifdef FILAMENTCHANGE_ZADD
target[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
#endif
}
RUNPLAN;
//move xy
if(code_seen('X'))
{
target[X_AXIS]= code_value();
}
else
{
#ifdef FILAMENTCHANGE_XPOS
target[X_AXIS]= FILAMENTCHANGE_XPOS ;
#endif
}
if(code_seen('Y'))
{
target[Y_AXIS]= code_value();
}
else
{
#ifdef FILAMENTCHANGE_YPOS
target[Y_AXIS]= FILAMENTCHANGE_YPOS ;
#endif
}
RUNPLAN;
if(code_seen('L'))
{
target[E_AXIS]+= code_value();
}
else
{
#ifdef FILAMENTCHANGE_FINALRETRACT
target[E_AXIS]+= FILAMENTCHANGE_FINALRETRACT ;
#endif
}
RUNPLAN;
//finish moves
st_synchronize();
//disable extruder steppers so filament can be removed
disable_e0();
disable_e1();
disable_e2();
delay(100);
LCD_ALERTMESSAGEPGM(MSG_FILAMENTCHANGE);
uint8_t cnt=0;
while(!lcd_clicked()){
cnt++;
manage_heater();
manage_inactivity(true);
lcd_update();
if(cnt==0)
{
#if BEEPER > 0
SET_OUTPUT(BEEPER);
WRITE(BEEPER,HIGH);
delay(3);
WRITE(BEEPER,LOW);
delay(3);
#else
#if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
lcd_buzz(1000/6,100);
#else
lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS,LCD_FEEDBACK_FREQUENCY_HZ);
#endif
#endif
}
}
//return to normal
if(code_seen('L'))
{
target[E_AXIS]+= -code_value();
}
else
{
#ifdef FILAMENTCHANGE_FINALRETRACT
target[E_AXIS]+=(-1)*FILAMENTCHANGE_FINALRETRACT ;
#endif
}
current_position[E_AXIS]=target[E_AXIS]; //the long retract of L is compensated by manual filament feeding
plan_set_e_position(current_position[E_AXIS]);
RUNPLAN; //should do nothing
#ifdef DELTA
calculate_delta(lastpos);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move xyz back
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], fr60, active_extruder); //final untretract
#else
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move xy back
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], fr60, active_extruder); //move z back
plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], fr60, active_extruder); //final untretract
#endif
}
break;
#endif //FILAMENTCHANGEENABLE
#ifdef DUAL_X_CARRIAGE
case 605: // Set dual x-carriage movement mode:
// M605 S0: Full control mode. The slicer has full control over x-carriage movement
// M605 S1: Auto-park mode. The inactive head will auto park/unpark without slicer involvement
// M605 S2 [Xnnn] [Rmmm]: Duplication mode. The second extruder will duplicate the first with nnn
// millimeters x-offset and an optional differential hotend temperature of
// mmm degrees. E.g., with "M605 S2 X100 R2" the second extruder will duplicate
// the first with a spacing of 100mm in the x direction and 2 degrees hotter.
//
// Note: the X axis should be homed after changing dual x-carriage mode.
{
st_synchronize();
if (code_seen('S'))
dual_x_carriage_mode = code_value();
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
{
if (code_seen('X'))
duplicate_extruder_x_offset = max(code_value(),X2_MIN_POS - x_home_pos(0));
if (code_seen('R'))
duplicate_extruder_temp_offset = code_value();
SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
SERIAL_ECHO(" ");
SERIAL_ECHO(extruder_offset[X_AXIS][0]);
SERIAL_ECHO(",");
SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
SERIAL_ECHO(" ");
SERIAL_ECHO(duplicate_extruder_x_offset);
SERIAL_ECHO(",");
SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
}
else if (dual_x_carriage_mode != DXC_FULL_CONTROL_MODE && dual_x_carriage_mode != DXC_AUTO_PARK_MODE)
{
dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
}
active_extruder_parked = false;
extruder_duplication_enabled = false;
delayed_move_time = 0;
}
break;
#endif //DUAL_X_CARRIAGE
case 907: // M907 Set digital trimpot motor current using axis codes.
{
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_current(i,code_value());
if(code_seen('B')) digipot_current(4,code_value());
if(code_seen('S')) for(int i=0;i<=4;i++) digipot_current(i,code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_XY_PIN
if(code_seen('X')) digipot_current(0, code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_Z_PIN
if(code_seen('Z')) digipot_current(1, code_value());
#endif
#ifdef MOTOR_CURRENT_PWM_E_PIN
if(code_seen('E')) digipot_current(2, code_value());
#endif
#ifdef DIGIPOT_I2C
// this one uses actual amps in floating point
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value());
// for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
for(int i=NUM_AXIS;i<DIGIPOT_I2C_NUM_CHANNELS;i++) if(code_seen('B'+i-NUM_AXIS)) digipot_i2c_set_current(i, code_value());
#endif
}
break;
case 908: // M908 Control digital trimpot directly.
{
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
uint8_t channel,current;
if(code_seen('P')) channel=code_value();
if(code_seen('S')) current=code_value();
digitalPotWrite(channel, current);
#endif
}
break;
case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
{
#if defined(X_MS1_PIN) && X_MS1_PIN > -1
if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
if(code_seen('B')) microstep_mode(4,code_value());
microstep_readings();
#endif
}
break;
case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
{
#if defined(X_MS1_PIN) && X_MS1_PIN > -1
if(code_seen('S')) switch((int)code_value())
{
case 1:
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,code_value(),-1);
if(code_seen('B')) microstep_ms(4,code_value(),-1);
break; break;
case 2:
for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,-1,code_value());
if(code_seen('B')) microstep_ms(4,-1,code_value());
break;
}
microstep_readings();
#endif
}
break;
case 999: // M999: Restart after being stopped
Stopped = false;
lcd_reset_alert_level();
gcode_LastN = Stopped_gcode_LastN;
FlushSerialRequestResend();
break;
}
}
else if(code_seen('T')) #endif //BLINKM
{
tmp_extruder = code_value(); case 200: // M200 D<millimeters> set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
if(tmp_extruder >= EXTRUDERS) { gcode_M200();
SERIAL_ECHO_START; break;
SERIAL_ECHO("T"); case 201: // M201
SERIAL_ECHO(tmp_extruder); gcode_M201();
SERIAL_ECHOLN(MSG_INVALID_EXTRUDER); break;
} #if 0 // Not used for Sprinter/grbl gen6
else { case 202: // M202
boolean make_move = false; gcode_M202();
if(code_seen('F')) { break;
make_move = true; #endif
next_feedrate = code_value(); case 203: // M203 max feedrate mm/sec
if(next_feedrate > 0.0) { gcode_M203();
feedrate = next_feedrate; break;
} case 204: // M204 acclereration S normal moves T filmanent only moves
} gcode_M204();
break;
case 205: //M205 advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
gcode_M205();
break;
case 206: // M206 additional homing offset
gcode_M206();
break;
#ifdef DELTA
case 665: // M665 set delta configurations L<diagonal_rod> R<delta_radius> S<segments_per_sec>
gcode_M665();
break;
case 666: // M666 set delta endstop adjustment
gcode_M666();
break;
#endif // DELTA
#ifdef FWRETRACT
case 207: //M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
gcode_M207();
break;
case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
gcode_M208();
break;
case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
gcode_M209();
break;
#endif // FWRETRACT
#if EXTRUDERS > 1 #if EXTRUDERS > 1
if(tmp_extruder != active_extruder) { case 218: // M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
// Save current position to return to after applying extruder offset gcode_M218();
memcpy(destination, current_position, sizeof(destination)); break;
#ifdef DUAL_X_CARRIAGE
if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && Stopped == false &&
(delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder)))
{
// Park old head: 1) raise 2) move to park position 3) lower
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[X_AXIS], active_extruder);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS],
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
st_synchronize();
}
// apply Y & Z extruder offset (x offset is already used in determining home pos)
current_position[Y_AXIS] = current_position[Y_AXIS] -
extruder_offset[Y_AXIS][active_extruder] +
extruder_offset[Y_AXIS][tmp_extruder];
current_position[Z_AXIS] = current_position[Z_AXIS] -
extruder_offset[Z_AXIS][active_extruder] +
extruder_offset[Z_AXIS][tmp_extruder];
active_extruder = tmp_extruder;
// This function resets the max/min values - the current position may be overwritten below.
axis_is_at_home(X_AXIS);
if (dual_x_carriage_mode == DXC_FULL_CONTROL_MODE)
{
current_position[X_AXIS] = inactive_extruder_x_pos;
inactive_extruder_x_pos = destination[X_AXIS];
}
else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
{
active_extruder_parked = (active_extruder == 0); // this triggers the second extruder to move into the duplication position
if (active_extruder == 0 || active_extruder_parked)
current_position[X_AXIS] = inactive_extruder_x_pos;
else
current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset;
inactive_extruder_x_pos = destination[X_AXIS];
extruder_duplication_enabled = false;
}
else
{
// record raised toolhead position for use by unpark
memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
raised_parked_position[Z_AXIS] += TOOLCHANGE_UNPARK_ZLIFT;
active_extruder_parked = true;
delayed_move_time = 0;
}
#else
// Offset extruder (only by XY)
int i;
for(i = 0; i < 2; i++) {
current_position[i] = current_position[i] -
extruder_offset[i][active_extruder] +
extruder_offset[i][tmp_extruder];
}
// Set the new active extruder and position
active_extruder = tmp_extruder;
#endif //else DUAL_X_CARRIAGE
#ifdef DELTA
calculate_delta(current_position); // change cartesian kinematic to delta kinematic;
//sent position to plan_set_position();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]);
#else
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif
// Move to the old position if 'F' was in the parameters
if(make_move && Stopped == false) {
prepare_move();
}
}
#endif #endif
SERIAL_ECHO_START;
SERIAL_ECHO(MSG_ACTIVE_EXTRUDER); case 220: // M220 S<factor in percent>- set speed factor override percentage
SERIAL_PROTOCOLLN((int)active_extruder); gcode_M220();
break;
case 221: // M221 S<factor in percent>- set extrude factor override percentage
gcode_M221();
break;
case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
gcode_M226();
break;
#if NUM_SERVOS > 0
case 280: // M280 - set servo position absolute. P: servo index, S: angle or microseconds
gcode_M280();
break;
#endif // NUM_SERVOS > 0
#if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
case 300: // M300 - Play beep tone
gcode_M300();
break;
#endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
#ifdef PIDTEMP
case 301: // M301
gcode_M301();
break;
#endif // PIDTEMP
#ifdef PIDTEMPBED
case 304: // M304
gcode_M304();
break;
#endif // PIDTEMPBED
#if defined(CHDK) || (defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1)
case 240: // M240 Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
gcode_M240();
break;
#endif // CHDK || PHOTOGRAPH_PIN
#ifdef DOGLCD
case 250: // M250 Set LCD contrast value: C<value> (value 0..63)
gcode_M250();
break;
#endif // DOGLCD
#ifdef PREVENT_DANGEROUS_EXTRUDE
case 302: // allow cold extrudes, or set the minimum extrude temperature
gcode_M302();
break;
#endif // PREVENT_DANGEROUS_EXTRUDE
case 303: // M303 PID autotune
gcode_M303();
break;
#ifdef SCARA
case 360: // M360 SCARA Theta pos1
if (gcode_M360()) return;
break;
case 361: // M361 SCARA Theta pos2
if (gcode_M361()) return;
break;
case 362: // M362 SCARA Psi pos1
if (gcode_M362()) return;
break;
case 363: // M363 SCARA Psi pos2
if (gcode_M363()) return;
break;
case 364: // M364 SCARA Psi pos3 (90 deg to Theta)
if (gcode_M364()) return;
break;
case 365: // M365 Set SCARA scaling for X Y Z
gcode_M365();
break;
#endif // SCARA
case 400: // M400 finish all moves
gcode_M400();
break;
#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS) && not defined(Z_PROBE_SLED)
case 401:
gcode_M401();
break;
case 402:
gcode_M402();
break;
#endif
#ifdef FILAMENT_SENSOR
case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width
gcode_M404();
break;
case 405: //M405 Turn on filament sensor for control
gcode_M405();
break;
case 406: //M406 Turn off filament sensor for control
gcode_M406();
break;
case 407: //M407 Display measured filament diameter
gcode_M407();
break;
#endif // FILAMENT_SENSOR
case 500: // M500 Store settings in EEPROM
gcode_M500();
break;
case 501: // M501 Read settings from EEPROM
gcode_M501();
break;
case 502: // M502 Revert to default settings
gcode_M502();
break;
case 503: // M503 print settings currently in memory
gcode_M503();
break;
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
case 540:
gcode_M540();
break;
#endif
#ifdef CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
case CUSTOM_M_CODE_SET_Z_PROBE_OFFSET:
gcode_SET_Z_PROBE_OFFSET();
break;
#endif // CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
#ifdef FILAMENTCHANGEENABLE
case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
gcode_M600();
break;
#endif // FILAMENTCHANGEENABLE
#ifdef DUAL_X_CARRIAGE
case 605:
gcode_M605();
break;
#endif // DUAL_X_CARRIAGE
case 907: // M907 Set digital trimpot motor current using axis codes.
gcode_M907();
break;
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
case 908: // M908 Control digital trimpot directly.
gcode_M908();
break;
#endif // DIGIPOTSS_PIN
case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
gcode_M350();
break;
case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
gcode_M351();
break;
case 999: // M999: Restart after being Stopped
gcode_M999();
break;
} }
} }
else else if (code_seen('T')) {
{ gcode_T();
}
else {
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND); SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
SERIAL_ECHO(cmdbuffer[bufindr]); SERIAL_ECHO(cmdbuffer[bufindr]);
@ -4053,13 +4845,13 @@ void clamp_to_software_endstops(float target[3])
#ifdef DELTA #ifdef DELTA
void recalc_delta_settings(float radius, float diagonal_rod) void recalc_delta_settings(float radius, float diagonal_rod)
{ {
delta_tower1_x= -SIN_60*radius; // front left tower delta_tower1_x= -SIN_60*radius; // front left tower
delta_tower1_y= -COS_60*radius; delta_tower1_y= -COS_60*radius;
delta_tower2_x= SIN_60*radius; // front right tower delta_tower2_x= SIN_60*radius; // front right tower
delta_tower2_y= -COS_60*radius; delta_tower2_y= -COS_60*radius;
delta_tower3_x= 0.0; // back middle tower delta_tower3_x= 0.0; // back middle tower
delta_tower3_y= radius; delta_tower3_y= radius;
delta_diagonal_rod_2= sq(diagonal_rod); delta_diagonal_rod_2= sq(diagonal_rod);
} }
void calculate_delta(float cartesian[3]) void calculate_delta(float cartesian[3])
@ -4097,12 +4889,12 @@ void prepare_move()
float difference[NUM_AXIS]; float difference[NUM_AXIS];
for (int8_t i=0; i < NUM_AXIS; i++) { for (int8_t i=0; i < NUM_AXIS; i++) {
difference[i] = destination[i] - current_position[i]; difference[i] = destination[i] - current_position[i];
} }
float cartesian_mm = sqrt( sq(difference[X_AXIS]) + float cartesian_mm = sqrt( sq(difference[X_AXIS]) +
sq(difference[Y_AXIS]) + sq(difference[Y_AXIS]) +
sq(difference[Z_AXIS])); sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
if (cartesian_mm < 0.000001) { return; } if (cartesian_mm < 0.000001) { return; }
float seconds = 6000 * cartesian_mm / feedrate / feedmultiply; float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
@ -4111,13 +4903,13 @@ int steps = max(1, int(scara_segments_per_second * seconds));
//SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds); //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
//SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps); //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
for (int s = 1; s <= steps; s++) { for (int s = 1; s <= steps; s++) {
float fraction = float(s) / float(steps); float fraction = float(s) / float(steps);
for(int8_t i=0; i < NUM_AXIS; i++) { for(int8_t i=0; i < NUM_AXIS; i++) {
destination[i] = current_position[i] + difference[i] * fraction; destination[i] = current_position[i] + difference[i] * fraction;
} }
calculate_delta(destination); calculate_delta(destination);
//SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]); //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
//SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]); //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
//SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]); //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
@ -4125,9 +4917,9 @@ for (int s = 1; s <= steps; s++) {
//SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
//SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]); //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
destination[E_AXIS], feedrate*feedmultiply/60/100.0, destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder); active_extruder);
} }
#endif // SCARA #endif // SCARA
@ -4300,7 +5092,7 @@ void calculate_SCARA_forward_Transform(float f_scara[3])
delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x; //theta delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x; //theta
delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y; //theta+phi delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y; //theta+phi
//SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]);
//SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
} }
@ -4390,9 +5182,9 @@ void handle_status_leds(void) {
void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
{ {
#if defined(KILL_PIN) && KILL_PIN > -1 #if defined(KILL_PIN) && KILL_PIN > -1
static int killCount = 0; // make the inactivity button a bit less responsive static int killCount = 0; // make the inactivity button a bit less responsive
const int KILL_DELAY = 10000; const int KILL_DELAY = 10000;
#endif #endif
@ -4401,7 +5193,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
const int HOME_DEBOUNCE_DELAY = 10000; const int HOME_DEBOUNCE_DELAY = 10000;
#endif #endif
if(buflen < (BUFSIZE-1)) if(buflen < (BUFSIZE-1))
get_command(); get_command();
@ -4418,6 +5210,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
disable_e0(); disable_e0();
disable_e1(); disable_e1();
disable_e2(); disable_e2();
disable_e3();
} }
} }
} }
@ -4459,7 +5252,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
{ {
if (homeDebounceCount == 0) if (homeDebounceCount == 0)
{ {
enquecommand_P((PSTR("G28"))); enquecommands_P((PSTR("G28")));
homeDebounceCount++; homeDebounceCount++;
LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME); LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME);
} }
@ -4523,6 +5316,7 @@ void kill()
disable_e0(); disable_e0();
disable_e1(); disable_e1();
disable_e2(); disable_e2();
disable_e3();
#if defined(PS_ON_PIN) && PS_ON_PIN > -1 #if defined(PS_ON_PIN) && PS_ON_PIN > -1
pinMode(PS_ON_PIN,INPUT); pinMode(PS_ON_PIN,INPUT);
@ -4535,7 +5329,7 @@ void kill()
sei(); // enable interrupts sei(); // enable interrupts
for ( int i=5; i--; lcd_update()) for ( int i=5; i--; lcd_update())
{ {
delay(200); delay(200);
} }
cli(); // disable interrupts cli(); // disable interrupts
suicide(); suicide();
@ -4656,23 +5450,13 @@ bool setTargetedHotend(int code){
return false; return false;
} }
float calculate_volumetric_multiplier(float diameter) { float calculate_volumetric_multiplier(float diameter) {
float area = .0; if (!volumetric_enabled || diameter == 0) return 1.0;
float radius = .0; float d2 = diameter * 0.5;
return 1.0 / (M_PI * d2 * d2);
radius = diameter * .5;
if (! volumetric_enabled || radius == 0) {
area = 1;
}
else {
area = M_PI * pow(radius, 2);
}
return 1.0 / area;
} }
void calculate_volumetric_multipliers() { void calculate_volumetric_multipliers() {
for (int i=0; i<EXTRUDERS; i++) for (int i=0; i<EXTRUDERS; i++)
volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]); volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
} }

View file

@ -50,10 +50,11 @@
#define BOARD_LEAPFROG 999 // Leapfrog #define BOARD_LEAPFROG 999 // Leapfrog
#define BOARD_WITBOX 41 // bq WITBOX #define BOARD_WITBOX 41 // bq WITBOX
#define BOARD_HEPHESTOS 42 // bq Prusa i3 Hephestos #define BOARD_HEPHESTOS 42 // bq Prusa i3 Hephestos
#define BOARD_BAM_DICE 401 // 2PrintBeta BAM&DICE with STK drivers
#define BOARD_BAM_DICE_DUE 402 // 2PrintBeta BAM&DICE Due with STK drivers
#define BOARD_99 99 // This is in pins.h but...? #define BOARD_99 99 // This is in pins.h but...?
#define MB(board) (MOTHERBOARD==BOARD_##board) #define MB(board) (MOTHERBOARD==BOARD_##board)
#define IS_RAMPS (MB(RAMPS_OLD) || MB(RAMPS_13_EFB) || MB(RAMPS_13_EEB) || MB(RAMPS_13_EFF) || MB(RAMPS_13_EEF))
#endif //__BOARDS_H #endif //__BOARDS_H

View file

@ -7,476 +7,382 @@
#ifdef SDSUPPORT #ifdef SDSUPPORT
CardReader::CardReader() {
filesize = 0;
sdpos = 0;
sdprinting = false;
cardOK = false;
saving = false;
logging = false;
workDirDepth = 0;
file_subcall_ctr = 0;
memset(workDirParents, 0, sizeof(workDirParents));
autostart_stilltocheck = true; //the SD start is delayed, because otherwise the serial cannot answer fast enough to make contact with the host software.
CardReader::CardReader() autostart_index = 0;
{
filesize = 0;
sdpos = 0;
sdprinting = false;
cardOK = false;
saving = false;
logging = false;
autostart_atmillis=0;
workDirDepth = 0;
file_subcall_ctr=0;
memset(workDirParents, 0, sizeof(workDirParents));
autostart_stilltocheck=true; //the SD start is delayed, because otherwise the serial cannot answer fast enough to make contact with the host software.
autostart_index=0;
//power to SD reader //power to SD reader
#if SDPOWER > -1 #if SDPOWER > -1
SET_OUTPUT(SDPOWER); OUT_WRITE(SDPOWER, HIGH);
WRITE(SDPOWER,HIGH);
#endif //SDPOWER #endif //SDPOWER
autostart_atmillis=millis()+5000; autostart_atmillis = millis() + 5000;
} }
char *createFilename(char *buffer,const dir_t &p) //buffer>12characters char *createFilename(char *buffer, const dir_t &p) { //buffer > 12characters
{ char *pos = buffer;
char *pos=buffer; for (uint8_t i = 0; i < 11; i++) {
for (uint8_t i = 0; i < 11; i++) if (p.name[i] == ' ') continue;
{ if (i == 8) *pos++ = '.';
if (p.name[i] == ' ')continue; *pos++ = p.name[i];
if (i == 8)
{
*pos++='.';
}
*pos++=p.name[i];
} }
*pos++=0; *pos++ = 0;
return buffer; return buffer;
} }
void CardReader::lsDive(const char *prepend, SdFile parent, const char * const match/*=NULL*/) {
void CardReader::lsDive(const char *prepend, SdFile parent, const char * const match/*=NULL*/)
{
dir_t p; dir_t p;
uint8_t cnt=0; uint8_t cnt = 0;
while (parent.readDir(p, longFilename) > 0)
{
if( DIR_IS_SUBDIR(&p) && lsAction!=LS_Count && lsAction!=LS_GetFilename) // hence LS_SerialPrint
{
while (parent.readDir(p, longFilename) > 0) {
if (DIR_IS_SUBDIR(&p) && lsAction != LS_Count && lsAction != LS_GetFilename) { // hence LS_SerialPrint
char path[FILENAME_LENGTH*2]; char path[FILENAME_LENGTH*2];
char lfilename[FILENAME_LENGTH]; char lfilename[FILENAME_LENGTH];
createFilename(lfilename,p); createFilename(lfilename, p);
path[0]=0; path[0] = 0;
if(strlen(prepend)==0) //avoid leading / if already in prepend if (prepend[0] == 0) strcat(path, "/"); //avoid leading / if already in prepend
{ strcat(path, prepend);
strcat(path,"/"); strcat(path, lfilename);
} strcat(path, "/");
strcat(path,prepend);
strcat(path,lfilename);
strcat(path,"/");
//Serial.print(path); //Serial.print(path);
SdFile dir; SdFile dir;
if(!dir.open(parent,lfilename, O_READ)) if (!dir.open(parent, lfilename, O_READ)) {
{ if (lsAction == LS_SerialPrint) {
if(lsAction==LS_SerialPrint)
{
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLN(MSG_SD_CANT_OPEN_SUBDIR); SERIAL_ECHOLN(MSG_SD_CANT_OPEN_SUBDIR);
SERIAL_ECHOLN(lfilename); SERIAL_ECHOLN(lfilename);
} }
} }
lsDive(path,dir); lsDive(path, dir);
//close done automatically by destructor of SdFile //close done automatically by destructor of SdFile
} }
else else {
{
char pn0 = p.name[0]; char pn0 = p.name[0];
if (pn0 == DIR_NAME_FREE) break; if (pn0 == DIR_NAME_FREE) break;
if (pn0 == DIR_NAME_DELETED || pn0 == '.' || pn0 == '_') continue; if (pn0 == DIR_NAME_DELETED || pn0 == '.') continue;
char lf0 = longFilename[0]; char lf0 = longFilename[0];
if (lf0 == '.' || lf0 == '_') continue; if (lf0 == '.') continue;
if (!DIR_IS_FILE_OR_SUBDIR(&p)) continue; if (!DIR_IS_FILE_OR_SUBDIR(&p)) continue;
filenameIsDir=DIR_IS_SUBDIR(&p);
filenameIsDir = DIR_IS_SUBDIR(&p);
if(!filenameIsDir) if (!filenameIsDir && (p.name[8] != 'G' || p.name[9] == '~')) continue;
{
if(p.name[8]!='G') continue; //if (cnt++ != nr) continue;
if(p.name[9]=='~') continue; createFilename(filename, p);
} if (lsAction == LS_SerialPrint) {
//if(cnt++!=nr) continue;
createFilename(filename,p);
if(lsAction==LS_SerialPrint)
{
SERIAL_PROTOCOL(prepend); SERIAL_PROTOCOL(prepend);
SERIAL_PROTOCOLLN(filename); SERIAL_PROTOCOLLN(filename);
} }
else if(lsAction==LS_Count) else if (lsAction == LS_Count) {
{
nrFiles++; nrFiles++;
} }
else if(lsAction==LS_GetFilename) else if (lsAction == LS_GetFilename) {
{
if (match != NULL) { if (match != NULL) {
if (strcasecmp(match, filename) == 0) return; if (strcasecmp(match, filename) == 0) return;
} }
else if (cnt == nrFiles) return; else if (cnt == nrFiles) return;
cnt++; cnt++;
} }
} }
} }
} }
void CardReader::ls() void CardReader::ls() {
{ lsAction = LS_SerialPrint;
lsAction=LS_SerialPrint;
if(lsAction==LS_Count)
nrFiles=0;
root.rewind(); root.rewind();
lsDive("",root); lsDive("", root);
} }
void CardReader::initsd() {
void CardReader::initsd()
{
cardOK = false; cardOK = false;
if(root.isOpen()) if (root.isOpen()) root.close();
root.close();
#ifdef SDSLOW #ifdef SDSLOW
if (!card.init(SPI_HALF_SPEED,SDSS) #define SPI_SPEED SPI_HALF_SPEED
#if defined(LCD_SDSS) && (LCD_SDSS != SDSS) #else
&& !card.init(SPI_HALF_SPEED,LCD_SDSS) #define SPI_SPEED SPI_FULL_SPEED
#endif #endif
)
#else if (!card.init(SPI_SPEED,SDSS)
if (!card.init(SPI_FULL_SPEED,SDSS) #if defined(LCD_SDSS) && (LCD_SDSS != SDSS)
#if defined(LCD_SDSS) && (LCD_SDSS != SDSS) && !card.init(SPI_SPEED, LCD_SDSS)
&& !card.init(SPI_FULL_SPEED,LCD_SDSS) #endif
#endif ) {
)
#endif
{
//if (!card.init(SPI_HALF_SPEED,SDSS)) //if (!card.init(SPI_HALF_SPEED,SDSS))
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_SD_INIT_FAIL); SERIAL_ECHOLNPGM(MSG_SD_INIT_FAIL);
} }
else if (!volume.init(&card)) else if (!volume.init(&card)) {
{
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_SD_VOL_INIT_FAIL); SERIAL_ERRORLNPGM(MSG_SD_VOL_INIT_FAIL);
} }
else if (!root.openRoot(&volume)) else if (!root.openRoot(&volume)) {
{
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_SD_OPENROOT_FAIL); SERIAL_ERRORLNPGM(MSG_SD_OPENROOT_FAIL);
} }
else else {
{
cardOK = true; cardOK = true;
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_SD_CARD_OK); SERIAL_ECHOLNPGM(MSG_SD_CARD_OK);
} }
workDir=root; workDir = root;
curDir=&root; curDir = &root;
/* /*
if(!workDir.openRoot(&volume)) if (!workDir.openRoot(&volume)) {
{
SERIAL_ECHOLNPGM(MSG_SD_WORKDIR_FAIL); SERIAL_ECHOLNPGM(MSG_SD_WORKDIR_FAIL);
} }
*/ */
} }
void CardReader::setroot() void CardReader::setroot() {
{ /*if (!workDir.openRoot(&volume)) {
/*if(!workDir.openRoot(&volume))
{
SERIAL_ECHOLNPGM(MSG_SD_WORKDIR_FAIL); SERIAL_ECHOLNPGM(MSG_SD_WORKDIR_FAIL);
}*/ }*/
workDir=root; workDir = root;
curDir = &workDir;
curDir=&workDir;
} }
void CardReader::release()
{ void CardReader::release() {
sdprinting = false; sdprinting = false;
cardOK = false; cardOK = false;
} }
void CardReader::startFileprint() void CardReader::startFileprint() {
{ if (cardOK) {
if(cardOK)
{
sdprinting = true; sdprinting = true;
} }
} }
void CardReader::pauseSDPrint() void CardReader::pauseSDPrint() {
{ if (sdprinting) sdprinting = false;
if(sdprinting)
{
sdprinting = false;
}
} }
void CardReader::openLogFile(char* name) {
void CardReader::openLogFile(char* name)
{
logging = true; logging = true;
openFile(name, false); openFile(name, false);
} }
void CardReader::getAbsFilename(char *t) void CardReader::getAbsFilename(char *t) {
{ uint8_t cnt = 0;
uint8_t cnt=0; *t = '/'; t++; cnt++;
*t='/';t++;cnt++; for (uint8_t i = 0; i < workDirDepth; i++) {
for(uint8_t i=0;i<workDirDepth;i++)
{
workDirParents[i].getFilename(t); //SDBaseFile.getfilename! workDirParents[i].getFilename(t); //SDBaseFile.getfilename!
while(*t!=0 && cnt< MAXPATHNAMELENGTH) while(*t && cnt < MAXPATHNAMELENGTH) { t++; cnt++; } //crawl counter forward.
{t++;cnt++;} //crawl counter forward.
} }
if(cnt<MAXPATHNAMELENGTH-FILENAME_LENGTH) if (cnt < MAXPATHNAMELENGTH - FILENAME_LENGTH)
file.getFilename(t); file.getFilename(t);
else else
t[0]=0; t[0] = 0;
} }
void CardReader::openFile(char* name,bool read, bool replace_current/*=true*/) void CardReader::openFile(char* name, bool read, bool replace_current/*=true*/) {
{ if (!cardOK) return;
if(!cardOK) if (file.isOpen()) { //replacing current file by new file, or subfile call
return; if (!replace_current) {
if(file.isOpen()) //replacing current file by new file, or subfile call if (file_subcall_ctr > SD_PROCEDURE_DEPTH - 1) {
{
if(!replace_current)
{
if((int)file_subcall_ctr>(int)SD_PROCEDURE_DEPTH-1)
{
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORPGM("trying to call sub-gcode files with too many levels. MAX level is:"); SERIAL_ERRORPGM("trying to call sub-gcode files with too many levels. MAX level is:");
SERIAL_ERRORLN(SD_PROCEDURE_DEPTH); SERIAL_ERRORLN(SD_PROCEDURE_DEPTH);
kill(); kill();
return; return;
} }
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("SUBROUTINE CALL target:\""); SERIAL_ECHOPGM("SUBROUTINE CALL target:\"");
SERIAL_ECHO(name); SERIAL_ECHO(name);
SERIAL_ECHOPGM("\" parent:\""); SERIAL_ECHOPGM("\" parent:\"");
//store current filename and position //store current filename and position
getAbsFilename(filenames[file_subcall_ctr]); getAbsFilename(filenames[file_subcall_ctr]);
SERIAL_ECHO(filenames[file_subcall_ctr]); SERIAL_ECHO(filenames[file_subcall_ctr]);
SERIAL_ECHOPGM("\" pos"); SERIAL_ECHOPGM("\" pos");
SERIAL_ECHOLN(sdpos); SERIAL_ECHOLN(sdpos);
filespos[file_subcall_ctr]=sdpos; filespos[file_subcall_ctr] = sdpos;
file_subcall_ctr++; file_subcall_ctr++;
} }
else else {
{
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("Now doing file: "); SERIAL_ECHOPGM("Now doing file: ");
SERIAL_ECHOLN(name); SERIAL_ECHOLN(name);
} }
file.close(); file.close();
} }
else //opening fresh file else { //opening fresh file
{ file_subcall_ctr = 0; //resetting procedure depth in case user cancels print while in procedure
file_subcall_ctr=0; //resetting procedure depth in case user cancels print while in procedure
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("Now fresh file: "); SERIAL_ECHOPGM("Now fresh file: ");
SERIAL_ECHOLN(name); SERIAL_ECHOLN(name);
} }
sdprinting = false; sdprinting = false;
SdFile myDir; SdFile myDir;
curDir=&root; curDir = &root;
char *fname=name; char *fname = name;
char *dirname_start,*dirname_end; char *dirname_start, *dirname_end;
if(name[0]=='/') if (name[0] == '/') {
{ dirname_start = &name[1];
dirname_start=strchr(name,'/')+1; while(dirname_start > 0) {
while(dirname_start>0) dirname_end = strchr(dirname_start, '/');
{ //SERIAL_ECHO("start:");SERIAL_ECHOLN((int)(dirname_start - name));
dirname_end=strchr(dirname_start,'/'); //SERIAL_ECHO("end :");SERIAL_ECHOLN((int)(dirname_end - name));
//SERIAL_ECHO("start:");SERIAL_ECHOLN((int)(dirname_start-name)); if (dirname_end > 0 && dirname_end > dirname_start) {
//SERIAL_ECHO("end :");SERIAL_ECHOLN((int)(dirname_end-name));
if(dirname_end>0 && dirname_end>dirname_start)
{
char subdirname[FILENAME_LENGTH]; char subdirname[FILENAME_LENGTH];
strncpy(subdirname, dirname_start, dirname_end-dirname_start); strncpy(subdirname, dirname_start, dirname_end - dirname_start);
subdirname[dirname_end-dirname_start]=0; subdirname[dirname_end - dirname_start] = 0;
SERIAL_ECHOLN(subdirname); SERIAL_ECHOLN(subdirname);
if(!myDir.open(curDir,subdirname,O_READ)) if (!myDir.open(curDir, subdirname, O_READ)) {
{
SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL); SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
SERIAL_PROTOCOL(subdirname); SERIAL_PROTOCOL(subdirname);
SERIAL_PROTOCOLLNPGM("."); SERIAL_PROTOCOLLNPGM(".");
return; return;
} }
else else {
{
//SERIAL_ECHOLN("dive ok"); //SERIAL_ECHOLN("dive ok");
} }
curDir=&myDir; curDir = &myDir;
dirname_start=dirname_end+1; dirname_start = dirname_end + 1;
} }
else // the reminder after all /fsa/fdsa/ is the filename else { // the remainder after all /fsa/fdsa/ is the filename
{ fname = dirname_start;
fname=dirname_start; //SERIAL_ECHOLN("remainder");
//SERIAL_ECHOLN("remaider");
//SERIAL_ECHOLN(fname); //SERIAL_ECHOLN(fname);
break; break;
} }
} }
} }
else //relative path else { //relative path
{ curDir = &workDir;
curDir=&workDir;
} }
if(read)
{ if (read) {
if (file.open(curDir, fname, O_READ)) if (file.open(curDir, fname, O_READ)) {
{
filesize = file.fileSize(); filesize = file.fileSize();
SERIAL_PROTOCOLPGM(MSG_SD_FILE_OPENED); SERIAL_PROTOCOLPGM(MSG_SD_FILE_OPENED);
SERIAL_PROTOCOL(fname); SERIAL_PROTOCOL(fname);
SERIAL_PROTOCOLPGM(MSG_SD_SIZE); SERIAL_PROTOCOLPGM(MSG_SD_SIZE);
SERIAL_PROTOCOLLN(filesize); SERIAL_PROTOCOLLN(filesize);
sdpos = 0; sdpos = 0;
SERIAL_PROTOCOLLNPGM(MSG_SD_FILE_SELECTED); SERIAL_PROTOCOLLNPGM(MSG_SD_FILE_SELECTED);
getfilename(0, fname); getfilename(0, fname);
lcd_setstatus(longFilename[0] ? longFilename : fname); lcd_setstatus(longFilename[0] ? longFilename : fname);
} }
else else {
{
SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL); SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
SERIAL_PROTOCOL(fname); SERIAL_PROTOCOL(fname);
SERIAL_PROTOCOLLNPGM("."); SERIAL_PROTOCOLLNPGM(".");
} }
} }
else else { //write
{ //write if (!file.open(curDir, fname, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) {
if (!file.open(curDir, fname, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))
{
SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL); SERIAL_PROTOCOLPGM(MSG_SD_OPEN_FILE_FAIL);
SERIAL_PROTOCOL(fname); SERIAL_PROTOCOL(fname);
SERIAL_PROTOCOLLNPGM("."); SERIAL_PROTOCOLLNPGM(".");
} }
else else {
{
saving = true; saving = true;
SERIAL_PROTOCOLPGM(MSG_SD_WRITE_TO_FILE); SERIAL_PROTOCOLPGM(MSG_SD_WRITE_TO_FILE);
SERIAL_PROTOCOLLN(name); SERIAL_PROTOCOLLN(name);
lcd_setstatus(fname); lcd_setstatus(fname);
} }
} }
} }
void CardReader::removeFile(char* name) void CardReader::removeFile(char* name) {
{ if (!cardOK) return;
if(!cardOK)
return;
file.close(); file.close();
sdprinting = false; sdprinting = false;
SdFile myDir; SdFile myDir;
curDir=&root; curDir = &root;
char *fname=name; char *fname = name;
char *dirname_start,*dirname_end; char *dirname_start, *dirname_end;
if(name[0]=='/') if (name[0] == '/') {
{ dirname_start = strchr(name, '/') + 1;
dirname_start=strchr(name,'/')+1; while (dirname_start > 0) {
while(dirname_start>0) dirname_end = strchr(dirname_start, '/');
{ //SERIAL_ECHO("start:");SERIAL_ECHOLN((int)(dirname_start - name));
dirname_end=strchr(dirname_start,'/'); //SERIAL_ECHO("end :");SERIAL_ECHOLN((int)(dirname_end - name));
//SERIAL_ECHO("start:");SERIAL_ECHOLN((int)(dirname_start-name)); if (dirname_end > 0 && dirname_end > dirname_start) {
//SERIAL_ECHO("end :");SERIAL_ECHOLN((int)(dirname_end-name));
if(dirname_end>0 && dirname_end>dirname_start)
{
char subdirname[FILENAME_LENGTH]; char subdirname[FILENAME_LENGTH];
strncpy(subdirname, dirname_start, dirname_end-dirname_start); strncpy(subdirname, dirname_start, dirname_end - dirname_start);
subdirname[dirname_end-dirname_start]=0; subdirname[dirname_end - dirname_start] = 0;
SERIAL_ECHOLN(subdirname); SERIAL_ECHOLN(subdirname);
if(!myDir.open(curDir,subdirname,O_READ)) if (!myDir.open(curDir, subdirname, O_READ)) {
{
SERIAL_PROTOCOLPGM("open failed, File: "); SERIAL_PROTOCOLPGM("open failed, File: ");
SERIAL_PROTOCOL(subdirname); SERIAL_PROTOCOL(subdirname);
SERIAL_PROTOCOLLNPGM("."); SERIAL_PROTOCOLLNPGM(".");
return; return;
} }
else else {
{
//SERIAL_ECHOLN("dive ok"); //SERIAL_ECHOLN("dive ok");
} }
curDir=&myDir; curDir = &myDir;
dirname_start=dirname_end+1; dirname_start = dirname_end + 1;
} }
else // the reminder after all /fsa/fdsa/ is the filename else { // the remainder after all /fsa/fdsa/ is the filename
{ fname = dirname_start;
fname=dirname_start; //SERIAL_ECHOLN("remainder");
//SERIAL_ECHOLN("remaider");
//SERIAL_ECHOLN(fname); //SERIAL_ECHOLN(fname);
break; break;
} }
} }
} }
else //relative path else { // relative path
{ curDir = &workDir;
curDir=&workDir; }
if (file.remove(curDir, fname)) {
SERIAL_PROTOCOLPGM("File deleted:");
SERIAL_PROTOCOLLN(fname);
sdpos = 0;
}
else {
SERIAL_PROTOCOLPGM("Deletion failed, File: ");
SERIAL_PROTOCOL(fname);
SERIAL_PROTOCOLLNPGM(".");
} }
if (file.remove(curDir, fname))
{
SERIAL_PROTOCOLPGM("File deleted:");
SERIAL_PROTOCOLLN(fname);
sdpos = 0;
}
else
{
SERIAL_PROTOCOLPGM("Deletion failed, File: ");
SERIAL_PROTOCOL(fname);
SERIAL_PROTOCOLLNPGM(".");
}
} }
void CardReader::getStatus() void CardReader::getStatus() {
{ if (cardOK) {
if(cardOK){
SERIAL_PROTOCOLPGM(MSG_SD_PRINTING_BYTE); SERIAL_PROTOCOLPGM(MSG_SD_PRINTING_BYTE);
SERIAL_PROTOCOL(sdpos); SERIAL_PROTOCOL(sdpos);
SERIAL_PROTOCOLPGM("/"); SERIAL_PROTOCOLPGM("/");
SERIAL_PROTOCOLLN(filesize); SERIAL_PROTOCOLLN(filesize);
} }
else{ else {
SERIAL_PROTOCOLLNPGM(MSG_SD_NOT_PRINTING); SERIAL_PROTOCOLLNPGM(MSG_SD_NOT_PRINTING);
} }
} }
void CardReader::write_command(char *buf)
{ void CardReader::write_command(char *buf) {
char* begin = buf; char* begin = buf;
char* npos = 0; char* npos = 0;
char* end = buf + strlen(buf) - 1; char* end = buf + strlen(buf) - 1;
file.writeError = false; file.writeError = false;
if((npos = strchr(buf, 'N')) != NULL) if ((npos = strchr(buf, 'N')) != NULL) {
{
begin = strchr(npos, ' ') + 1; begin = strchr(npos, ' ') + 1;
end = strchr(npos, '*') - 1; end = strchr(npos, '*') - 1;
} }
@ -484,162 +390,129 @@ void CardReader::write_command(char *buf)
end[2] = '\n'; end[2] = '\n';
end[3] = '\0'; end[3] = '\0';
file.write(begin); file.write(begin);
if (file.writeError) if (file.writeError) {
{
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_SD_ERR_WRITE_TO_FILE); SERIAL_ERRORLNPGM(MSG_SD_ERR_WRITE_TO_FILE);
} }
} }
void CardReader::checkautostart(bool force) {
if (!force && (!autostart_stilltocheck || autostart_atmillis < millis()))
return;
void CardReader::checkautostart(bool force) autostart_stilltocheck = false;
{
if(!force) if (!cardOK) {
{
if(!autostart_stilltocheck)
return;
if(autostart_atmillis<millis())
return;
}
autostart_stilltocheck=false;
if(!cardOK)
{
initsd(); initsd();
if(!cardOK) //fail if (!cardOK) return; // fail
return;
} }
char autoname[30]; char autoname[30];
sprintf_P(autoname, PSTR("auto%i.g"), autostart_index); sprintf_P(autoname, PSTR("auto%i.g"), autostart_index);
for(int8_t i=0;i<(int8_t)strlen(autoname);i++) for (int8_t i = 0; i < (int8_t)strlen(autoname); i++) autoname[i] = tolower(autoname[i]);
autoname[i]=tolower(autoname[i]);
dir_t p; dir_t p;
root.rewind(); root.rewind();
bool found=false;
while (root.readDir(p, NULL) > 0)
{
for(int8_t i=0;i<(int8_t)strlen((char*)p.name);i++)
p.name[i]=tolower(p.name[i]);
//Serial.print((char*)p.name);
//Serial.print(" ");
//Serial.println(autoname);
if(p.name[9]!='~') //skip safety copies
if(strncmp((char*)p.name,autoname,5)==0)
{
char cmd[30];
bool found = false;
while (root.readDir(p, NULL) > 0) {
for (int8_t i = 0; i < (int8_t)strlen((char*)p.name); i++) p.name[i] = tolower(p.name[i]);
if (p.name[9] != '~' && strncmp((char*)p.name, autoname, 5) == 0) {
char cmd[30];
sprintf_P(cmd, PSTR("M23 %s"), autoname); sprintf_P(cmd, PSTR("M23 %s"), autoname);
enquecommand(cmd); enquecommand(cmd);
enquecommand_P(PSTR("M24")); enquecommands_P(PSTR("M24"));
found=true; found = true;
} }
} }
if(!found) if (!found)
autostart_index=-1; autostart_index = -1;
else else
autostart_index++; autostart_index++;
} }
void CardReader::closefile(bool store_location) void CardReader::closefile(bool store_location) {
{
file.sync(); file.sync();
file.close(); file.close();
saving = false; saving = logging = false;
logging = false;
if (store_location) {
if(store_location)
{
//future: store printer state, filename and position for continuing a stopped print //future: store printer state, filename and position for continuing a stopped print
// so one can unplug the printer and continue printing the next day. // so one can unplug the printer and continue printing the next day.
} }
} }
void CardReader::getfilename(uint16_t nr, const char * const match/*=NULL*/) /**
{ * Get the name of a file in the current directory by index
curDir=&workDir; */
lsAction=LS_GetFilename; void CardReader::getfilename(uint16_t nr, const char * const match/*=NULL*/) {
nrFiles=nr; curDir = &workDir;
lsAction = LS_GetFilename;
nrFiles = nr;
curDir->rewind(); curDir->rewind();
lsDive("",*curDir,match); lsDive("", *curDir, match);
} }
uint16_t CardReader::getnrfilenames() uint16_t CardReader::getnrfilenames() {
{ curDir = &workDir;
curDir=&workDir; lsAction = LS_Count;
lsAction=LS_Count; nrFiles = 0;
nrFiles=0;
curDir->rewind(); curDir->rewind();
lsDive("",*curDir); lsDive("", *curDir);
//SERIAL_ECHOLN(nrFiles); //SERIAL_ECHOLN(nrFiles);
return nrFiles; return nrFiles;
} }
void CardReader::chdir(const char * relpath) void CardReader::chdir(const char * relpath) {
{
SdFile newfile; SdFile newfile;
SdFile *parent=&root; SdFile *parent = &root;
if(workDir.isOpen()) if (workDir.isOpen()) parent = &workDir;
parent=&workDir;
if (!newfile.open(*parent, relpath, O_READ)) {
if(!newfile.open(*parent,relpath, O_READ)) SERIAL_ECHO_START;
{ SERIAL_ECHOPGM(MSG_SD_CANT_ENTER_SUBDIR);
SERIAL_ECHO_START; SERIAL_ECHOLN(relpath);
SERIAL_ECHOPGM(MSG_SD_CANT_ENTER_SUBDIR);
SERIAL_ECHOLN(relpath);
} }
else else {
{
if (workDirDepth < MAX_DIR_DEPTH) { if (workDirDepth < MAX_DIR_DEPTH) {
for (int d = ++workDirDepth; d--;) ++workDirDepth;
workDirParents[d+1] = workDirParents[d]; for (int d = workDirDepth; d--;) workDirParents[d + 1] = workDirParents[d];
workDirParents[0]=*parent; workDirParents[0] = *parent;
} }
workDir=newfile; workDir = newfile;
} }
} }
void CardReader::updir() void CardReader::updir() {
{ if (workDirDepth > 0) {
if(workDirDepth > 0)
{
--workDirDepth; --workDirDepth;
workDir = workDirParents[0]; workDir = workDirParents[0];
int d;
for (int d = 0; d < workDirDepth; d++) for (int d = 0; d < workDirDepth; d++)
workDirParents[d] = workDirParents[d+1]; workDirParents[d] = workDirParents[d+1];
} }
} }
void CardReader::printingHasFinished() {
void CardReader::printingHasFinished() st_synchronize();
{ if (file_subcall_ctr > 0) { // Heading up to a parent file that called current as a procedure.
st_synchronize(); file.close();
if(file_subcall_ctr>0) //heading up to a parent file that called current as a procedure. file_subcall_ctr--;
{ openFile(filenames[file_subcall_ctr], true, true);
file.close(); setIndex(filespos[file_subcall_ctr]);
file_subcall_ctr--; startFileprint();
openFile(filenames[file_subcall_ctr],true,true); }
setIndex(filespos[file_subcall_ctr]); else {
startFileprint(); quickStop();
} file.close();
else sdprinting = false;
{ if (SD_FINISHED_STEPPERRELEASE) {
quickStop(); //finishAndDisableSteppers();
file.close(); enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
sdprinting = false;
if(SD_FINISHED_STEPPERRELEASE)
{
//finishAndDisableSteppers();
enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND));
}
autotempShutdown();
} }
autotempShutdown();
}
} }
#endif //SDSUPPORT #endif //SDSUPPORT

View file

@ -3,21 +3,21 @@
#ifdef SDSUPPORT #ifdef SDSUPPORT
#define MAX_DIR_DEPTH 10 #define MAX_DIR_DEPTH 10 // Maximum folder depth
#include "SdFile.h" #include "SdFile.h"
enum LsAction {LS_SerialPrint,LS_Count,LS_GetFilename}; enum LsAction { LS_SerialPrint, LS_Count, LS_GetFilename };
class CardReader
{ class CardReader {
public: public:
CardReader(); CardReader();
void initsd(); void initsd();
void write_command(char *buf); void write_command(char *buf);
//files auto[0-9].g on the sd card are performed in a row //files auto[0-9].g on the sd card are performed in a row
//this is to delay autostart and hence the initialisaiton of the sd card to some seconds after the normal init, so the device is available quick after a reset //this is to delay autostart and hence the initialisaiton of the sd card to some seconds after the normal init, so the device is available quick after a reset
void checkautostart(bool x); void checkautostart(bool x);
void openFile(char* name,bool read,bool replace_current=true); void openFile(char* name,bool read,bool replace_current=true);
void openLogFile(char* name); void openLogFile(char* name);
void removeFile(char* name); void removeFile(char* name);
@ -30,9 +30,8 @@ public:
void getfilename(uint16_t nr, const char* const match=NULL); void getfilename(uint16_t nr, const char* const match=NULL);
uint16_t getnrfilenames(); uint16_t getnrfilenames();
void getAbsFilename(char *t); void getAbsFilename(char *t);
void ls(); void ls();
void chdir(const char * relpath); void chdir(const char * relpath);
@ -41,56 +40,52 @@ public:
FORCE_INLINE bool isFileOpen() { return file.isOpen(); } FORCE_INLINE bool isFileOpen() { return file.isOpen(); }
FORCE_INLINE bool eof() { return sdpos>=filesize ;}; FORCE_INLINE bool eof() { return sdpos >= filesize; }
FORCE_INLINE int16_t get() { sdpos = file.curPosition();return (int16_t)file.read();}; FORCE_INLINE int16_t get() { sdpos = file.curPosition(); return (int16_t)file.read(); }
FORCE_INLINE void setIndex(long index) {sdpos = index;file.seekSet(index);}; FORCE_INLINE void setIndex(long index) { sdpos = index; file.seekSet(index); }
FORCE_INLINE uint8_t percentDone(){if(!isFileOpen()) return 0; if(filesize) return sdpos/((filesize+99)/100); else return 0;}; FORCE_INLINE uint8_t percentDone() { return (isFileOpen() && filesize) ? sdpos / ((filesize + 99) / 100) : 0; }
FORCE_INLINE char* getWorkDirName(){workDir.getFilename(filename);return filename;}; FORCE_INLINE char* getWorkDirName() { workDir.getFilename(filename); return filename; }
public: public:
bool saving; bool saving, logging, sdprinting, cardOK, filenameIsDir;
bool logging; char filename[FILENAME_LENGTH], longFilename[LONG_FILENAME_LENGTH];
bool sdprinting;
bool cardOK;
char filename[FILENAME_LENGTH];
char longFilename[LONG_FILENAME_LENGTH];
bool filenameIsDir;
int autostart_index; int autostart_index;
private: private:
SdFile root,*curDir,workDir,workDirParents[MAX_DIR_DEPTH]; SdFile root, *curDir, workDir, workDirParents[MAX_DIR_DEPTH];
uint16_t workDirDepth; uint16_t workDirDepth;
Sd2Card card; Sd2Card card;
SdVolume volume; SdVolume volume;
SdFile file; SdFile file;
#define SD_PROCEDURE_DEPTH 1 #define SD_PROCEDURE_DEPTH 1
#define MAXPATHNAMELENGTH (FILENAME_LENGTH*MAX_DIR_DEPTH+MAX_DIR_DEPTH+1) #define MAXPATHNAMELENGTH (FILENAME_LENGTH*MAX_DIR_DEPTH + MAX_DIR_DEPTH + 1)
uint8_t file_subcall_ctr; uint8_t file_subcall_ctr;
uint32_t filespos[SD_PROCEDURE_DEPTH]; uint32_t filespos[SD_PROCEDURE_DEPTH];
char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH]; char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH];
uint32_t filesize; uint32_t filesize;
//int16_t n;
unsigned long autostart_atmillis; unsigned long autostart_atmillis;
uint32_t sdpos ; uint32_t sdpos;
bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware. bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware.
LsAction lsAction; //stored for recursion. LsAction lsAction; //stored for recursion.
int16_t nrFiles; //counter for the files in the current directory and recycled as position counter for getting the nrFiles'th name in the directory. uint16_t nrFiles; //counter for the files in the current directory and recycled as position counter for getting the nrFiles'th name in the directory.
char* diveDirName; char* diveDirName;
void lsDive(const char *prepend, SdFile parent, const char * const match=NULL); void lsDive(const char *prepend, SdFile parent, const char * const match=NULL);
}; };
extern CardReader card; extern CardReader card;
#define IS_SD_PRINTING (card.sdprinting) #define IS_SD_PRINTING (card.sdprinting)
#if (SDCARDDETECT > -1) #if (SDCARDDETECT > -1)
# ifdef SDCARDDETECTINVERTED #ifdef SDCARDDETECTINVERTED
# define IS_SD_INSERTED (READ(SDCARDDETECT)!=0) #define IS_SD_INSERTED (READ(SDCARDDETECT) != 0)
# else #else
# define IS_SD_INSERTED (READ(SDCARDDETECT)==0) #define IS_SD_INSERTED (READ(SDCARDDETECT) == 0)
# endif //SDCARDTETECTINVERTED #endif
#else #else
//If we don't have a card detect line, aways asume the card is inserted //No card detect line? Assume the card is inserted.
# define IS_SD_INSERTED true #define IS_SD_INSERTED true
#endif #endif
#else #else
@ -98,4 +93,5 @@ extern CardReader card;
#define IS_SD_PRINTING (false) #define IS_SD_PRINTING (false)
#endif //SDSUPPORT #endif //SDSUPPORT
#endif
#endif //__CARDREADER_H

View file

@ -1,59 +1,58 @@
#include "Configuration.h" #include "Configuration.h"
#ifdef DIGIPOT_I2C #ifdef DIGIPOT_I2C
#include "Stream.h" #include "Stream.h"
#include "utility/twi.h" #include "utility/twi.h"
#include "Wire.h" #include "Wire.h"
// Settings for the I2C based DIGIPOT (MCP4451) on Azteeg X3 Pro // Settings for the I2C based DIGIPOT (MCP4451) on Azteeg X3 Pro
#if MB(5DPRINT) #if MB(5DPRINT)
#define DIGIPOT_I2C_FACTOR 117.96 #define DIGIPOT_I2C_FACTOR 117.96
#define DIGIPOT_I2C_MAX_CURRENT 1.736 #define DIGIPOT_I2C_MAX_CURRENT 1.736
#else #else
#define DIGIPOT_I2C_FACTOR 106.7 #define DIGIPOT_I2C_FACTOR 106.7
#define DIGIPOT_I2C_MAX_CURRENT 2.5 #define DIGIPOT_I2C_MAX_CURRENT 2.5
#endif #endif
static byte current_to_wiper( float current ){ static byte current_to_wiper(float current) {
return byte(ceil(float((DIGIPOT_I2C_FACTOR*current)))); return byte(ceil(float((DIGIPOT_I2C_FACTOR*current))));
} }
static void i2c_send(byte addr, byte a, byte b) static void i2c_send(byte addr, byte a, byte b) {
{ Wire.beginTransmission(addr);
Wire.beginTransmission(addr); Wire.write(a);
Wire.write(a); Wire.write(b);
Wire.write(b); Wire.endTransmission();
Wire.endTransmission();
} }
// This is for the MCP4451 I2C based digipot // This is for the MCP4451 I2C based digipot
void digipot_i2c_set_current( int channel, float current ) void digipot_i2c_set_current(int channel, float current) {
{ current = min( (float) max( current, 0.0f ), DIGIPOT_I2C_MAX_CURRENT);
current = min( (float) max( current, 0.0f ), DIGIPOT_I2C_MAX_CURRENT); // these addresses are specific to Azteeg X3 Pro, can be set to others,
// these addresses are specific to Azteeg X3 Pro, can be set to others, // In this case first digipot is at address A0=0, A1= 0, second one is at A0=0, A1= 1
// In this case first digipot is at address A0=0, A1= 0, second one is at A0=0, A1= 1 byte addr = 0x2C; // channel 0-3
byte addr= 0x2C; // channel 0-3 if (channel >= 4) {
if(channel >= 4) { addr = 0x2E; // channel 4-7
addr= 0x2E; // channel 4-7 channel -= 4;
channel-= 4; }
}
// Initial setup // Initial setup
i2c_send( addr, 0x40, 0xff ); i2c_send(addr, 0x40, 0xff);
i2c_send( addr, 0xA0, 0xff ); i2c_send(addr, 0xA0, 0xff);
// Set actual wiper value // Set actual wiper value
byte addresses[4] = { 0x00, 0x10, 0x60, 0x70 }; byte addresses[4] = { 0x00, 0x10, 0x60, 0x70 };
i2c_send( addr, addresses[channel], current_to_wiper(current) ); i2c_send(addr, addresses[channel], current_to_wiper(current));
} }
void digipot_i2c_init() void digipot_i2c_init() {
{ const float digipot_motor_current[] = DIGIPOT_I2C_MOTOR_CURRENTS;
const float digipot_motor_current[] = DIGIPOT_I2C_MOTOR_CURRENTS; Wire.begin();
Wire.begin(); // setup initial currents as defined in Configuration_adv.h
// setup initial currents as defined in Configuration_adv.h for(int i = 0; i <= sizeof(digipot_motor_current) / sizeof(float); i++) {
for(int i=0;i<=sizeof(digipot_motor_current)/sizeof(float);i++) { digipot_i2c_set_current(i, digipot_motor_current[i]);
digipot_i2c_set_current(i, digipot_motor_current[i]); }
}
} }
#endif
#endif //DIGIPOT_I2C

View file

@ -21,17 +21,13 @@
**/ **/
#ifdef ULTIPANEL #ifdef ULTIPANEL
#define BLEN_A 0 #define BLEN_A 0
#define BLEN_B 1 #define BLEN_B 1
#define BLEN_C 2 #define BLEN_C 2
#define EN_A (1<<BLEN_A) #define EN_A (1<<BLEN_A)
#define EN_B (1<<BLEN_B) #define EN_B (1<<BLEN_B)
#define EN_C (1<<BLEN_C) #define EN_C (1<<BLEN_C)
#define encrot0 0 #define LCD_CLICKED (buttons&EN_C)
#define encrot1 2
#define encrot2 3
#define encrot3 1
#define LCD_CLICKED (buttons&EN_C)
#endif #endif
#include <U8glib.h> #include <U8glib.h>
@ -92,6 +88,9 @@ U8GLIB_ST7920_128X64_RRD u8g(0);
#elif defined(MAKRPANEL) #elif defined(MAKRPANEL)
// The MaKrPanel display, ST7565 controller as well // The MaKrPanel display, ST7565 controller as well
U8GLIB_NHD_C12864 u8g(DOGLCD_CS, DOGLCD_A0); U8GLIB_NHD_C12864 u8g(DOGLCD_CS, DOGLCD_A0);
#elif defined(VIKI2) || defined(miniVIKI)
// Mini Viki and Viki 2.0 LCD, ST7565 controller as well
U8GLIB_NHD_C12864 u8g(DOGLCD_CS, DOGLCD_A0);
#else #else
// for regular DOGM128 display with HW-SPI // for regular DOGM128 display with HW-SPI
U8GLIB_DOGM128 u8g(DOGLCD_CS, DOGLCD_A0); // HW-SPI Com: CS, A0 U8GLIB_DOGM128 u8g(DOGLCD_CS, DOGLCD_A0); // HW-SPI Com: CS, A0
@ -312,7 +311,7 @@ static void lcd_implementation_drawmenu_generic(uint8_t row, const char* pstr, c
static void _drawmenu_setting_edit_generic(uint8_t row, const char* pstr, char pre_char, const char* data, bool pgm) { static void _drawmenu_setting_edit_generic(uint8_t row, const char* pstr, char pre_char, const char* data, bool pgm) {
char c; char c;
uint8_t n = LCD_WIDTH - 2 - (pgm ? strlen_P(data) : (strlen(data))); uint8_t n = LCD_WIDTH - 2 - (pgm ? lcd_strlen_P(data) : (lcd_strlen((char*)data)));
lcd_implementation_mark_as_selected(row, pre_char); lcd_implementation_mark_as_selected(row, pre_char);
@ -374,18 +373,18 @@ void lcd_implementation_drawedit(const char* pstr, char* value) {
uint8_t char_width = DOG_CHAR_WIDTH; uint8_t char_width = DOG_CHAR_WIDTH;
#ifdef USE_BIG_EDIT_FONT #ifdef USE_BIG_EDIT_FONT
if (strlen_P(pstr) <= LCD_WIDTH_EDIT - 1) { if (lcd_strlen_P(pstr) <= LCD_WIDTH_EDIT - 1) {
u8g.setFont(FONT_MENU_EDIT); u8g.setFont(FONT_MENU_EDIT);
lcd_width = LCD_WIDTH_EDIT + 1; lcd_width = LCD_WIDTH_EDIT + 1;
char_width = DOG_CHAR_WIDTH_EDIT; char_width = DOG_CHAR_WIDTH_EDIT;
if (strlen_P(pstr) >= LCD_WIDTH_EDIT - strlen(value)) rows = 2; if (lcd_strlen_P(pstr) >= LCD_WIDTH_EDIT - lcd_strlen(value)) rows = 2;
} }
else { else {
u8g.setFont(FONT_MENU); u8g.setFont(FONT_MENU);
} }
#endif #endif
if (strlen_P(pstr) > LCD_WIDTH - 2 - strlen(value)) rows = 2; if (lcd_strlen_P(pstr) > LCD_WIDTH - 2 - lcd_strlen(value)) rows = 2;
const float kHalfChar = DOG_CHAR_HEIGHT_EDIT / 2; const float kHalfChar = DOG_CHAR_HEIGHT_EDIT / 2;
float rowHeight = u8g.getHeight() / (rows + 1); // 1/(rows+1) = 1/2 or 1/3 float rowHeight = u8g.getHeight() / (rows + 1); // 1/(rows+1) = 1/2 or 1/3
@ -393,7 +392,7 @@ void lcd_implementation_drawedit(const char* pstr, char* value) {
u8g.setPrintPos(0, rowHeight + kHalfChar); u8g.setPrintPos(0, rowHeight + kHalfChar);
lcd_printPGM(pstr); lcd_printPGM(pstr);
u8g.print(':'); u8g.print(':');
u8g.setPrintPos((lcd_width-1-strlen(value)) * char_width, rows * rowHeight + kHalfChar); u8g.setPrintPos((lcd_width-1-lcd_strlen(value)) * char_width, rows * rowHeight + kHalfChar);
u8g.print(value); u8g.print(value);
} }

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -122,6 +122,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 1 #define TEMP_SENSOR_0 1
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 0 #define TEMP_SENSOR_BED 0
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -139,6 +140,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -147,6 +149,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 260 #define HEATER_0_MAXTEMP 260
#define HEATER_1_MAXTEMP 260 #define HEATER_1_MAXTEMP 260
#define HEATER_2_MAXTEMP 260 #define HEATER_2_MAXTEMP 260
#define HEATER_3_MAXTEMP 260
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -351,8 +354,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -583,10 +587,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
#define REPRAP_DISCOUNT_SMART_CONTROLLER #define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -620,6 +634,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -284,6 +284,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -124,6 +124,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 5 #define TEMP_SENSOR_0 5
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 5 #define TEMP_SENSOR_BED 5
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -141,6 +142,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -149,6 +151,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -357,10 +360,11 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define INVERT_X_DIR false // for Mendel set to false, for Orca set to true #define INVERT_X_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -593,10 +597,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -630,6 +644,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -284,6 +284,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -142,6 +142,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 1 #define TEMP_SENSOR_0 1
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 1 #define TEMP_SENSOR_BED 1
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -159,6 +160,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -167,6 +169,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -380,8 +383,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstop s when homing; 1=MAX, -1=MIN // Sets direction of endstop s when homing; 1=MAX, -1=MIN
@ -586,10 +590,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -623,6 +637,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -78,6 +78,7 @@
#define EXTRUDER_0_AUTO_FAN_PIN -1 #define EXTRUDER_0_AUTO_FAN_PIN -1
#define EXTRUDER_1_AUTO_FAN_PIN -1 #define EXTRUDER_1_AUTO_FAN_PIN -1
#define EXTRUDER_2_AUTO_FAN_PIN -1 #define EXTRUDER_2_AUTO_FAN_PIN -1
#define EXTRUDER_3_AUTO_FAN_PIN -1
#define EXTRUDER_AUTO_FAN_TEMPERATURE 50 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50
#define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed #define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed
@ -286,6 +287,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@ -478,6 +484,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#define THERMISTORHEATER_2 TEMP_SENSOR_2 #define THERMISTORHEATER_2 TEMP_SENSOR_2
#define HEATER_2_USES_THERMISTOR #define HEATER_2_USES_THERMISTOR
#endif #endif
#if TEMP_SENSOR_3 > 0
#define THERMISTORHEATER_3 TEMP_SENSOR_3
#define HEATER_3_USES_THERMISTOR
#endif
#if TEMP_SENSOR_BED > 0 #if TEMP_SENSOR_BED > 0
#define THERMISTORBED TEMP_SENSOR_BED #define THERMISTORBED TEMP_SENSOR_BED
#define BED_USES_THERMISTOR #define BED_USES_THERMISTOR
@ -491,6 +501,9 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#if TEMP_SENSOR_2 == -1 #if TEMP_SENSOR_2 == -1
#define HEATER_2_USES_AD595 #define HEATER_2_USES_AD595
#endif #endif
#if TEMP_SENSOR_3 == -1
#define HEATER_3_USES_AD595
#endif
#if TEMP_SENSOR_BED == -1 #if TEMP_SENSOR_BED == -1
#define BED_USES_AD595 #define BED_USES_AD595
#endif #endif
@ -509,6 +522,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#undef HEATER_2_MINTEMP #undef HEATER_2_MINTEMP
#undef HEATER_2_MAXTEMP #undef HEATER_2_MAXTEMP
#endif #endif
#if TEMP_SENSOR_3 == 0
#undef HEATER_3_MINTEMP
#undef HEATER_3_MAXTEMP
#endif
#if TEMP_SENSOR_BED == 0 #if TEMP_SENSOR_BED == 0
#undef BED_MINTEMP #undef BED_MINTEMP
#undef BED_MAXTEMP #undef BED_MAXTEMP

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -125,6 +125,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 1 #define TEMP_SENSOR_0 1
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 0 #define TEMP_SENSOR_BED 0
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -142,6 +143,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -150,6 +152,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 260 #define HEATER_0_MAXTEMP 260
#define HEATER_1_MAXTEMP 260 #define HEATER_1_MAXTEMP 260
#define HEATER_2_MAXTEMP 260 #define HEATER_2_MAXTEMP 260
#define HEATER_3_MAXTEMP 260
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -351,12 +354,13 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DISABLE_E false // For all extruders #define DISABLE_E false // For all extruders
#define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled #define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled
#define INVERT_X_DIR true // for Mendel set to false, for Orca set to true #define INVERT_X_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -587,10 +591,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
#define REPRAP_DISCOUNT_SMART_CONTROLLER #define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -624,6 +638,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -284,6 +284,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -147,6 +147,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 -1 #define TEMP_SENSOR_0 -1
#define TEMP_SENSOR_1 -1 #define TEMP_SENSOR_1 -1
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 0 #define TEMP_SENSOR_BED 0
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -164,6 +165,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -172,6 +174,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -368,8 +371,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define INVERT_Z_DIR false #define INVERT_Z_DIR false
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -491,10 +495,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -535,6 +549,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -78,6 +78,7 @@
#define EXTRUDER_0_AUTO_FAN_PIN -1 #define EXTRUDER_0_AUTO_FAN_PIN -1
#define EXTRUDER_1_AUTO_FAN_PIN -1 #define EXTRUDER_1_AUTO_FAN_PIN -1
#define EXTRUDER_2_AUTO_FAN_PIN -1 #define EXTRUDER_2_AUTO_FAN_PIN -1
#define EXTRUDER_3_AUTO_FAN_PIN -1
#define EXTRUDER_AUTO_FAN_TEMPERATURE 50 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50
#define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed #define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed
@ -278,6 +279,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@ -472,6 +478,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#define THERMISTORHEATER_2 TEMP_SENSOR_2 #define THERMISTORHEATER_2 TEMP_SENSOR_2
#define HEATER_2_USES_THERMISTOR #define HEATER_2_USES_THERMISTOR
#endif #endif
#if TEMP_SENSOR_3 > 0
#define THERMISTORHEATER_3 TEMP_SENSOR_3
#define HEATER_3_USES_THERMISTOR
#endif
#if TEMP_SENSOR_BED > 0 #if TEMP_SENSOR_BED > 0
#define THERMISTORBED TEMP_SENSOR_BED #define THERMISTORBED TEMP_SENSOR_BED
#define BED_USES_THERMISTOR #define BED_USES_THERMISTOR
@ -485,6 +495,9 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#if TEMP_SENSOR_2 == -1 #if TEMP_SENSOR_2 == -1
#define HEATER_2_USES_AD595 #define HEATER_2_USES_AD595
#endif #endif
#if TEMP_SENSOR_3 == -1
#define HEATER_3_USES_AD595
#endif
#if TEMP_SENSOR_BED == -1 #if TEMP_SENSOR_BED == -1
#define BED_USES_AD595 #define BED_USES_AD595
#endif #endif
@ -503,6 +516,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#undef HEATER_2_MINTEMP #undef HEATER_2_MINTEMP
#undef HEATER_2_MAXTEMP #undef HEATER_2_MAXTEMP
#endif #endif
#if TEMP_SENSOR_3 == 0
#undef HEATER_3_MINTEMP
#undef HEATER_3_MAXTEMP
#endif
#if TEMP_SENSOR_BED == 0 #if TEMP_SENSOR_BED == 0
#undef BED_MINTEMP #undef BED_MINTEMP
#undef BED_MAXTEMP #undef BED_MAXTEMP

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -127,6 +127,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 1 #define TEMP_SENSOR_0 1
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 12 #define TEMP_SENSOR_BED 12
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -144,6 +145,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -152,6 +154,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the // If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
@ -352,9 +355,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define INVERT_X_DIR false // for Mendel set to false, for Orca set to true #define INVERT_X_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -561,10 +565,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -598,6 +612,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -78,6 +78,7 @@
#define EXTRUDER_0_AUTO_FAN_PIN -1 #define EXTRUDER_0_AUTO_FAN_PIN -1
#define EXTRUDER_1_AUTO_FAN_PIN -1 #define EXTRUDER_1_AUTO_FAN_PIN -1
#define EXTRUDER_2_AUTO_FAN_PIN -1 #define EXTRUDER_2_AUTO_FAN_PIN -1
#define EXTRUDER_3_AUTO_FAN_PIN -1
#define EXTRUDER_AUTO_FAN_TEMPERATURE 50 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50
#define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed #define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed
@ -282,6 +283,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@ -469,6 +475,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#define THERMISTORHEATER_2 TEMP_SENSOR_2 #define THERMISTORHEATER_2 TEMP_SENSOR_2
#define HEATER_2_USES_THERMISTOR #define HEATER_2_USES_THERMISTOR
#endif #endif
#if TEMP_SENSOR_3 > 0
#define THERMISTORHEATER_3 TEMP_SENSOR_3
#define HEATER_3_USES_THERMISTOR
#endif
#if TEMP_SENSOR_BED > 0 #if TEMP_SENSOR_BED > 0
#define THERMISTORBED TEMP_SENSOR_BED #define THERMISTORBED TEMP_SENSOR_BED
#define BED_USES_THERMISTOR #define BED_USES_THERMISTOR
@ -482,6 +492,9 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#if TEMP_SENSOR_2 == -1 #if TEMP_SENSOR_2 == -1
#define HEATER_2_USES_AD595 #define HEATER_2_USES_AD595
#endif #endif
#if TEMP_SENSOR_3 == -1
#define HEATER_3_USES_AD595
#endif
#if TEMP_SENSOR_BED == -1 #if TEMP_SENSOR_BED == -1
#define BED_USES_AD595 #define BED_USES_AD595
#endif #endif
@ -500,6 +513,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#undef HEATER_2_MINTEMP #undef HEATER_2_MINTEMP
#undef HEATER_2_MAXTEMP #undef HEATER_2_MAXTEMP
#endif #endif
#if TEMP_SENSOR_3 == 0
#undef HEATER_3_MINTEMP
#undef HEATER_3_MAXTEMP
#endif
#if TEMP_SENSOR_BED == 0 #if TEMP_SENSOR_BED == 0
#undef BED_MINTEMP #undef BED_MINTEMP
#undef BED_MAXTEMP #undef BED_MAXTEMP

View file

@ -1,4 +1,4 @@
#ifndef CONFIGURATION_H #ifndef CONFIGURATION_H
#define CONFIGURATION_H #define CONFIGURATION_H
#include "boards.h" #include "boards.h"
@ -126,6 +126,7 @@ Here are some standard links for getting your machine calibrated:
#define TEMP_SENSOR_0 5 #define TEMP_SENSOR_0 5
#define TEMP_SENSOR_1 0 #define TEMP_SENSOR_1 0
#define TEMP_SENSOR_2 0 #define TEMP_SENSOR_2 0
#define TEMP_SENSOR_3 0
#define TEMP_SENSOR_BED 5 #define TEMP_SENSOR_BED 5
// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
@ -143,6 +144,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MINTEMP 5 #define HEATER_0_MINTEMP 5
#define HEATER_1_MINTEMP 5 #define HEATER_1_MINTEMP 5
#define HEATER_2_MINTEMP 5 #define HEATER_2_MINTEMP 5
#define HEATER_3_MINTEMP 5
#define BED_MINTEMP 5 #define BED_MINTEMP 5
// When temperature exceeds max temp, your heater will be switched off. // When temperature exceeds max temp, your heater will be switched off.
@ -151,6 +153,7 @@ Here are some standard links for getting your machine calibrated:
#define HEATER_0_MAXTEMP 275 #define HEATER_0_MAXTEMP 275
#define HEATER_1_MAXTEMP 275 #define HEATER_1_MAXTEMP 275
#define HEATER_2_MAXTEMP 275 #define HEATER_2_MAXTEMP 275
#define HEATER_3_MAXTEMP 275
#define BED_MAXTEMP 150 #define BED_MAXTEMP 150
#define CONFIG_STEPPERS_TOSHIBA 1 #define CONFIG_STEPPERS_TOSHIBA 1
@ -353,11 +356,12 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled #define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled
#define INVERT_X_DIR false // for Mendel set to false, for Orca set to true #define INVERT_X_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_Y_DIR true // for Mendel set to true, for Orca set to false #define INVERT_Y_DIR true // for Mendel set to true, for Orca set to false
#define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true #define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true
#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
#define INVERT_E3_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false
// ENDSTOP SETTINGS: // ENDSTOP SETTINGS:
// Sets direction of endstops when homing; 1=MAX, -1=MIN // Sets direction of endstops when homing; 1=MAX, -1=MIN
@ -574,10 +578,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click //#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click
// PanelOne from T3P3 (via RAMPS 1.4 AUX2/AUX3)
// http://reprap.org/wiki/PanelOne
//#define PANEL_ONE
// The MaKr3d Makr-Panel with graphic controller and SD support // The MaKr3d Makr-Panel with graphic controller and SD support
// http://reprap.org/wiki/MaKr3d_MaKrPanel // http://reprap.org/wiki/MaKr3d_MaKrPanel
//#define MAKRPANEL //#define MAKRPANEL
// The Panucatt Devices Viki 2.0 and mini Viki with Graphic LCD
// http://panucatt.com
// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib
//#define VIKI2
//#define miniVIKI
// The RepRapDiscount Smart Controller (white PCB) // The RepRapDiscount Smart Controller (white PCB)
// http://reprap.org/wiki/RepRapDiscount_Smart_Controller // http://reprap.org/wiki/RepRapDiscount_Smart_Controller
//#define REPRAP_DISCOUNT_SMART_CONTROLLER //#define REPRAP_DISCOUNT_SMART_CONTROLLER
@ -611,6 +625,26 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define DEFAULT_LCD_CONTRAST 17 #define DEFAULT_LCD_CONTRAST 17
#endif #endif
#if defined(miniVIKI) || defined(VIKI2)
#define ULTRA_LCD //general LCD support, also 16x2
#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family)
#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
#ifdef miniVIKI
#define DEFAULT_LCD_CONTRAST 95
#else
#define DEFAULT_LCD_CONTRAST 40
#endif
#define ENCODER_PULSES_PER_STEP 4
#define ENCODER_STEPS_PER_MENU_ITEM 1
#endif
#if defined (PANEL_ONE)
#define SDSUPPORT
#define ULTIMAKERCONTROLLER
#endif
#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
#define DOGLCD #define DOGLCD
#define U8GLIB_ST7920 #define U8GLIB_ST7920

View file

@ -78,6 +78,7 @@
#define EXTRUDER_0_AUTO_FAN_PIN -1 #define EXTRUDER_0_AUTO_FAN_PIN -1
#define EXTRUDER_1_AUTO_FAN_PIN -1 #define EXTRUDER_1_AUTO_FAN_PIN -1
#define EXTRUDER_2_AUTO_FAN_PIN -1 #define EXTRUDER_2_AUTO_FAN_PIN -1
#define EXTRUDER_3_AUTO_FAN_PIN -1
#define EXTRUDER_AUTO_FAN_TEMPERATURE 50 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50
#define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed #define EXTRUDER_AUTO_FAN_SPEED 255 // == full speed
@ -283,6 +284,11 @@
//=============================Additional Features=========================== //=============================Additional Features===========================
//=========================================================================== //===========================================================================
#define ENCODER_RATE_MULTIPLIER // If defined, certain menu edit operations automatically multiply the steps when the encoder is moved quickly
#define ENCODER_10X_STEPS_PER_SEC 75 // If the encoder steps per sec exceed this value, multiple the steps moved by ten to quickly advance the value
#define ENCODER_100X_STEPS_PER_SEC 160 // If the encoder steps per sec exceed this value, multiple the steps moved by 100 to really quickly advance the value
//#define ENCODER_RATE_MULTIPLIER_DEBUG // If defined, output the encoder steps per second value
//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ //#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/
#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again #define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again
@ -472,6 +478,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#define THERMISTORHEATER_2 TEMP_SENSOR_2 #define THERMISTORHEATER_2 TEMP_SENSOR_2
#define HEATER_2_USES_THERMISTOR #define HEATER_2_USES_THERMISTOR
#endif #endif
#if TEMP_SENSOR_3 > 0
#define THERMISTORHEATER_3 TEMP_SENSOR_3
#define HEATER_3_USES_THERMISTOR
#endif
#if TEMP_SENSOR_BED > 0 #if TEMP_SENSOR_BED > 0
#define THERMISTORBED TEMP_SENSOR_BED #define THERMISTORBED TEMP_SENSOR_BED
#define BED_USES_THERMISTOR #define BED_USES_THERMISTOR
@ -485,6 +495,9 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#if TEMP_SENSOR_2 == -1 #if TEMP_SENSOR_2 == -1
#define HEATER_2_USES_AD595 #define HEATER_2_USES_AD595
#endif #endif
#if TEMP_SENSOR_3 == -1
#define HEATER_3_USES_AD595
#endif
#if TEMP_SENSOR_BED == -1 #if TEMP_SENSOR_BED == -1
#define BED_USES_AD595 #define BED_USES_AD595
#endif #endif
@ -503,6 +516,10 @@ const unsigned int dropsegments=5; //everything with less than this number of st
#undef HEATER_2_MINTEMP #undef HEATER_2_MINTEMP
#undef HEATER_2_MAXTEMP #undef HEATER_2_MAXTEMP
#endif #endif
#if TEMP_SENSOR_3 == 0
#undef HEATER_3_MINTEMP
#undef HEATER_3_MAXTEMP
#endif
#if TEMP_SENSOR_BED == 0 #if TEMP_SENSOR_BED == 0
#undef BED_MINTEMP #undef BED_MINTEMP
#undef BED_MAXTEMP #undef BED_MAXTEMP

View file

@ -83,6 +83,9 @@
/// check if pin is an timer wrapper /// check if pin is an timer wrapper
#define GET_TIMER(IO) _GET_TIMER(IO) #define GET_TIMER(IO) _GET_TIMER(IO)
// Shorthand
#define OUT_WRITE(IO, v) { SET_OUTPUT(IO); WRITE(IO, v); }
/* /*
ports and functions ports and functions

View file

@ -121,6 +121,7 @@
#define MSG_UNKNOWN_COMMAND "Unknown command: \"" #define MSG_UNKNOWN_COMMAND "Unknown command: \""
#define MSG_ACTIVE_EXTRUDER "Active Extruder: " #define MSG_ACTIVE_EXTRUDER "Active Extruder: "
#define MSG_INVALID_EXTRUDER "Invalid extruder" #define MSG_INVALID_EXTRUDER "Invalid extruder"
#define MSG_INVALID_SOLENOID "Invalid solenoid"
#define MSG_X_MIN "x_min: " #define MSG_X_MIN "x_min: "
#define MSG_X_MAX "x_max: " #define MSG_X_MAX "x_max: "
#define MSG_Y_MIN "y_min: " #define MSG_Y_MIN "y_min: "
@ -159,6 +160,43 @@
#define MSG_ERR_EEPROM_WRITE "Error writing to EEPROM!" #define MSG_ERR_EEPROM_WRITE "Error writing to EEPROM!"
// temperature.cpp strings
#define MSG_PID_AUTOTUNE "PID Autotune"
#define MSG_PID_AUTOTUNE_START MSG_PID_AUTOTUNE " start"
#define MSG_PID_AUTOTUNE_FAILED MSG_PID_AUTOTUNE " failed!"
#define MSG_PID_BAD_EXTRUDER_NUM MSG_PID_AUTOTUNE_FAILED " Bad extruder number"
#define MSG_PID_TEMP_TOO_HIGH MSG_PID_AUTOTUNE_FAILED " Temperature too high"
#define MSG_PID_TIMEOUT MSG_PID_AUTOTUNE_FAILED " timeout"
#define MSG_BIAS " bias: "
#define MSG_D " d: "
#define MSG_MIN " min: "
#define MSG_MAX " max: "
#define MSG_KU " Ku: "
#define MSG_TU " Tu: "
#define MSG_CLASSIC_PID " Classic PID "
#define MSG_KP " Kp: "
#define MSG_KI " Ki: "
#define MSG_KD " Kd: "
#define MSG_OK_B "ok B:"
#define MSG_OK_T "ok T:"
#define MSG_AT " @:"
#define MSG_PID_AUTOTUNE_FINISHED MSG_PID_AUTOTUNE " finished! Put the last Kp, Ki and Kd constants from above into Configuration.h"
#define MSG_PID_DEBUG " PID_DEBUG "
#define MSG_PID_DEBUG_INPUT ": Input "
#define MSG_PID_DEBUG_OUTPUT " Output "
#define MSG_PID_DEBUG_PTERM " pTerm "
#define MSG_PID_DEBUG_ITERM " iTerm "
#define MSG_PID_DEBUG_DTERM " dTerm "
#define MSG_HEATING_FAILED "Heating failed"
#define MSG_EXTRUDER_SWITCHED_OFF "Extruder switched off. Temperature difference between temp sensors is too high !"
#define MSG_INVALID_EXTRUDER_NUM " - Invalid extruder number !"
#define MSG_THERMAL_RUNAWAY_STOP "Thermal Runaway, system stopped! Heater_ID: "
#define MSG_SWITCHED_OFF_MAX " switched off. MAXTEMP triggered !!"
#define MSG_MINTEMP_EXTRUDER_OFF ": Extruder switched off. MINTEMP triggered !"
#define MSG_MAXTEMP_EXTRUDER_OFF ": Extruder" MSG_SWITCHED_OFF_MAX
#define MSG_MAXTEMP_BED_OFF "Heated bed" MSG_SWITCHED_OFF_MAX
// LCD Menu Messages // LCD Menu Messages
// Add your own character. Reference: https://github.com/MarlinFirmware/Marlin/pull/1434 photos // Add your own character. Reference: https://github.com/MarlinFirmware/Marlin/pull/1434 photos
@ -223,5 +261,6 @@
*/ */
#include LANGUAGE_INCLUDE #include LANGUAGE_INCLUDE
#include "language_en.h"
#endif //__LANGUAGE_H #endif //__LANGUAGE_H

View file

@ -8,124 +8,416 @@
#ifndef LANGUAGE_EN_H #ifndef LANGUAGE_EN_H
#define LANGUAGE_EN_H #define LANGUAGE_EN_H
#ifndef WELCOME_MSG
#define WELCOME_MSG MACHINE_NAME " ready." #define WELCOME_MSG MACHINE_NAME " ready."
#endif
#ifndef MSG_SD_INSERTED
#define MSG_SD_INSERTED "Card inserted" #define MSG_SD_INSERTED "Card inserted"
#endif
#ifndef MSG_SD_REMOVED
#define MSG_SD_REMOVED "Card removed" #define MSG_SD_REMOVED "Card removed"
#endif
#ifndef MSG_MAIN
#define MSG_MAIN "Main" #define MSG_MAIN "Main"
#endif
#ifndef MSG_AUTOSTART
#define MSG_AUTOSTART "Autostart" #define MSG_AUTOSTART "Autostart"
#endif
#ifndef MSG_DISABLE_STEPPERS
#define MSG_DISABLE_STEPPERS "Disable steppers" #define MSG_DISABLE_STEPPERS "Disable steppers"
#endif
#ifndef MSG_AUTO_HOME
#define MSG_AUTO_HOME "Auto home" #define MSG_AUTO_HOME "Auto home"
#endif
#ifndef MSG_SET_HOME_OFFSETS
#define MSG_SET_HOME_OFFSETS "Set home offsets" #define MSG_SET_HOME_OFFSETS "Set home offsets"
#endif
#ifndef MSG_SET_ORIGIN
#define MSG_SET_ORIGIN "Set origin" #define MSG_SET_ORIGIN "Set origin"
#endif
#ifndef MSG_PREHEAT_PLA
#define MSG_PREHEAT_PLA "Preheat PLA" #define MSG_PREHEAT_PLA "Preheat PLA"
#endif
#ifndef MSG_PREHEAT_PLA_N
#define MSG_PREHEAT_PLA_N MSG_PREHEAT_PLA " " #define MSG_PREHEAT_PLA_N MSG_PREHEAT_PLA " "
#endif
#ifndef MSG_PREHEAT_PLA_ALL
#define MSG_PREHEAT_PLA_ALL MSG_PREHEAT_PLA " All" #define MSG_PREHEAT_PLA_ALL MSG_PREHEAT_PLA " All"
#endif
#ifndef MSG_PREHEAT_PLA_BEDONLY
#define MSG_PREHEAT_PLA_BEDONLY MSG_PREHEAT_PLA " Bed" #define MSG_PREHEAT_PLA_BEDONLY MSG_PREHEAT_PLA " Bed"
#endif
#ifndef MSG_PREHEAT_PLA_SETTINGS
#define MSG_PREHEAT_PLA_SETTINGS MSG_PREHEAT_PLA " conf" #define MSG_PREHEAT_PLA_SETTINGS MSG_PREHEAT_PLA " conf"
#endif
#ifndef MSG_PREHEAT_ABS
#define MSG_PREHEAT_ABS "Preheat ABS" #define MSG_PREHEAT_ABS "Preheat ABS"
#endif
#ifndef MSG_PREHEAT_ABS_N
#define MSG_PREHEAT_ABS_N MSG_PREHEAT_ABS " " #define MSG_PREHEAT_ABS_N MSG_PREHEAT_ABS " "
#endif
#ifndef MSG_PREHEAT_ABS_ALL
#define MSG_PREHEAT_ABS_ALL MSG_PREHEAT_ABS " All" #define MSG_PREHEAT_ABS_ALL MSG_PREHEAT_ABS " All"
#endif
#ifndef MSG_PREHEAT_ABS_BEDONLY
#define MSG_PREHEAT_ABS_BEDONLY MSG_PREHEAT_ABS " Bed" #define MSG_PREHEAT_ABS_BEDONLY MSG_PREHEAT_ABS " Bed"
#endif
#ifndef MSG_PREHEAT_ABS_SETTINGS
#define MSG_PREHEAT_ABS_SETTINGS MSG_PREHEAT_ABS " conf" #define MSG_PREHEAT_ABS_SETTINGS MSG_PREHEAT_ABS " conf"
#endif
#ifndef MSG_H1
#define MSG_H1 "1"
#endif
#ifndef MSG_H2
#define MSG_H2 "2"
#endif
#ifndef MSG_H3
#define MSG_H3 "3"
#endif
#ifndef MSG_H4
#define MSG_H4 "4"
#endif
#ifndef MSG_COOLDOWN
#define MSG_COOLDOWN "Cooldown" #define MSG_COOLDOWN "Cooldown"
#endif
#ifndef MSG_SWITCH_PS_ON
#define MSG_SWITCH_PS_ON "Switch power on" #define MSG_SWITCH_PS_ON "Switch power on"
#endif
#ifndef MSG_SWITCH_PS_OFF
#define MSG_SWITCH_PS_OFF "Switch power off" #define MSG_SWITCH_PS_OFF "Switch power off"
#endif
#ifndef MSG_EXTRUDE
#define MSG_EXTRUDE "Extrude" #define MSG_EXTRUDE "Extrude"
#endif
#ifndef MSG_RETRACT
#define MSG_RETRACT "Retract" #define MSG_RETRACT "Retract"
#endif
#ifndef MSG_MOVE_AXIS
#define MSG_MOVE_AXIS "Move axis" #define MSG_MOVE_AXIS "Move axis"
#endif
#ifndef MSG_MOVE_X
#define MSG_MOVE_X "Move X" #define MSG_MOVE_X "Move X"
#endif
#ifndef MSG_MOVE_Y
#define MSG_MOVE_Y "Move Y" #define MSG_MOVE_Y "Move Y"
#endif
#ifndef MSG_MOVE_Z
#define MSG_MOVE_Z "Move Z" #define MSG_MOVE_Z "Move Z"
#endif
#ifndef MSG_MOVE_E
#define MSG_MOVE_E "Extruder" #define MSG_MOVE_E "Extruder"
#endif
#ifndef MSG_MOVE_01MM
#define MSG_MOVE_01MM "Move 0.1mm" #define MSG_MOVE_01MM "Move 0.1mm"
#endif
#ifndef MSG_MOVE_1MM
#define MSG_MOVE_1MM "Move 1mm" #define MSG_MOVE_1MM "Move 1mm"
#endif
#ifndef MSG_MOVE_10MM
#define MSG_MOVE_10MM "Move 10mm" #define MSG_MOVE_10MM "Move 10mm"
#endif
#ifndef MSG_SPEED
#define MSG_SPEED "Speed" #define MSG_SPEED "Speed"
#endif
#ifndef MSG_NOZZLE
#define MSG_NOZZLE "Nozzle" #define MSG_NOZZLE "Nozzle"
#endif
#ifndef MSG_N2
#define MSG_N2 " 2"
#endif
#ifndef MSG_N3
#define MSG_N3 " 3"
#endif
#ifndef MSG_N4
#define MSG_N4 " 4"
#endif
#ifndef MSG_BED
#define MSG_BED "Bed" #define MSG_BED "Bed"
#endif
#ifndef MSG_FAN_SPEED
#define MSG_FAN_SPEED "Fan speed" #define MSG_FAN_SPEED "Fan speed"
#endif
#ifndef MSG_FLOW
#define MSG_FLOW "Flow" #define MSG_FLOW "Flow"
#endif
#ifndef MSG_F0
#define MSG_F0 " 0"
#endif
#ifndef MSG_F1
#define MSG_F1 " 1"
#endif
#ifndef MSG_F2
#define MSG_F2 " 2"
#endif
#ifndef MSG_F3
#define MSG_F3 " 3"
#endif
#ifndef MSG_CONTROL
#define MSG_CONTROL "Control" #define MSG_CONTROL "Control"
#endif
#ifndef MSG_MIN
#define MSG_MIN " " STR_THERMOMETER " Min" #define MSG_MIN " " STR_THERMOMETER " Min"
#endif
#ifndef MSG_MAX
#define MSG_MAX " " STR_THERMOMETER " Max" #define MSG_MAX " " STR_THERMOMETER " Max"
#endif
#ifndef MSG_FACTOR
#define MSG_FACTOR " " STR_THERMOMETER " Fact" #define MSG_FACTOR " " STR_THERMOMETER " Fact"
#endif
#ifndef MSG_AUTOTEMP
#define MSG_AUTOTEMP "Autotemp" #define MSG_AUTOTEMP "Autotemp"
#endif
#ifndef MSG_ON
#define MSG_ON "On " #define MSG_ON "On "
#endif
#ifndef MSG_OFF
#define MSG_OFF "Off" #define MSG_OFF "Off"
#endif
#ifndef MSG_PID_P
#define MSG_PID_P "PID-P" #define MSG_PID_P "PID-P"
#endif
#ifndef MSG_PID_I
#define MSG_PID_I "PID-I" #define MSG_PID_I "PID-I"
#endif
#ifndef MSG_PID_D
#define MSG_PID_D "PID-D" #define MSG_PID_D "PID-D"
#endif
#ifndef MSG_PID_C
#define MSG_PID_C "PID-C" #define MSG_PID_C "PID-C"
#endif
#ifndef MSG_E2
#define MSG_E2 " E2"
#endif
#ifndef MSG_E3
#define MSG_E3 " E3"
#endif
#ifndef MSG_E4
#define MSG_E4 " E4"
#endif
#ifndef MSG_ACC
#define MSG_ACC "Accel" #define MSG_ACC "Accel"
#endif
#ifndef MSG_VXY_JERK
#define MSG_VXY_JERK "Vxy-jerk" #define MSG_VXY_JERK "Vxy-jerk"
#endif
#ifndef MSG_VZ_JERK
#define MSG_VZ_JERK "Vz-jerk" #define MSG_VZ_JERK "Vz-jerk"
#endif
#ifndef MSG_VE_JERK
#define MSG_VE_JERK "Ve-jerk" #define MSG_VE_JERK "Ve-jerk"
#endif
#ifndef MSG_VMAX
#define MSG_VMAX "Vmax " #define MSG_VMAX "Vmax "
#endif
#ifndef MSG_X
#define MSG_X "x" #define MSG_X "x"
#endif
#ifndef MSG_Y
#define MSG_Y "y" #define MSG_Y "y"
#endif
#ifndef MSG_Z
#define MSG_Z "z" #define MSG_Z "z"
#endif
#ifndef MSG_E
#define MSG_E "e" #define MSG_E "e"
#endif
#ifndef MSG_VMIN
#define MSG_VMIN "Vmin" #define MSG_VMIN "Vmin"
#endif
#ifndef MSG_VTRAV_MIN
#define MSG_VTRAV_MIN "VTrav min" #define MSG_VTRAV_MIN "VTrav min"
#endif
#ifndef MSG_AMAX
#define MSG_AMAX "Amax " #define MSG_AMAX "Amax "
#endif
#ifndef MSG_A_RETRACT
#define MSG_A_RETRACT "A-retract" #define MSG_A_RETRACT "A-retract"
#endif
#ifndef MSG_XSTEPS
#define MSG_XSTEPS "Xsteps/mm" #define MSG_XSTEPS "Xsteps/mm"
#endif
#ifndef MSG_YSTEPS
#define MSG_YSTEPS "Ysteps/mm" #define MSG_YSTEPS "Ysteps/mm"
#endif
#ifndef MSG_ZSTEPS
#define MSG_ZSTEPS "Zsteps/mm" #define MSG_ZSTEPS "Zsteps/mm"
#endif
#ifndef MSG_ESTEPS
#define MSG_ESTEPS "Esteps/mm" #define MSG_ESTEPS "Esteps/mm"
#endif
#ifndef MSG_TEMPERATURE
#define MSG_TEMPERATURE "Temperature" #define MSG_TEMPERATURE "Temperature"
#endif
#ifndef MSG_MOTION
#define MSG_MOTION "Motion" #define MSG_MOTION "Motion"
#endif
#ifndef MSG_VOLUMETRIC
#define MSG_VOLUMETRIC "Filament" #define MSG_VOLUMETRIC "Filament"
#define MSG_VOLUMETRIC_ENABLED "E in mm" STR_h3 #endif
#ifndef MSG_VOLUMETRIC_ENABLED
#define MSG_VOLUMETRIC_ENABLED "E in mm" STR_h3
#endif
#ifndef MSG_FILAMENT_SIZE_EXTRUDER_0
#define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1" #define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1"
#endif
#ifndef MSG_FILAMENT_SIZE_EXTRUDER_1
#define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2" #define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2"
#endif
#ifndef MSG_FILAMENT_SIZE_EXTRUDER_2
#define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3" #define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3"
#endif
#ifndef MSG_FILAMENT_SIZE_EXTRUDER_3
#define MSG_FILAMENT_SIZE_EXTRUDER_3 "Fil. Dia. 4" #define MSG_FILAMENT_SIZE_EXTRUDER_3 "Fil. Dia. 4"
#endif
#ifndef MSG_CONTRAST
#define MSG_CONTRAST "LCD contrast" #define MSG_CONTRAST "LCD contrast"
#endif
#ifndef MSG_STORE_EPROM
#define MSG_STORE_EPROM "Store memory" #define MSG_STORE_EPROM "Store memory"
#endif
#ifndef MSG_LOAD_EPROM
#define MSG_LOAD_EPROM "Load memory" #define MSG_LOAD_EPROM "Load memory"
#endif
#ifndef MSG_RESTORE_FAILSAFE
#define MSG_RESTORE_FAILSAFE "Restore failsafe" #define MSG_RESTORE_FAILSAFE "Restore failsafe"
#endif
#ifndef MSG_REFRESH
#define MSG_REFRESH "Refresh" #define MSG_REFRESH "Refresh"
#endif
#ifndef MSG_WATCH
#define MSG_WATCH "Info screen" #define MSG_WATCH "Info screen"
#endif
#ifndef MSG_PREPARE
#define MSG_PREPARE "Prepare" #define MSG_PREPARE "Prepare"
#endif
#ifndef MSG_TUNE
#define MSG_TUNE "Tune" #define MSG_TUNE "Tune"
#endif
#ifndef MSG_PAUSE_PRINT
#define MSG_PAUSE_PRINT "Pause print" #define MSG_PAUSE_PRINT "Pause print"
#endif
#ifndef MSG_RESUME_PRINT
#define MSG_RESUME_PRINT "Resume print" #define MSG_RESUME_PRINT "Resume print"
#endif
#ifndef MSG_STOP_PRINT
#define MSG_STOP_PRINT "Stop print" #define MSG_STOP_PRINT "Stop print"
#endif
#ifndef MSG_CARD_MENU
#define MSG_CARD_MENU "Print from SD" #define MSG_CARD_MENU "Print from SD"
#endif
#ifndef MSG_NO_CARD
#define MSG_NO_CARD "No SD card" #define MSG_NO_CARD "No SD card"
#endif
#ifndef MSG_DWELL
#define MSG_DWELL "Sleep..." #define MSG_DWELL "Sleep..."
#endif
#ifndef MSG_USERWAIT
#define MSG_USERWAIT "Wait for user..." #define MSG_USERWAIT "Wait for user..."
#endif
#ifndef MSG_RESUMING
#define MSG_RESUMING "Resuming print" #define MSG_RESUMING "Resuming print"
#endif
#ifndef MSG_PRINT_ABORTED
#define MSG_PRINT_ABORTED "Print aborted" #define MSG_PRINT_ABORTED "Print aborted"
#endif
#ifndef MSG_NO_MOVE
#define MSG_NO_MOVE "No move." #define MSG_NO_MOVE "No move."
#endif
#ifndef MSG_KILLED
#define MSG_KILLED "KILLED. " #define MSG_KILLED "KILLED. "
#endif
#ifndef MSG_STOPPED
#define MSG_STOPPED "STOPPED. " #define MSG_STOPPED "STOPPED. "
#endif
#ifndef MSG_CONTROL_RETRACT
#define MSG_CONTROL_RETRACT "Retract mm" #define MSG_CONTROL_RETRACT "Retract mm"
#endif
#ifndef MSG_CONTROL_RETRACT_SWAP
#define MSG_CONTROL_RETRACT_SWAP "Swap Re.mm" #define MSG_CONTROL_RETRACT_SWAP "Swap Re.mm"
#endif
#ifndef MSG_CONTROL_RETRACTF
#define MSG_CONTROL_RETRACTF "Retract V" #define MSG_CONTROL_RETRACTF "Retract V"
#endif
#ifndef MSG_CONTROL_RETRACT_ZLIFT
#define MSG_CONTROL_RETRACT_ZLIFT "Hop mm" #define MSG_CONTROL_RETRACT_ZLIFT "Hop mm"
#endif
#ifndef MSG_CONTROL_RETRACT_RECOVER
#define MSG_CONTROL_RETRACT_RECOVER "UnRet +mm" #define MSG_CONTROL_RETRACT_RECOVER "UnRet +mm"
#endif
#ifndef MSG_CONTROL_RETRACT_RECOVER_SWAP
#define MSG_CONTROL_RETRACT_RECOVER_SWAP "S UnRet+mm" #define MSG_CONTROL_RETRACT_RECOVER_SWAP "S UnRet+mm"
#endif
#ifndef MSG_CONTROL_RETRACT_RECOVERF
#define MSG_CONTROL_RETRACT_RECOVERF "UnRet V" #define MSG_CONTROL_RETRACT_RECOVERF "UnRet V"
#endif
#ifndef MSG_AUTORETRACT
#define MSG_AUTORETRACT "AutoRetr." #define MSG_AUTORETRACT "AutoRetr."
#endif
#ifndef MSG_FILAMENTCHANGE
#define MSG_FILAMENTCHANGE "Change filament" #define MSG_FILAMENTCHANGE "Change filament"
#endif
#ifndef MSG_INIT_SDCARD
#define MSG_INIT_SDCARD "Init. SD card" #define MSG_INIT_SDCARD "Init. SD card"
#endif
#ifndef MSG_CNG_SDCARD
#define MSG_CNG_SDCARD "Change SD card" #define MSG_CNG_SDCARD "Change SD card"
#endif
#ifndef MSG_ZPROBE_OUT
#define MSG_ZPROBE_OUT "Z probe out. bed" #define MSG_ZPROBE_OUT "Z probe out. bed"
#endif
#ifndef MSG_POSITION_UNKNOWN
#define MSG_POSITION_UNKNOWN "Home X/Y before Z" #define MSG_POSITION_UNKNOWN "Home X/Y before Z"
#endif
#ifndef MSG_ZPROBE_ZOFFSET
#define MSG_ZPROBE_ZOFFSET "Z Offset" #define MSG_ZPROBE_ZOFFSET "Z Offset"
#endif
#ifndef MSG_BABYSTEP_X
#define MSG_BABYSTEP_X "Babystep X" #define MSG_BABYSTEP_X "Babystep X"
#endif
#ifndef MSG_BABYSTEP_Y
#define MSG_BABYSTEP_Y "Babystep Y" #define MSG_BABYSTEP_Y "Babystep Y"
#endif
#ifndef MSG_BABYSTEP_Z
#define MSG_BABYSTEP_Z "Babystep Z" #define MSG_BABYSTEP_Z "Babystep Z"
#endif
#ifndef MSG_ENDSTOP_ABORT
#define MSG_ENDSTOP_ABORT "Endstop abort" #define MSG_ENDSTOP_ABORT "Endstop abort"
#endif
#ifndef MSG_HEATING_FAILED_LCD
#define MSG_HEATING_FAILED_LCD "Heating failed"
#endif
#ifndef MSG_ERR_REDUNDANT_TEMP
#define MSG_ERR_REDUNDANT_TEMP "Err: REDUNDANT TEMP ERROR"
#endif
#ifndef MSG_THERMAL_RUNAWAY
#define MSG_THERMAL_RUNAWAY "THERMAL RUNAWAY"
#endif
#ifndef MSG_ERR_MAXTEMP
#define MSG_ERR_MAXTEMP "Err: MAXTEMP"
#endif
#ifndef MSG_ERR_MINTEMP
#define MSG_ERR_MINTEMP "Err: MINTEMP"
#endif
#ifndef MSG_ERR_MAXTEMP_BED
#define MSG_ERR_MAXTEMP_BED "Err: MAXTEMP BED"
#endif
#ifdef DELTA_CALIBRATION_MENU #ifdef DELTA_CALIBRATION_MENU
#define MSG_DELTA_CALIBRATE "Delta Calibration" #ifndef MSG_DELTA_CALIBRATE
#define MSG_DELTA_CALIBRATE_X "Calibrate X" #define MSG_DELTA_CALIBRATE "Delta Calibration"
#define MSG_DELTA_CALIBRATE_Y "Calibrate Y" #endif
#define MSG_DELTA_CALIBRATE_Z "Calibrate Z" #ifndef MSG_DELTA_CALIBRATE_X
#define MSG_DELTA_CALIBRATE_CENTER "Calibrate Center" #define MSG_DELTA_CALIBRATE_X "Calibrate X"
#endif
#ifndef MSG_DELTA_CALIBRATE_Y
#define MSG_DELTA_CALIBRATE_Y "Calibrate Y"
#endif
#ifndef MSG_DELTA_CALIBRATE_Z
#define MSG_DELTA_CALIBRATE_Z "Calibrate Z"
#endif
#ifndef MSG_DELTA_CALIBRATE_CENTER
#define MSG_DELTA_CALIBRATE_CENTER "Calibrate Center"
#endif
#endif // DELTA_CALIBRATION_MENU #endif // DELTA_CALIBRATION_MENU
#endif // LANGUAGE_EN_H #endif // LANGUAGE_EN_H

View file

@ -17,38 +17,38 @@
#define MSG_AUTO_HOME "Aja referenssiin" #define MSG_AUTO_HOME "Aja referenssiin"
#define MSG_SET_HOME_OFFSETS "Set home offsets" #define MSG_SET_HOME_OFFSETS "Set home offsets"
#define MSG_SET_ORIGIN "Aseta origo" #define MSG_SET_ORIGIN "Aseta origo"
#define MSG_PREHEAT_PLA "Esilammita PLA" #define MSG_PREHEAT_PLA "Esil" STR_ae "mmit" STR_ae " PLA"
#define MSG_PREHEAT_PLA_N "Esilammita PLA " #define MSG_PREHEAT_PLA_N "Esil" STR_ae "mmit" STR_ae " PLA "
#define MSG_PREHEAT_PLA_ALL "Esila. PLA Kaikki" #define MSG_PREHEAT_PLA_ALL "Esil" STR_ae ". PLA Kaikki"
#define MSG_PREHEAT_PLA_BEDONLY "Esila. PLA Alusta" #define MSG_PREHEAT_PLA_BEDONLY "Esil" STR_ae ". PLA Alusta"
#define MSG_PREHEAT_PLA_SETTINGS "Esilamm. PLA konf" #define MSG_PREHEAT_PLA_SETTINGS "Esil" STR_ae "mm. PLA konf"
#define MSG_PREHEAT_ABS "Esilammita ABS" #define MSG_PREHEAT_ABS "Esil" STR_ae "mmit" STR_ae " ABS"
#define MSG_PREHEAT_ABS_N "Esilammita ABS " #define MSG_PREHEAT_ABS_N "Esil" STR_ae "mmit" STR_ae " ABS "
#define MSG_PREHEAT_ABS_ALL "Esila. ABS Kaikki" #define MSG_PREHEAT_ABS_ALL "Esil" STR_ae ". ABS Kaikki"
#define MSG_PREHEAT_ABS_BEDONLY "Esila. ABS Alusta" #define MSG_PREHEAT_ABS_BEDONLY "Esil" STR_ae ". ABS Alusta"
#define MSG_PREHEAT_ABS_SETTINGS "Esilamm. ABS konf" #define MSG_PREHEAT_ABS_SETTINGS "Esil" STR_ae "mm. ABS konf"
#define MSG_COOLDOWN "Jaahdyta" #define MSG_COOLDOWN "J" STR_ae "" STR_ae "hdyt" STR_ae ""
#define MSG_SWITCH_PS_ON "Virta paalle" #define MSG_SWITCH_PS_ON "Virta p" STR_ae "" STR_ae "lle"
#define MSG_SWITCH_PS_OFF "Virta pois" #define MSG_SWITCH_PS_OFF "Virta pois"
#define MSG_EXTRUDE "Pursota" #define MSG_EXTRUDE "Pursota"
#define MSG_RETRACT "Veda takaisin" #define MSG_RETRACT "Ved" STR_ae " takaisin"
#define MSG_MOVE_AXIS "Liikuta akseleita" #define MSG_MOVE_AXIS "Liikuta akseleita"
#define MSG_MOVE_X "Move X" #define MSG_MOVE_X "Liikuta X"
#define MSG_MOVE_Y "Move Y" #define MSG_MOVE_Y "Liikuta Y"
#define MSG_MOVE_Z "Move Z" #define MSG_MOVE_Z "Liikuta Z"
#define MSG_MOVE_E "Extruder" #define MSG_MOVE_E "Extruder"
#define MSG_MOVE_01MM "Move 0.1mm" #define MSG_MOVE_01MM "Liikuta 0.1mm"
#define MSG_MOVE_1MM "Move 1mm" #define MSG_MOVE_1MM "Liikuta 1mm"
#define MSG_MOVE_10MM "Move 10mm" #define MSG_MOVE_10MM "Liikuta 10mm"
#define MSG_SPEED "Nopeus" #define MSG_SPEED "Nopeus"
#define MSG_NOZZLE "Suutin" #define MSG_NOZZLE "Suutin"
#define MSG_BED "Alusta" #define MSG_BED "Alusta"
#define MSG_FAN_SPEED "Tuul. nopeus" #define MSG_FAN_SPEED "Tuul. nopeus"
#define MSG_FLOW "Virtaus" #define MSG_FLOW "Virtaus"
#define MSG_CONTROL "Kontrolli" #define MSG_CONTROL "Kontrolli"
#define MSG_MIN " \002 Min" #define MSG_MIN STR_THERMOMETER " Min"
#define MSG_MAX " \002 Max" #define MSG_MAX STR_THERMOMETER " Max"
#define MSG_FACTOR " \002 Kerr" #define MSG_FACTOR STR_THERMOMETER " Kerr"
#define MSG_AUTOTEMP "Autotemp" #define MSG_AUTOTEMP "Autotemp"
#define MSG_ON "On " #define MSG_ON "On "
#define MSG_OFF "Off" #define MSG_OFF "Off"
@ -73,24 +73,24 @@
#define MSG_YSTEPS "Ysteps/mm" #define MSG_YSTEPS "Ysteps/mm"
#define MSG_ZSTEPS "Zsteps/mm" #define MSG_ZSTEPS "Zsteps/mm"
#define MSG_ESTEPS "Esteps/mm" #define MSG_ESTEPS "Esteps/mm"
#define MSG_TEMPERATURE "Lampotila" #define MSG_TEMPERATURE "L" STR_ae "mp" STR_oe "tila"
#define MSG_MOTION "Liike" #define MSG_MOTION "Liike"
#define MSG_VOLUMETRIC "Filament" #define MSG_VOLUMETRIC "Filament"
#define MSG_VOLUMETRIC_ENABLED "E in mm3" #define MSG_VOLUMETRIC_ENABLED "E in mm3"
#define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1" #define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1"
#define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2" #define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2"
#define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3" #define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3"
#define MSG_CONTRAST "LCD contrast" #define MSG_CONTRAST "LCD kontrasti"
#define MSG_STORE_EPROM "Tallenna muistiin" #define MSG_STORE_EPROM "Tallenna muistiin"
#define MSG_LOAD_EPROM "Lataa muistista" #define MSG_LOAD_EPROM "Lataa muistista"
#define MSG_RESTORE_FAILSAFE "Palauta oletus" #define MSG_RESTORE_FAILSAFE "Palauta oletus"
#define MSG_REFRESH "Paivita" #define MSG_REFRESH "P" STR_ae "ivit" STR_ae ""
#define MSG_WATCH "Seuraa" #define MSG_WATCH "Seuraa"
#define MSG_PREPARE "Valmistele" #define MSG_PREPARE "Valmistele"
#define MSG_TUNE "Saada" #define MSG_TUNE "S" STR_ae "" STR_ae "d" STR_ae ""
#define MSG_PAUSE_PRINT "Keskeyta tulostus" #define MSG_PAUSE_PRINT "Keskeyt" STR_ae " tulostus"
#define MSG_RESUME_PRINT "Jatka tulostusta" #define MSG_RESUME_PRINT "Jatka tulostusta"
#define MSG_STOP_PRINT "Pysayta tulostus" #define MSG_STOP_PRINT "Pys" STR_ae "yt" STR_ae " tulostus"
#define MSG_CARD_MENU "Korttivalikko" #define MSG_CARD_MENU "Korttivalikko"
#define MSG_NO_CARD "Ei korttia" #define MSG_NO_CARD "Ei korttia"
#define MSG_DWELL "Nukkumassa..." #define MSG_DWELL "Nukkumassa..."
@ -100,9 +100,9 @@
#define MSG_NO_MOVE "Ei liiketta." #define MSG_NO_MOVE "Ei liiketta."
#define MSG_KILLED "KILLED. " #define MSG_KILLED "KILLED. "
#define MSG_STOPPED "STOPPED. " #define MSG_STOPPED "STOPPED. "
#define MSG_CONTROL_RETRACT "Veda mm" #define MSG_CONTROL_RETRACT "Ved" STR_ae " mm"
#define MSG_CONTROL_RETRACT_SWAP "Va. Veda mm" #define MSG_CONTROL_RETRACT_SWAP "Va. Ved" STR_ae " mm"
#define MSG_CONTROL_RETRACTF "Veda V" #define MSG_CONTROL_RETRACTF "Ved" STR_ae " V"
#define MSG_CONTROL_RETRACT_ZLIFT "Z mm" #define MSG_CONTROL_RETRACT_ZLIFT "Z mm"
#define MSG_CONTROL_RETRACT_RECOVER "UnRet +mm" #define MSG_CONTROL_RETRACT_RECOVER "UnRet +mm"
#define MSG_CONTROL_RETRACT_RECOVER_SWAP "Va. UnRet +mm" #define MSG_CONTROL_RETRACT_RECOVER_SWAP "Va. UnRet +mm"
@ -120,11 +120,11 @@
#define MSG_ENDSTOP_ABORT "Endstop abort" #define MSG_ENDSTOP_ABORT "Endstop abort"
#ifdef DELTA_CALIBRATION_MENU #ifdef DELTA_CALIBRATION_MENU
#define MSG_DELTA_CALIBRATE "Delta Calibration" #define MSG_DELTA_CALIBRATE "Delta Kalibrointi"
#define MSG_DELTA_CALIBRATE_X "Calibrate X" #define MSG_DELTA_CALIBRATE_X "Kalibroi X"
#define MSG_DELTA_CALIBRATE_Y "Calibrate Y" #define MSG_DELTA_CALIBRATE_Y "Kalibroi Y"
#define MSG_DELTA_CALIBRATE_Z "Calibrate Z" #define MSG_DELTA_CALIBRATE_Z "Kalibroi Z"
#define MSG_DELTA_CALIBRATE_CENTER "Calibrate Center" #define MSG_DELTA_CALIBRATE_CENTER "Kalibroi Center"
#endif // DELTA_CALIBRATION_MENU #endif // DELTA_CALIBRATION_MENU
#endif // LANGUAGE_FI_H #endif // LANGUAGE_FI_H

View file

@ -13,7 +13,7 @@
#define WELCOME_MSG MACHINE_NAME " Готов." #define WELCOME_MSG MACHINE_NAME " Готов."
#define MSG_SD_INSERTED "Карта вставлена" #define MSG_SD_INSERTED "Карта вставлена"
#define MSG_SD_REMOVED "Карта извлечена" #define MSG_SD_REMOVED "Карта извлечена"
#define MSG_MAIN "Меню \003" #define MSG_MAIN "Меню"
#define MSG_AUTOSTART "Автостарт" #define MSG_AUTOSTART "Автостарт"
#define MSG_DISABLE_STEPPERS "Выкл. двигатели" #define MSG_DISABLE_STEPPERS "Выкл. двигатели"
#define MSG_AUTO_HOME "Парковка" #define MSG_AUTO_HOME "Парковка"
@ -43,14 +43,14 @@
#define MSG_MOVE_1MM "Move 1mm" #define MSG_MOVE_1MM "Move 1mm"
#define MSG_MOVE_10MM "Move 10mm" #define MSG_MOVE_10MM "Move 10mm"
#define MSG_SPEED "Скорость" #define MSG_SPEED "Скорость"
#define MSG_NOZZLE "\002 Фильера" #define MSG_NOZZLE LCD_STR_THERMOMETER " Фильера"
#define MSG_BED "\002 Кровать" #define MSG_BED LCD_STR_THERMOMETER " Кровать"
#define MSG_FAN_SPEED "Куллер" #define MSG_FAN_SPEED "Куллер"
#define MSG_FLOW "Поток" #define MSG_FLOW "Поток"
#define MSG_CONTROL "Настройки \003" #define MSG_CONTROL "Настройки"
#define MSG_MIN "\002 Минимум" #define MSG_MIN LCD_STR_THERMOMETER " Минимум"
#define MSG_MAX "\002 Максимум" #define MSG_MAX LCD_STR_THERMOMETER " Максимум"
#define MSG_FACTOR "\002 Фактор" #define MSG_FACTOR LCD_STR_THERMOMETER " Фактор"
#define MSG_AUTOTEMP "Autotemp" #define MSG_AUTOTEMP "Autotemp"
#define MSG_ON "Вкл. " #define MSG_ON "Вкл. "
#define MSG_OFF "Выкл. " #define MSG_OFF "Выкл. "
@ -75,10 +75,10 @@
#define MSG_YSTEPS "Y шаг/mm" #define MSG_YSTEPS "Y шаг/mm"
#define MSG_ZSTEPS "Z шаг/mm" #define MSG_ZSTEPS "Z шаг/mm"
#define MSG_ESTEPS "E шаг/mm" #define MSG_ESTEPS "E шаг/mm"
#define MSG_TEMPERATURE "Температура \x7E" #define MSG_TEMPERATURE "Температура"
#define MSG_MOTION "Скорости \x7E" #define MSG_MOTION "Скорости"
#define MSG_VOLUMETRIC "Filament" #define MSG_VOLUMETRIC "Filament"
#define MSG_VOLUMETRIC_ENABLED "E in mm3" #define MSG_VOLUMETRIC_ENABLED "E in mm3"
#define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1" #define MSG_FILAMENT_SIZE_EXTRUDER_0 "Fil. Dia. 1"
#define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2" #define MSG_FILAMENT_SIZE_EXTRUDER_1 "Fil. Dia. 2"
#define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3" #define MSG_FILAMENT_SIZE_EXTRUDER_2 "Fil. Dia. 3"
@ -86,14 +86,14 @@
#define MSG_STORE_EPROM "Сохранить в EPROM" #define MSG_STORE_EPROM "Сохранить в EPROM"
#define MSG_LOAD_EPROM "Загруз. из EPROM" #define MSG_LOAD_EPROM "Загруз. из EPROM"
#define MSG_RESTORE_FAILSAFE "Сброс настроек" #define MSG_RESTORE_FAILSAFE "Сброс настроек"
#define MSG_REFRESH "\004Обновить" #define MSG_REFRESH LCD_STR_REFRESH "Обновить"
#define MSG_WATCH "Обзор \003" #define MSG_WATCH "Обзор"
#define MSG_PREPARE "Действия \x7E" #define MSG_PREPARE "Действия"
#define MSG_TUNE "Настройки \x7E" #define MSG_TUNE "Настройки"
#define MSG_PAUSE_PRINT "Продолжить печать" #define MSG_PAUSE_PRINT "Продолжить печать"
#define MSG_RESUME_PRINT "возобн. печать" #define MSG_RESUME_PRINT "возобн. печать"
#define MSG_STOP_PRINT "Остановить печать" #define MSG_STOP_PRINT "Остановить печать"
#define MSG_CARD_MENU "Меню карты \x7E" #define MSG_CARD_MENU "Меню карты"
#define MSG_NO_CARD "Нет карты" #define MSG_NO_CARD "Нет карты"
#define MSG_DWELL "Сон..." #define MSG_DWELL "Сон..."
#define MSG_USERWAIT "Ожиданиие" #define MSG_USERWAIT "Ожиданиие"

View file

@ -36,7 +36,7 @@
#include "pins_SETHI.h" #include "pins_SETHI.h"
#elif MB(RAMPS_OLD) #elif MB(RAMPS_OLD)
#include "pins_RAMPS_OLD.h" #include "pins_RAMPS_OLD.h"
#elif IS_RAMPS #elif MB(RAMPS_13_EFB) || MB(RAMPS_13_EEB) || MB(RAMPS_13_EFF) || MB(RAMPS_13_EEF)
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#elif MB(DUEMILANOVE_328P) #elif MB(DUEMILANOVE_328P)
#include "pins_DUEMILANOVE_328P.h" #include "pins_DUEMILANOVE_328P.h"
@ -110,6 +110,10 @@
#include "pins_WITBOX.h" #include "pins_WITBOX.h"
#elif MB(HEPHESTOS) #elif MB(HEPHESTOS)
#include "pins_HEPHESTOS.h" #include "pins_HEPHESTOS.h"
#elif MB(BAM_DICE)
#include "pins_RAMPS_13.h"
#elif MB(BAM_DICE_DUE)
#include "pins_BAM_DICE_DUE.h"
#elif MB(99) #elif MB(99)
#include "pins_99.h" #include "pins_99.h"
#else #else

View file

@ -4,6 +4,8 @@
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#define Z_ENABLE_PIN 63
#define X_MAX_PIN 2 #define X_MAX_PIN 2
#define Y_MAX_PIN 15 #define Y_MAX_PIN 15
#define Z_MAX_PIN -1 #define Z_MAX_PIN -1

View file

@ -3,3 +3,11 @@
*/ */
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#define FAN_PIN 9 // (Sprinter config)
#define HEATER_1_PIN -1
#ifdef TEMP_STAT_LEDS
#define STAT_LED_RED 6
#define STAT_LED_BLUE 11
#endif

View file

@ -4,6 +4,9 @@
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#define FAN_PIN 9 // (Sprinter config)
#define BEEPER 33
#define E2_STEP_PIN 23 #define E2_STEP_PIN 23
#define E2_DIR_PIN 25 #define E2_DIR_PIN 25
#define E2_ENABLE_PIN 40 #define E2_ENABLE_PIN 40
@ -16,15 +19,16 @@
#define E4_DIR_PIN 37 #define E4_DIR_PIN 37
#define E4_ENABLE_PIN 42 #define E4_ENABLE_PIN 42
#define HEATER_1_PIN -1
#define HEATER_2_PIN 16 #define HEATER_2_PIN 16
#define HEATER_3_PIN 17 #define HEATER_3_PIN 17
#define HEATER_4_PIN 4 #define HEATER_4_PIN 4
#define HEATER_5_PIN 5 #define HEATER_5_PIN 5
#define HEATER_6_PIN 6 #define HEATER_6_PIN 6
#define HEATER_7_PIN 11 #define HEATER_7_PIN 11
#define TEMP_2_PIN 12 // ANALOG NUMBERING #define TEMP_2_PIN 12 // ANALOG NUMBERING
#define TEMP_3_PIN 11 // ANALOG NUMBERING #define TEMP_3_PIN 11 // ANALOG NUMBERING
#define TEMP_4_PIN 10 // ANALOG NUMBERING #define TEMP_4_PIN 10 // ANALOG NUMBERING
#define TC1 4 // ANALOG NUMBERING Thermo couple on Azteeg X3Pro #define TC1 4 // ANALOG NUMBERING Thermo couple on Azteeg X3Pro
#define TC2 5 // ANALOG NUMBERING Thermo couple on Azteeg X3Pro #define TC2 5 // ANALOG NUMBERING Thermo couple on Azteeg X3Pro

View file

@ -0,0 +1,11 @@
/**
* BAM&DICE Due (Arduino Mega) pin assignments
*/
#include "pins_RAMPS_13.h"
#define FAN_PIN 9 // (Sprinter config)
#define HEATER_1_PIN -1
#define TEMP_0_PIN 9 // ANALOG NUMBERING
#define TEMP_1_PIN 11 // ANALOG NUMBERING

View file

@ -87,9 +87,3 @@
// Cheaptronic v1.0 does not use this port // Cheaptronic v1.0 does not use this port
#define SDCARDDETECT -1 #define SDCARDDETECT -1
// Encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1

View file

@ -74,12 +74,6 @@
#define BLEN_B 1 #define BLEN_B 1
#define BLEN_A 0 #define BLEN_A 0
//encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
#endif // RA_CONTROL_PANEL #endif // RA_CONTROL_PANEL
#ifdef RA_DISCO #ifdef RA_DISCO

View file

@ -3,3 +3,6 @@
*/ */
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#define FAN_PIN 9 // (Sprinter config)
#define HEATER_1_PIN -1

View file

@ -83,10 +83,4 @@
#define SDCARDDETECT -1 // Ramps does not use this port #define SDCARDDETECT -1 // Ramps does not use this port
//encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
#endif // ULTRA_LCD && NEWPANEL #endif // ULTRA_LCD && NEWPANEL

View file

@ -80,9 +80,3 @@
#define BLEN_A 0 #define BLEN_A 0
#define SDCARDDETECT -1 // Megatronics does not use this port #define SDCARDDETECT -1 // Megatronics does not use this port
// Encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1

View file

@ -95,9 +95,3 @@
#define BLEN_A 0 #define BLEN_A 0
#define SDCARDDETECT -1 // Megatronics does not use this port #define SDCARDDETECT -1 // Megatronics does not use this port
// Encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1

View file

@ -8,6 +8,20 @@
#define LARGE_FLASH true #define LARGE_FLASH true
// Servo support
#ifdef NUM_SERVOS
#define SERVO0_PIN 46 //AUX3-6
#if NUM_SERVOS > 1
#define SERVO1_PIN 47 //AUX3-5
#if NUM_SERVOS > 2
#define SERVO2_PIN 48 //AUX3-4
#if NUM_SERVOS > 3
#define SERVO2_PIN 49 //AUX3-3
#endif
#endif
#endif
#endif
#define X_STEP_PIN 58 #define X_STEP_PIN 58
#define X_DIR_PIN 57 #define X_DIR_PIN 57
#define X_ENABLE_PIN 59 #define X_ENABLE_PIN 59
@ -81,9 +95,3 @@
#define BLEN_A 0 #define BLEN_A 0
#define SDCARDDETECT -1 // Megatronics does not use this port #define SDCARDDETECT -1 // Megatronics does not use this port
// Encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1

View file

@ -116,11 +116,6 @@
#define SDCARDDETECT 81 // Ramps does not use this port #define SDCARDDETECT 81 // Ramps does not use this port
//encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
#else //!NEWPANEL - old style panel with shift register #else //!NEWPANEL - old style panel with shift register
//arduino pin witch triggers an piezzo beeper //arduino pin witch triggers an piezzo beeper
#define BEEPER 33 No Beeper added #define BEEPER 33 No Beeper added
@ -138,12 +133,6 @@
#define LCD_PINS_D6 27 #define LCD_PINS_D6 27
#define LCD_PINS_D7 29 #define LCD_PINS_D7 29
//encoder rotation values
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
//bits in the shift register that carry the buttons for: //bits in the shift register that carry the buttons for:
// left up center down right red // left up center down right red
#define BL_LE 7 #define BL_LE 7

View file

@ -7,10 +7,8 @@
* RAMPS_13_EEB (Extruder, Extruder, Bed) * RAMPS_13_EEB (Extruder, Extruder, Bed)
* RAMPS_13_EFF (Extruder, Fan, Fan) * RAMPS_13_EFF (Extruder, Fan, Fan)
* RAMPS_13_EEF (Extruder, Extruder, Fan) * RAMPS_13_EEF (Extruder, Extruder, Fan)
* 3DRAG *
* K8200 * Other pins_MYBOARD.h files may override these defaults
* AZTEEG_X3
* AZTEEG_X3_PRO
*/ */
#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) #if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__)
@ -63,7 +61,7 @@
#define FILWIDTH_PIN 5 #define FILWIDTH_PIN 5
#endif #endif
#if MB(RAMPS_13_EFB) || MB(RAMPS_13_EFF) || MB(AZTEEG_X3) || MB(AZTEEG_X3_PRO) || MB(WITBOX) || MB(HEPHESTOS) #if MB(RAMPS_13_EFB) || MB(RAMPS_13_EFF)
#define FAN_PIN 9 // (Sprinter config) #define FAN_PIN 9 // (Sprinter config)
#if MB(RAMPS_13_EFF) #if MB(RAMPS_13_EFF)
#define CONTROLLERFAN_PIN -1 // Pin used for the fan to cool controller #define CONTROLLERFAN_PIN -1 // Pin used for the fan to cool controller
@ -88,7 +86,7 @@
#define HEATER_0_PIN 10 // EXTRUDER 1 #define HEATER_0_PIN 10 // EXTRUDER 1
#endif #endif
#if MB(RAMPS_13_EFB) || MB(AZTEEG_X3) || MB(WITBOX) || MB(HEPHESTOS) #if MB(RAMPS_13_EFB)
#define HEATER_1_PIN -1 #define HEATER_1_PIN -1
#else #else
#define HEATER_1_PIN 9 // EXTRUDER 2 (FAN On Sprinter) #define HEATER_1_PIN 9 // EXTRUDER 2 (FAN On Sprinter)
@ -110,40 +108,36 @@
#ifdef NUM_SERVOS #ifdef NUM_SERVOS
#define SERVO0_PIN 11 #define SERVO0_PIN 11
#if NUM_SERVOS > 1 #if NUM_SERVOS > 1
#define SERVO1_PIN 6 #define SERVO1_PIN 6
#endif #if NUM_SERVOS > 2
#define SERVO2_PIN 5
#if NUM_SERVOS > 2 #if NUM_SERVOS > 3
#define SERVO2_PIN 5 #define SERVO3_PIN 4
#endif #endif
#endif
#if NUM_SERVOS > 3
#define SERVO3_PIN 4
#endif
#endif
#if MB(AZTEEG_X3_PRO)
#define BEEPER 33
#endif
#ifdef TEMP_STAT_LEDS
#if MB(AZTEEG_X3)
#define STAT_LED_RED 6
#define STAT_LED_BLUE 11
#endif #endif
#endif #endif
#ifdef ULTRA_LCD #ifdef ULTRA_LCD
#ifdef NEWPANEL #ifdef NEWPANEL
#define LCD_PINS_RS 16 #ifdef PANEL_ONE
#define LCD_PINS_ENABLE 17 #define LCD_PINS_RS 40
#define LCD_PINS_D4 23 #define LCD_PINS_ENABLE 42
#define LCD_PINS_D5 25 #define LCD_PINS_D4 65
#define LCD_PINS_D6 27 #define LCD_PINS_D5 66
#define LCD_PINS_D7 29 #define LCD_PINS_D6 44
#define LCD_PINS_D7 64
#else
#define LCD_PINS_RS 16
#define LCD_PINS_ENABLE 17
#define LCD_PINS_D4 23
#define LCD_PINS_D5 25
#define LCD_PINS_D6 27
#define LCD_PINS_D7 29
#endif
#ifdef REPRAP_DISCOUNT_SMART_CONTROLLER #ifdef REPRAP_DISCOUNT_SMART_CONTROLLER
#define BEEPER 37 #define BEEPER 37
@ -178,6 +172,10 @@
#define SHIFT_OUT 40 // shift register #define SHIFT_OUT 40 // shift register
#define SHIFT_CLK 44 // shift register #define SHIFT_CLK 44 // shift register
#define SHIFT_LD 42 // shift register #define SHIFT_LD 42 // shift register
#elif defined(PANEL_ONE)
#define BTN_EN1 59 // AUX2 PIN 3
#define BTN_EN2 63 // AUX2 PIN 4
#define BTN_ENC 49 // AUX3 PIN 7
#else #else
#define BTN_EN1 37 #define BTN_EN1 37
#define BTN_EN2 35 #define BTN_EN2 35

View file

@ -6,6 +6,10 @@
#error Oops! Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu. #error Oops! Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu.
#endif #endif
#if EXTRUDERS > 3
#error RUMBA supports up to 3 extruders. Comment this line to keep going.
#endif
#define X_STEP_PIN 17 #define X_STEP_PIN 17
#define X_DIR_PIN 16 #define X_DIR_PIN 16
#define X_ENABLE_PIN 48 #define X_ENABLE_PIN 48

View file

@ -3,3 +3,6 @@
*/ */
#include "pins_RAMPS_13.h" #include "pins_RAMPS_13.h"
#define FAN_PIN 9 // (Sprinter config)
#define HEATER_1_PIN -1

View file

@ -629,13 +629,21 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
block->direction_bits |= (1<<Y_AXIS); block->direction_bits |= (1<<Y_AXIS);
} }
#else #else
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
}
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0) if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{ {
block->direction_bits |= (1<<X_AXIS); block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
} }
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0) if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{ {
block->direction_bits |= (1<<Y_AXIS); block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
} }
#endif #endif
if (target[Z_AXIS] < position[Z_AXIS]) if (target[Z_AXIS] < position[Z_AXIS])

View file

@ -43,7 +43,7 @@ block_t *current_block; // A pointer to the block currently being traced
//=========================================================================== //===========================================================================
//=============================private variables ============================ //=============================private variables ============================
//=========================================================================== //===========================================================================
//static makes it inpossible to be called from outside of this file by extern.! //static makes it impossible to be called from outside of this file by extern.!
// Variables used by The Stepper Driver Interrupt // Variables used by The Stepper Driver Interrupt
static unsigned char out_bits; // The next stepping-bits to be output static unsigned char out_bits; // The next stepping-bits to be output
@ -187,7 +187,7 @@ void checkHitEndstops()
SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]); SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Z"); LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT "Z");
} }
SERIAL_ECHOLN(""); SERIAL_EOL;
endstop_x_hit=false; endstop_x_hit=false;
endstop_y_hit=false; endstop_y_hit=false;
endstop_z_hit=false; endstop_z_hit=false;
@ -399,85 +399,84 @@ ISR(TIMER1_COMPA_vect)
count_direction[Y_AXIS]=1; count_direction[Y_AXIS]=1;
} }
// Set direction en check limit switches if(check_endstops) // check X and Y Endstops
#ifndef COREXY {
if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis #ifndef COREXY
#else if ((out_bits & (1<<X_AXIS)) != 0) // stepping along -X axis (regular cartesians bot)
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) { //-X occurs for -A and -B #else
#endif if (!((current_block->steps_x == current_block->steps_y) && ((out_bits & (1<<X_AXIS))>>X_AXIS != (out_bits & (1<<Y_AXIS))>>Y_AXIS))) // AlexBorro: If DeltaX == -DeltaY, the movement is only in Y axis
CHECK_ENDSTOPS if ((out_bits & (1<<X_HEAD)) != 0) //AlexBorro: Head direction in -X axis for CoreXY bots.
{ #endif
#ifdef DUAL_X_CARRIAGE { // -direction
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder #ifdef DUAL_X_CARRIAGE
if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
|| (current_block->active_extruder != 0 && X2_HOME_DIR == -1)) if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
#endif #endif
{ {
#if defined(X_MIN_PIN) && X_MIN_PIN > -1 #if defined(X_MIN_PIN) && X_MIN_PIN > -1
bool x_min_endstop=(READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING); bool x_min_endstop=(READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) { if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0))
endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; {
endstop_x_hit=true; endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
step_events_completed = current_block->step_event_count; endstop_x_hit=true;
step_events_completed = current_block->step_event_count;
}
old_x_min_endstop = x_min_endstop;
#endif
} }
old_x_min_endstop = x_min_endstop;
#endif
} }
} else
} { // +direction
else { // +direction #ifdef DUAL_X_CARRIAGE
CHECK_ENDSTOPS // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
{ if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
#ifdef DUAL_X_CARRIAGE #endif
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder {
if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) #if defined(X_MAX_PIN) && X_MAX_PIN > -1
|| (current_block->active_extruder != 0 && X2_HOME_DIR == 1)) bool x_max_endstop=(READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
#endif if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0))
{ {
#if defined(X_MAX_PIN) && X_MAX_PIN > -1 endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
bool x_max_endstop=(READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING); endstop_x_hit=true;
if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){ step_events_completed = current_block->step_event_count;
endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; }
endstop_x_hit=true; old_x_max_endstop = x_max_endstop;
step_events_completed = current_block->step_event_count; #endif
} }
old_x_max_endstop = x_max_endstop;
#endif
} }
}
}
#ifndef COREXY #ifndef COREXY
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction if ((out_bits & (1<<Y_AXIS)) != 0) // -direction
#else #else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) { // -Y occurs for -A and +B if (!((current_block->steps_x == current_block->steps_y) && ((out_bits & (1<<X_AXIS))>>X_AXIS == (out_bits & (1<<Y_AXIS))>>Y_AXIS))) // AlexBorro: If DeltaX == DeltaY, the movement is only in X axis
#endif if ((out_bits & (1<<Y_HEAD)) != 0) //AlexBorro: Head direction in -Y axis for CoreXY bots.
CHECK_ENDSTOPS
{
#if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
bool y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed = current_block->step_event_count;
}
old_y_min_endstop = y_min_endstop;
#endif #endif
} { // -direction
} #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
else { // +direction bool y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
CHECK_ENDSTOPS if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0))
{ {
#if defined(Y_MAX_PIN) && Y_MAX_PIN > -1 endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
bool y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING); endstop_y_hit=true;
if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){ step_events_completed = current_block->step_event_count;
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; }
endstop_y_hit=true; old_y_min_endstop = y_min_endstop;
step_events_completed = current_block->step_event_count; #endif
} }
old_y_max_endstop = y_max_endstop; else
#endif { // +direction
} #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
bool y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0))
{
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed = current_block->step_event_count;
}
old_y_max_endstop = y_max_endstop;
#endif
}
} }
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
@ -960,51 +959,41 @@ void st_init()
//Initialize Step Pins //Initialize Step Pins
#if defined(X_STEP_PIN) && (X_STEP_PIN > -1) #if defined(X_STEP_PIN) && (X_STEP_PIN > -1)
SET_OUTPUT(X_STEP_PIN); OUT_WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
disable_x(); disable_x();
#endif #endif
#if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1) #if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1)
SET_OUTPUT(X2_STEP_PIN); OUT_WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
disable_x(); disable_x();
#endif #endif
#if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1) #if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1)
SET_OUTPUT(Y_STEP_PIN); OUT_WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
#if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1) #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1)
SET_OUTPUT(Y2_STEP_PIN); OUT_WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
#endif #endif
disable_y(); disable_y();
#endif #endif
#if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1) #if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1)
SET_OUTPUT(Z_STEP_PIN); OUT_WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
#if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1) #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1)
SET_OUTPUT(Z2_STEP_PIN); OUT_WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
#endif #endif
disable_z(); disable_z();
#endif #endif
#if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1) #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)
SET_OUTPUT(E0_STEP_PIN); OUT_WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
disable_e0(); disable_e0();
#endif #endif
#if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1) #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
SET_OUTPUT(E1_STEP_PIN); OUT_WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
disable_e1(); disable_e1();
#endif #endif
#if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1) #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
SET_OUTPUT(E2_STEP_PIN); OUT_WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
disable_e2(); disable_e2();
#endif #endif
#if defined(E3_STEP_PIN) && (E3_STEP_PIN > -1) #if defined(E3_STEP_PIN) && (E3_STEP_PIN > -1)
SET_OUTPUT(E3_STEP_PIN); OUT_WRITE(E3_STEP_PIN,INVERT_E_STEP_PIN);
WRITE(E3_STEP_PIN,INVERT_E_STEP_PIN);
disable_e3(); disable_e3();
#endif #endif

View file

@ -33,9 +33,43 @@
#include "ultralcd.h" #include "ultralcd.h"
#include "temperature.h" #include "temperature.h"
#include "watchdog.h" #include "watchdog.h"
#include "language.h"
#include "Sd2PinMap.h" #include "Sd2PinMap.h"
//===========================================================================
//================================== macros =================================
//===========================================================================
#if EXTRUDERS > 4
#error Unsupported number of extruders
#elif EXTRUDERS > 3
#define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2, v3, v4 }
#elif EXTRUDERS > 2
#define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2, v3 }
#elif EXTRUDERS > 1
#define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2 }
#else
#define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1 }
#endif
#define HAS_TEMP_0 (defined(TEMP_0_PIN) && TEMP_0_PIN >= 0)
#define HAS_TEMP_1 (defined(TEMP_1_PIN) && TEMP_1_PIN >= 0)
#define HAS_TEMP_2 (defined(TEMP_2_PIN) && TEMP_2_PIN >= 0)
#define HAS_TEMP_3 (defined(TEMP_3_PIN) && TEMP_3_PIN >= 0)
#define HAS_TEMP_BED (defined(TEMP_BED_PIN) && TEMP_BED_PIN >= 0)
#define HAS_FILAMENT_SENSOR (defined(FILAMENT_SENSOR) && defined(FILWIDTH_PIN) && FILWIDTH_PIN >= 0)
#define HAS_HEATER_0 (defined(HEATER_0_PIN) && HEATER_0_PIN >= 0)
#define HAS_HEATER_1 (defined(HEATER_1_PIN) && HEATER_1_PIN >= 0)
#define HAS_HEATER_2 (defined(HEATER_2_PIN) && HEATER_2_PIN >= 0)
#define HAS_HEATER_3 (defined(HEATER_3_PIN) && HEATER_3_PIN >= 0)
#define HAS_HEATER_BED (defined(HEATER_BED_PIN) && HEATER_BED_PIN >= 0)
#define HAS_AUTO_FAN_0 (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN >= 0)
#define HAS_AUTO_FAN_1 (defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN >= 0)
#define HAS_AUTO_FAN_2 (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN >= 0)
#define HAS_AUTO_FAN_3 (defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN >= 0)
#define HAS_AUTO_FAN HAS_AUTO_FAN_0 || HAS_AUTO_FAN_1 || HAS_AUTO_FAN_2 || HAS_AUTO_FAN_3
#define HAS_FAN (defined(FAN_PIN) && FAN_PIN >= 0)
//=========================================================================== //===========================================================================
//============================= public variables ============================ //============================= public variables ============================
@ -71,7 +105,7 @@ float current_temperature_bed = 0.0;
unsigned char soft_pwm_bed; unsigned char soft_pwm_bed;
#ifdef BABYSTEPPING #ifdef BABYSTEPPING
volatile int babystepsTodo[3]={0,0,0}; volatile int babystepsTodo[3] = { 0 };
#endif #endif
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
@ -116,40 +150,26 @@ static volatile bool temp_meas_ready = false;
#ifdef FAN_SOFT_PWM #ifdef FAN_SOFT_PWM
static unsigned char soft_pwm_fan; static unsigned char soft_pwm_fan;
#endif #endif
#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \ #if HAS_AUTO_FAN
(defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
(defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
static unsigned long extruder_autofan_last_check; static unsigned long extruder_autofan_last_check;
#endif #endif
#if EXTRUDERS > 4
# error Unsupported number of extruders
#elif EXTRUDERS > 3
# define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2, v3, v4 }
#elif EXTRUDERS > 2
# define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2, v3 }
#elif EXTRUDERS > 1
# define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1, v2 }
#else
# define ARRAY_BY_EXTRUDERS(v1, v2, v3, v4) { v1 }
#endif
#ifdef PIDTEMP #ifdef PIDTEMP
#ifdef PID_PARAMS_PER_EXTRUDER #ifdef PID_PARAMS_PER_EXTRUDER
float Kp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kp, DEFAULT_Kp, DEFAULT_Kp); float Kp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kp, DEFAULT_Kp, DEFAULT_Kp, DEFAULT_Kp);
float Ki[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Ki*PID_dT, DEFAULT_Ki*PID_dT, DEFAULT_Ki*PID_dT); float Ki[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Ki*PID_dT, DEFAULT_Ki*PID_dT, DEFAULT_Ki*PID_dT, DEFAULT_Ki*PID_dT);
float Kd[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kd / PID_dT, DEFAULT_Kd / PID_dT, DEFAULT_Kd / PID_dT); float Kd[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kd / PID_dT, DEFAULT_Kd / PID_dT, DEFAULT_Kd / PID_dT, DEFAULT_Kd / PID_dT);
#ifdef PID_ADD_EXTRUSION_RATE #ifdef PID_ADD_EXTRUSION_RATE
float Kc[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kc, DEFAULT_Kc, DEFAULT_Kc); float Kc[EXTRUDERS] = ARRAY_BY_EXTRUDERS(DEFAULT_Kc, DEFAULT_Kc, DEFAULT_Kc, DEFAULT_Kc);
#endif // PID_ADD_EXTRUSION_RATE #endif // PID_ADD_EXTRUSION_RATE
#else //PID_PARAMS_PER_EXTRUDER #else //PID_PARAMS_PER_EXTRUDER
float Kp = DEFAULT_Kp; float Kp = DEFAULT_Kp;
float Ki = DEFAULT_Ki * PID_dT; float Ki = DEFAULT_Ki * PID_dT;
float Kd = DEFAULT_Kd / PID_dT; float Kd = DEFAULT_Kd / PID_dT;
#ifdef PID_ADD_EXTRUSION_RATE #ifdef PID_ADD_EXTRUSION_RATE
float Kc = DEFAULT_Kc; float Kc = DEFAULT_Kc;
#endif // PID_ADD_EXTRUSION_RATE #endif // PID_ADD_EXTRUSION_RATE
#endif // PID_PARAMS_PER_EXTRUDER #endif // PID_PARAMS_PER_EXTRUDER
#endif //PIDTEMP #endif //PIDTEMP
// Init min and max temp with extreme values to prevent false errors during startup // Init min and max temp with extreme values to prevent false errors during startup
@ -159,7 +179,7 @@ static int minttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 0, 0, 0, 0 );
static int maxttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 16383, 16383, 16383, 16383 ); static int maxttemp[EXTRUDERS] = ARRAY_BY_EXTRUDERS( 16383, 16383, 16383, 16383 );
//static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP; /* No bed mintemp error implemented?!? */ //static int bed_minttemp_raw = HEATER_BED_RAW_LO_TEMP; /* No bed mintemp error implemented?!? */
#ifdef BED_MAXTEMP #ifdef BED_MAXTEMP
static int bed_maxttemp_raw = HEATER_BED_RAW_HI_TEMP; static int bed_maxttemp_raw = HEATER_BED_RAW_HI_TEMP;
#endif #endif
#ifdef TEMP_SENSOR_1_AS_REDUNDANT #ifdef TEMP_SENSOR_1_AS_REDUNDANT
@ -175,12 +195,12 @@ static float analog2tempBed(int raw);
static void updateTemperaturesFromRawValues(); static void updateTemperaturesFromRawValues();
#ifdef WATCH_TEMP_PERIOD #ifdef WATCH_TEMP_PERIOD
int watch_start_temp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0,0); int watch_start_temp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0,0);
unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0,0); unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0,0);
#endif //WATCH_TEMP_PERIOD #endif //WATCH_TEMP_PERIOD
#ifndef SOFT_PWM_SCALE #ifndef SOFT_PWM_SCALE
#define SOFT_PWM_SCALE 0 #define SOFT_PWM_SCALE 0
#endif #endif
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
@ -198,113 +218,98 @@ unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0,0);
void PID_autotune(float temp, int extruder, int ncycles) void PID_autotune(float temp, int extruder, int ncycles)
{ {
float input = 0.0; float input = 0.0;
int cycles=0; int cycles = 0;
bool heating = true; bool heating = true;
unsigned long temp_millis = millis(); unsigned long temp_millis = millis(), t1 = temp_millis, t2 = temp_millis;
unsigned long t1=temp_millis; long t_high = 0, t_low = 0;
unsigned long t2=temp_millis;
long t_high = 0;
long t_low = 0;
long bias, d; long bias, d;
float Ku, Tu; float Ku, Tu;
float Kp, Ki, Kd; float Kp, Ki, Kd;
float max = 0, min = 10000; float max = 0, min = 10000;
#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \ #if HAS_AUTO_FAN
(defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \ unsigned long extruder_autofan_last_check = temp_millis;
(defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1) || \
(defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN > -1)
unsigned long extruder_autofan_last_check = millis();
#endif
if ((extruder >= EXTRUDERS)
#if (TEMP_BED_PIN <= -1)
||(extruder < 0)
#endif #endif
){
SERIAL_ECHOLN("PID Autotune failed. Bad extruder number."); if (extruder >= EXTRUDERS
return; #if !HAS_TEMP_BED
} || extruder < 0
#endif
) {
SERIAL_ECHOLN(MSG_PID_BAD_EXTRUDER_NUM);
return;
}
SERIAL_ECHOLN("PID Autotune start"); SERIAL_ECHOLN(MSG_PID_AUTOTUNE_START);
disable_heater(); // switch off all heaters. disable_heater(); // switch off all heaters.
if (extruder<0) if (extruder < 0)
{ soft_pwm_bed = bias = d = MAX_BED_POWER / 2;
soft_pwm_bed = (MAX_BED_POWER)/2; else
bias = d = (MAX_BED_POWER)/2; soft_pwm[extruder] = bias = d = PID_MAX / 2;
}
else
{
soft_pwm[extruder] = (PID_MAX)/2;
bias = d = (PID_MAX)/2;
}
// PID Tuning loop
for(;;) {
unsigned long ms = millis();
if (temp_meas_ready == true) { // temp sample ready
for(;;) {
if(temp_meas_ready == true) { // temp sample ready
updateTemperaturesFromRawValues(); updateTemperaturesFromRawValues();
input = (extruder<0)?current_temperature_bed:current_temperature[extruder]; input = (extruder<0)?current_temperature_bed:current_temperature[extruder];
max=max(max,input); max = max(max, input);
min=min(min,input); min = min(min, input);
#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \ #if HAS_AUTO_FAN
(defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \ if (ms > extruder_autofan_last_check + 2500) {
(defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1) || \ checkExtruderAutoFans();
(defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN > -1) extruder_autofan_last_check = ms;
if(millis() - extruder_autofan_last_check > 2500) { }
checkExtruderAutoFans();
extruder_autofan_last_check = millis();
}
#endif #endif
if(heating == true && input > temp) { if (heating == true && input > temp) {
if(millis() - t2 > 5000) { if (ms - t2 > 5000) {
heating=false; heating = false;
if (extruder<0) if (extruder < 0)
soft_pwm_bed = (bias - d) >> 1; soft_pwm_bed = (bias - d) >> 1;
else else
soft_pwm[extruder] = (bias - d) >> 1; soft_pwm[extruder] = (bias - d) >> 1;
t1=millis(); t1 = ms;
t_high=t1 - t2; t_high = t1 - t2;
max=temp; max = temp;
} }
} }
if(heating == false && input < temp) { if (heating == false && input < temp) {
if(millis() - t1 > 5000) { if (ms - t1 > 5000) {
heating=true; heating = true;
t2=millis(); t2 = ms;
t_low=t2 - t1; t_low = t2 - t1;
if(cycles > 0) { if (cycles > 0) {
long max_pow = extruder < 0 ? MAX_BED_POWER : PID_MAX;
bias += (d*(t_high - t_low))/(t_low + t_high); bias += (d*(t_high - t_low))/(t_low + t_high);
bias = constrain(bias, 20 ,(extruder<0?(MAX_BED_POWER):(PID_MAX))-20); bias = constrain(bias, 20, max_pow - 20);
if(bias > (extruder<0?(MAX_BED_POWER):(PID_MAX))/2) d = (extruder<0?(MAX_BED_POWER):(PID_MAX)) - 1 - bias; d = (bias > max_pow / 2) ? max_pow - 1 - bias : bias;
else d = bias;
SERIAL_PROTOCOLPGM(" bias: "); SERIAL_PROTOCOL(bias); SERIAL_PROTOCOLPGM(MSG_BIAS); SERIAL_PROTOCOL(bias);
SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL(d); SERIAL_PROTOCOLPGM(MSG_D); SERIAL_PROTOCOL(d);
SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL(min); SERIAL_PROTOCOLPGM(MSG_MIN); SERIAL_PROTOCOL(min);
SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOLLN(max); SERIAL_PROTOCOLPGM(MSG_MAX); SERIAL_PROTOCOLLN(max);
if(cycles > 2) { if (cycles > 2) {
Ku = (4.0*d)/(3.14159*(max-min)/2.0); Ku = (4.0 * d) / (3.14159265 * (max - min) / 2.0);
Tu = ((float)(t_low + t_high)/1000.0); Tu = ((float)(t_low + t_high) / 1000.0);
SERIAL_PROTOCOLPGM(" Ku: "); SERIAL_PROTOCOL(Ku); SERIAL_PROTOCOLPGM(MSG_KU); SERIAL_PROTOCOL(Ku);
SERIAL_PROTOCOLPGM(" Tu: "); SERIAL_PROTOCOLLN(Tu); SERIAL_PROTOCOLPGM(MSG_TU); SERIAL_PROTOCOLLN(Tu);
Kp = 0.6*Ku; Kp = 0.6 * Ku;
Ki = 2*Kp/Tu; Ki = 2 * Kp / Tu;
Kd = Kp*Tu/8; Kd = Kp * Tu / 8;
SERIAL_PROTOCOLLNPGM(" Classic PID "); SERIAL_PROTOCOLLNPGM(MSG_CLASSIC_PID);
SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp); SERIAL_PROTOCOLPGM(MSG_KP); SERIAL_PROTOCOLLN(Kp);
SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki); SERIAL_PROTOCOLPGM(MSG_KI); SERIAL_PROTOCOLLN(Ki);
SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd); SERIAL_PROTOCOLPGM(MSG_KD); SERIAL_PROTOCOLLN(Kd);
/* /*
Kp = 0.33*Ku; Kp = 0.33*Ku;
Ki = Kp/Tu; Ki = Kp/Tu;
@ -323,79 +328,80 @@ void PID_autotune(float temp, int extruder, int ncycles)
*/ */
} }
} }
if (extruder<0) if (extruder < 0)
soft_pwm_bed = (bias + d) >> 1; soft_pwm_bed = (bias + d) >> 1;
else else
soft_pwm[extruder] = (bias + d) >> 1; soft_pwm[extruder] = (bias + d) >> 1;
cycles++; cycles++;
min=temp; min = temp;
} }
} }
} }
if(input > (temp + 20)) { if (input > temp + 20) {
SERIAL_PROTOCOLLNPGM("PID Autotune failed! Temperature too high"); SERIAL_PROTOCOLLNPGM(MSG_PID_TEMP_TOO_HIGH);
return; return;
} }
if(millis() - temp_millis > 2000) { // Every 2 seconds...
if (ms > temp_millis + 2000) {
int p; int p;
if (extruder<0){ if (extruder < 0) {
p=soft_pwm_bed; p = soft_pwm_bed;
SERIAL_PROTOCOLPGM("ok B:"); SERIAL_PROTOCOLPGM(MSG_OK_B);
}else{ }
p=soft_pwm[extruder]; else {
SERIAL_PROTOCOLPGM("ok T:"); p = soft_pwm[extruder];
SERIAL_PROTOCOLPGM(MSG_OK_T);
} }
SERIAL_PROTOCOL(input);
SERIAL_PROTOCOLPGM(" @:");
SERIAL_PROTOCOLLN(p);
temp_millis = millis(); SERIAL_PROTOCOL(input);
} SERIAL_PROTOCOLPGM(MSG_AT);
if(((millis() - t1) + (millis() - t2)) > (10L*60L*1000L*2L)) { SERIAL_PROTOCOLLN(p);
SERIAL_PROTOCOLLNPGM("PID Autotune failed! timeout");
temp_millis = ms;
} // every 2 seconds
// Over 2 minutes?
if (((ms - t1) + (ms - t2)) > (10L*60L*1000L*2L)) {
SERIAL_PROTOCOLLNPGM(MSG_PID_TIMEOUT);
return; return;
} }
if(cycles > ncycles) { if (cycles > ncycles) {
SERIAL_PROTOCOLLNPGM("PID Autotune finished! Put the last Kp, Ki and Kd constants from above into Configuration.h"); SERIAL_PROTOCOLLNPGM(MSG_PID_AUTOTUNE_FINISHED);
return; return;
} }
lcd_update(); lcd_update();
} }
} }
void updatePID() void updatePID() {
{ #ifdef PIDTEMP
#ifdef PIDTEMP for (int e = 0; e < EXTRUDERS; e++) {
for(int e = 0; e < EXTRUDERS; e++) { temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / PID_PARAM(Ki,e);
temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / PID_PARAM(Ki,e); }
} #endif
#endif #ifdef PIDTEMPBED
#ifdef PIDTEMPBED temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi; #endif
#endif
} }
int getHeaterPower(int heater) { int getHeaterPower(int heater) {
if (heater<0) return heater < 0 ? soft_pwm_bed : soft_pwm[heater];
return soft_pwm_bed;
return soft_pwm[heater];
} }
#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \ #if HAS_AUTO_FAN
(defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
(defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
#if defined(FAN_PIN) && FAN_PIN > -1 #if HAS_FAN
#if EXTRUDER_0_AUTO_FAN_PIN == FAN_PIN #if EXTRUDER_0_AUTO_FAN_PIN == FAN_PIN
#error "You cannot set EXTRUDER_0_AUTO_FAN_PIN equal to FAN_PIN" #error "You cannot set EXTRUDER_0_AUTO_FAN_PIN equal to FAN_PIN"
#endif #endif
#if EXTRUDER_1_AUTO_FAN_PIN == FAN_PIN #if EXTRUDER_1_AUTO_FAN_PIN == FAN_PIN
#error "You cannot set EXTRUDER_1_AUTO_FAN_PIN equal to FAN_PIN" #error "You cannot set EXTRUDER_1_AUTO_FAN_PIN equal to FAN_PIN"
#endif #endif
#if EXTRUDER_2_AUTO_FAN_PIN == FAN_PIN #if EXTRUDER_2_AUTO_FAN_PIN == FAN_PIN
#error "You cannot set EXTRUDER_2_AUTO_FAN_PIN equal to FAN_PIN" #error "You cannot set EXTRUDER_2_AUTO_FAN_PIN equal to FAN_PIN"
#endif #endif
#if EXTRUDER_3_AUTO_FAN_PIN == FAN_PIN
#error "You cannot set EXTRUDER_3_AUTO_FAN_PIN equal to FAN_PIN"
#endif
#endif #endif
void setExtruderAutoFanState(int pin, bool state) void setExtruderAutoFanState(int pin, bool state)
@ -412,20 +418,20 @@ void checkExtruderAutoFans()
uint8_t fanState = 0; uint8_t fanState = 0;
// which fan pins need to be turned on? // which fan pins need to be turned on?
#if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_0
if (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE) if (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE)
fanState |= 1; fanState |= 1;
#endif #endif
#if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_1
if (current_temperature[1] > EXTRUDER_AUTO_FAN_TEMPERATURE) if (current_temperature[1] > EXTRUDER_AUTO_FAN_TEMPERATURE)
{ {
if (EXTRUDER_1_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) if (EXTRUDER_1_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN)
fanState |= 1; fanState |= 1;
else else
fanState |= 2; fanState |= 2;
} }
#endif #endif
#if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_2
if (current_temperature[2] > EXTRUDER_AUTO_FAN_TEMPERATURE) if (current_temperature[2] > EXTRUDER_AUTO_FAN_TEMPERATURE)
{ {
if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) if (EXTRUDER_2_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN)
@ -436,7 +442,7 @@ void checkExtruderAutoFans()
fanState |= 4; fanState |= 4;
} }
#endif #endif
#if defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_3
if (current_temperature[3] > EXTRUDER_AUTO_FAN_TEMPERATURE) if (current_temperature[3] > EXTRUDER_AUTO_FAN_TEMPERATURE)
{ {
if (EXTRUDER_3_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN) if (EXTRUDER_3_AUTO_FAN_PIN == EXTRUDER_0_AUTO_FAN_PIN)
@ -451,69 +457,103 @@ void checkExtruderAutoFans()
#endif #endif
// update extruder auto fan states // update extruder auto fan states
#if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_0
setExtruderAutoFanState(EXTRUDER_0_AUTO_FAN_PIN, (fanState & 1) != 0); setExtruderAutoFanState(EXTRUDER_0_AUTO_FAN_PIN, (fanState & 1) != 0);
#endif #endif
#if defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_1
if (EXTRUDER_1_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN) if (EXTRUDER_1_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN)
setExtruderAutoFanState(EXTRUDER_1_AUTO_FAN_PIN, (fanState & 2) != 0); setExtruderAutoFanState(EXTRUDER_1_AUTO_FAN_PIN, (fanState & 2) != 0);
#endif #endif
#if defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_2
if (EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN if (EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN
&& EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN)
setExtruderAutoFanState(EXTRUDER_2_AUTO_FAN_PIN, (fanState & 4) != 0); setExtruderAutoFanState(EXTRUDER_2_AUTO_FAN_PIN, (fanState & 4) != 0);
#endif #endif
#if defined(EXTRUDER_3_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN > -1 #if HAS_AUTO_FAN_3
if (EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN if (EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN
&& EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_1_AUTO_FAN_PIN
&& EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_3_AUTO_FAN_PIN != EXTRUDER_2_AUTO_FAN_PIN)
setExtruderAutoFanState(EXTRUDER_3_AUTO_FAN_PIN, (fanState & 8) != 0); setExtruderAutoFanState(EXTRUDER_3_AUTO_FAN_PIN, (fanState & 8) != 0);
#endif #endif
} }
#endif // any extruder auto fan pins set #endif // any extruder auto fan pins set
void manage_heater() //
{ // Error checking and Write Routines
float pid_input; //
float pid_output; #if !HAS_HEATER_0
#error HEATER_0_PIN not defined for this board
#endif
#define WRITE_HEATER_0P(v) WRITE(HEATER_0_PIN, v)
#if EXTRUDERS > 1 || defined(HEATERS_PARALLEL)
#if !HAS_HEATER_1
#error HEATER_1_PIN not defined for this board
#endif
#define WRITE_HEATER_1(v) WRITE(HEATER_1_PIN, v)
#if EXTRUDERS > 2
#if !HAS_HEATER_2
#error HEATER_2_PIN not defined for this board
#endif
#define WRITE_HEATER_2(v) WRITE(HEATER_2_PIN, v)
#if EXTRUDERS > 3
#if !HAS_HEATER_3
#error HEATER_3_PIN not defined for this board
#endif
#define WRITE_HEATER_3(v) WRITE(HEATER_3_PIN, v)
#endif
#endif
#endif
#ifdef HEATERS_PARALLEL
#define WRITE_HEATER_0(v) { WRITE_HEATER_0P(v); WRITE_HEATER_1(v); }
#else
#define WRITE_HEATER_0(v) WRITE_HEATER_0P(v)
#endif
#if HAS_HEATER_BED
#define WRITE_HEATER_BED(v) WRITE(HEATER_BED_PIN, v)
#endif
#if HAS_FAN
#define WRITE_FAN(v) WRITE(FAN_PIN, v)
#endif
if(temp_meas_ready != true) //better readability void manage_heater() {
return;
if (!temp_meas_ready) return;
float pid_input, pid_output;
updateTemperaturesFromRawValues(); updateTemperaturesFromRawValues();
#ifdef HEATER_0_USES_MAX6675 #ifdef HEATER_0_USES_MAX6675
if (current_temperature[0] > 1023 || current_temperature[0] > HEATER_0_MAXTEMP) { float ct = current_temperature[0];
max_temp_error(0); if (ct > min(HEATER_0_MAXTEMP, 1023)) max_temp_error(0);
} if (ct < max(HEATER_0_MINTEMP, 0.01)) min_temp_error(0);
if (current_temperature[0] == 0 || current_temperature[0] < HEATER_0_MINTEMP) {
min_temp_error(0);
}
#endif //HEATER_0_USES_MAX6675 #endif //HEATER_0_USES_MAX6675
for(int e = 0; e < EXTRUDERS; e++) unsigned long ms = millis();
{
#if defined (THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 // Loop through all extruders
thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_RUNAWAY_PROTECTION_PERIOD, THERMAL_RUNAWAY_PROTECTION_HYSTERESIS); for (int e = 0; e < EXTRUDERS; e++) {
#endif
#ifdef PIDTEMP #if defined (THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0
pid_input = current_temperature[e]; thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_RUNAWAY_PROTECTION_PERIOD, THERMAL_RUNAWAY_PROTECTION_HYSTERESIS);
#endif
#ifndef PID_OPENLOOP #ifdef PIDTEMP
pid_input = current_temperature[e];
#ifndef PID_OPENLOOP
pid_error[e] = target_temperature[e] - pid_input; pid_error[e] = target_temperature[e] - pid_input;
if(pid_error[e] > PID_FUNCTIONAL_RANGE) { if (pid_error[e] > PID_FUNCTIONAL_RANGE) {
pid_output = BANG_MAX; pid_output = BANG_MAX;
pid_reset[e] = true; pid_reset[e] = true;
} }
else if(pid_error[e] < -PID_FUNCTIONAL_RANGE || target_temperature[e] == 0) { else if (pid_error[e] < -PID_FUNCTIONAL_RANGE || target_temperature[e] == 0) {
pid_output = 0; pid_output = 0;
pid_reset[e] = true; pid_reset[e] = true;
} }
else { else {
if(pid_reset[e] == true) { if (pid_reset[e] == true) {
temp_iState[e] = 0.0; temp_iState[e] = 0.0;
pid_reset[e] = false; pid_reset[e] = false;
} }
@ -524,95 +564,89 @@ void manage_heater()
//K1 defined in Configuration.h in the PID settings //K1 defined in Configuration.h in the PID settings
#define K2 (1.0-K1) #define K2 (1.0-K1)
dTerm[e] = (PID_PARAM(Kd,e) * (pid_input - temp_dState[e]))*K2 + (K1 * dTerm[e]); dTerm[e] = (PID_PARAM(Kd,e) * (pid_input - temp_dState[e])) * K2 + (K1 * dTerm[e]);
pid_output = pTerm[e] + iTerm[e] - dTerm[e]; pid_output = pTerm[e] + iTerm[e] - dTerm[e];
if (pid_output > PID_MAX) { if (pid_output > PID_MAX) {
if (pid_error[e] > 0 ) temp_iState[e] -= pid_error[e]; // conditional un-integration if (pid_error[e] > 0) temp_iState[e] -= pid_error[e]; // conditional un-integration
pid_output=PID_MAX; pid_output = PID_MAX;
} else if (pid_output < 0){ }
if (pid_error[e] < 0 ) temp_iState[e] -= pid_error[e]; // conditional un-integration else if (pid_output < 0) {
pid_output=0; if (pid_error[e] < 0) temp_iState[e] -= pid_error[e]; // conditional un-integration
pid_output = 0;
} }
} }
temp_dState[e] = pid_input; temp_dState[e] = pid_input;
#else #else
pid_output = constrain(target_temperature[e], 0, PID_MAX); pid_output = constrain(target_temperature[e], 0, PID_MAX);
#endif //PID_OPENLOOP #endif //PID_OPENLOOP
#ifdef PID_DEBUG
SERIAL_ECHO_START; #ifdef PID_DEBUG
SERIAL_ECHO(" PID_DEBUG "); SERIAL_ECHO_START;
SERIAL_ECHO(e); SERIAL_ECHO(MSG_PID_DEBUG);
SERIAL_ECHO(": Input "); SERIAL_ECHO(e);
SERIAL_ECHO(pid_input); SERIAL_ECHO(MSG_PID_DEBUG_INPUT);
SERIAL_ECHO(" Output "); SERIAL_ECHO(pid_input);
SERIAL_ECHO(pid_output); SERIAL_ECHO(MSG_PID_DEBUG_OUTPUT);
SERIAL_ECHO(" pTerm "); SERIAL_ECHO(pid_output);
SERIAL_ECHO(pTerm[e]); SERIAL_ECHO(MSG_PID_DEBUG_PTERM);
SERIAL_ECHO(" iTerm "); SERIAL_ECHO(pTerm[e]);
SERIAL_ECHO(iTerm[e]); SERIAL_ECHO(MSG_PID_DEBUG_ITERM);
SERIAL_ECHO(" dTerm "); SERIAL_ECHO(iTerm[e]);
SERIAL_ECHOLN(dTerm[e]); SERIAL_ECHO(MSG_PID_DEBUG_DTERM);
#endif //PID_DEBUG SERIAL_ECHOLN(dTerm[e]);
#else /* PID off */ #endif //PID_DEBUG
pid_output = 0;
if(current_temperature[e] < target_temperature[e]) { #else /* PID off */
pid_output = PID_MAX;
} pid_output = 0;
#endif if (current_temperature[e] < target_temperature[e]) pid_output = PID_MAX;
#endif
// Check if temperature is within the correct range // Check if temperature is within the correct range
if((current_temperature[e] > minttemp[e]) && (current_temperature[e] < maxttemp[e])) soft_pwm[e] = current_temperature[e] > minttemp[e] && current_temperature[e] < maxttemp[e] ? (int)pid_output >> 1 : 0;
{
soft_pwm[e] = (int)pid_output >> 1;
}
else {
soft_pwm[e] = 0;
}
#ifdef WATCH_TEMP_PERIOD #ifdef WATCH_TEMP_PERIOD
if(watchmillis[e] && millis() - watchmillis[e] > WATCH_TEMP_PERIOD) if (watchmillis[e] && ms > watchmillis[e] + WATCH_TEMP_PERIOD) {
{ if (degHotend(e) < watch_start_temp[e] + WATCH_TEMP_INCREASE) {
if(degHotend(e) < watch_start_temp[e] + WATCH_TEMP_INCREASE) setTargetHotend(0, e);
{ LCD_MESSAGEPGM(MSG_HEATING_FAILED_LCD); // translatable
setTargetHotend(0, e); SERIAL_ECHO_START;
LCD_MESSAGEPGM("Heating failed"); SERIAL_ECHOLNPGM(MSG_HEATING_FAILED);
SERIAL_ECHO_START;
SERIAL_ECHOLN("Heating failed");
}else{
watchmillis[e] = 0;
} }
} else {
#endif watchmillis[e] = 0;
}
}
#endif //WATCH_TEMP_PERIOD
#ifdef TEMP_SENSOR_1_AS_REDUNDANT #ifdef TEMP_SENSOR_1_AS_REDUNDANT
if(fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) { if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
disable_heater(); disable_heater();
if(IsStopped() == false) { if (IsStopped() == false) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Extruder switched off. Temperature difference between temp sensors is too high !"); SERIAL_ERRORLNPGM(MSG_EXTRUDER_SWITCHED_OFF);
LCD_ALERTMESSAGEPGM("Err: REDUNDANT TEMP ERROR"); LCD_ALERTMESSAGEPGM(MSG_ERR_REDUNDANT_TEMP); // translatable
} }
#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
Stop(); Stop();
#endif #endif
} }
#endif #endif //TEMP_SENSOR_1_AS_REDUNDANT
} // End extruder for loop
#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \ } // Extruders Loop
(defined(EXTRUDER_1_AUTO_FAN_PIN) && EXTRUDER_1_AUTO_FAN_PIN > -1) || \
(defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1) #if HAS_AUTO_FAN
if(millis() - extruder_autofan_last_check > 2500) // only need to check fan state very infrequently if (ms > extruder_autofan_last_check + 2500) { // only need to check fan state very infrequently
{ checkExtruderAutoFans();
checkExtruderAutoFans(); extruder_autofan_last_check = ms;
extruder_autofan_last_check = millis(); }
}
#endif #endif
#ifndef PIDTEMPBED #ifndef PIDTEMPBED
if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL) if (ms < previous_millis_bed_heater + BED_CHECK_INTERVAL) return;
return; previous_millis_bed_heater = ms;
previous_millis_bed_heater = millis(); #endif //PIDTEMPBED
#endif
#if TEMP_SENSOR_BED != 0 #if TEMP_SENSOR_BED != 0
@ -620,102 +654,75 @@ void manage_heater()
thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, 9, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS); thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, 9, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS);
#endif #endif
#ifdef PIDTEMPBED #ifdef PIDTEMPBED
pid_input = current_temperature_bed; pid_input = current_temperature_bed;
#ifndef PID_OPENLOOP #ifndef PID_OPENLOOP
pid_error_bed = target_temperature_bed - pid_input; pid_error_bed = target_temperature_bed - pid_input;
pTerm_bed = bedKp * pid_error_bed; pTerm_bed = bedKp * pid_error_bed;
temp_iState_bed += pid_error_bed; temp_iState_bed += pid_error_bed;
temp_iState_bed = constrain(temp_iState_bed, temp_iState_min_bed, temp_iState_max_bed); temp_iState_bed = constrain(temp_iState_bed, temp_iState_min_bed, temp_iState_max_bed);
iTerm_bed = bedKi * temp_iState_bed; iTerm_bed = bedKi * temp_iState_bed;
//K1 defined in Configuration.h in the PID settings //K1 defined in Configuration.h in the PID settings
#define K2 (1.0-K1) #define K2 (1.0-K1)
dTerm_bed= (bedKd * (pid_input - temp_dState_bed))*K2 + (K1 * dTerm_bed); dTerm_bed = (bedKd * (pid_input - temp_dState_bed))*K2 + (K1 * dTerm_bed);
temp_dState_bed = pid_input; temp_dState_bed = pid_input;
pid_output = pTerm_bed + iTerm_bed - dTerm_bed; pid_output = pTerm_bed + iTerm_bed - dTerm_bed;
if (pid_output > MAX_BED_POWER) { if (pid_output > MAX_BED_POWER) {
if (pid_error_bed > 0 ) temp_iState_bed -= pid_error_bed; // conditional un-integration if (pid_error_bed > 0) temp_iState_bed -= pid_error_bed; // conditional un-integration
pid_output=MAX_BED_POWER; pid_output = MAX_BED_POWER;
} else if (pid_output < 0){ }
if (pid_error_bed < 0 ) temp_iState_bed -= pid_error_bed; // conditional un-integration else if (pid_output < 0) {
pid_output=0; if (pid_error_bed < 0) temp_iState_bed -= pid_error_bed; // conditional un-integration
} pid_output = 0;
}
#else #else
pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER); pid_output = constrain(target_temperature_bed, 0, MAX_BED_POWER);
#endif //PID_OPENLOOP #endif //PID_OPENLOOP
if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP)) soft_pwm_bed = current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP ? (int)pid_output >> 1 : 0;
{
soft_pwm_bed = (int)pid_output >> 1;
}
else {
soft_pwm_bed = 0;
}
#elif !defined(BED_LIMIT_SWITCHING) #elif !defined(BED_LIMIT_SWITCHING)
// Check if temperature is within the correct range // Check if temperature is within the correct range
if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP)) if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
{ soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
if(current_temperature_bed >= target_temperature_bed)
{
soft_pwm_bed = 0;
}
else
{
soft_pwm_bed = MAX_BED_POWER>>1;
}
} }
else else {
{
soft_pwm_bed = 0; soft_pwm_bed = 0;
WRITE(HEATER_BED_PIN,LOW); WRITE_HEATER_BED(LOW);
} }
#else //#ifdef BED_LIMIT_SWITCHING #else //#ifdef BED_LIMIT_SWITCHING
// Check if temperature is within the correct band // Check if temperature is within the correct band
if((current_temperature_bed > BED_MINTEMP) && (current_temperature_bed < BED_MAXTEMP)) if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
{ if (current_temperature_bed >= target_temperature_bed + BED_HYSTERESIS)
if(current_temperature_bed > target_temperature_bed + BED_HYSTERESIS)
{
soft_pwm_bed = 0; soft_pwm_bed = 0;
} else if (current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS)
else if(current_temperature_bed <= target_temperature_bed - BED_HYSTERESIS) soft_pwm_bed = MAX_BED_POWER >> 1;
{
soft_pwm_bed = MAX_BED_POWER>>1;
}
} }
else else {
{
soft_pwm_bed = 0; soft_pwm_bed = 0;
WRITE(HEATER_BED_PIN,LOW); WRITE_HEATER_BED(LOW);
} }
#endif #endif
#endif #endif //TEMP_SENSOR_BED != 0
//code for controlling the extruder rate based on the width sensor // Control the extruder rate based on the width sensor
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
if(filament_sensor) if (filament_sensor) {
{ meas_shift_index = delay_index1 - meas_delay_cm;
meas_shift_index=delay_index1-meas_delay_cm; if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1; //loop around buffer if needed
if(meas_shift_index<0)
meas_shift_index = meas_shift_index + (MAX_MEASUREMENT_DELAY+1); //loop around buffer if needed
//get the delayed info and add 100 to reconstitute to a percent of the nominal filament diameter // Get the delayed info and add 100 to reconstitute to a percent of
//then square it to get an area // the nominal filament diameter then square it to get an area
meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
if(meas_shift_index<0) float vm = pow((measurement_delay[meas_shift_index] + 100.0) / 100.0, 2);
meas_shift_index=0; if (vm < 0.01) vm = 0.01;
else if (meas_shift_index>MAX_MEASUREMENT_DELAY) volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = vm;
meas_shift_index=MAX_MEASUREMENT_DELAY; }
#endif //FILAMENT_SENSOR
volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/100.0,2);
if (volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] <0.01)
volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]=0.01;
}
#endif
} }
#define PGM_RD_W(x) (short)pgm_read_word(&x) #define PGM_RD_W(x) (short)pgm_read_word(&x)
@ -723,14 +730,14 @@ void manage_heater()
// For hot end temperature measurement. // For hot end temperature measurement.
static float analog2temp(int raw, uint8_t e) { static float analog2temp(int raw, uint8_t e) {
#ifdef TEMP_SENSOR_1_AS_REDUNDANT #ifdef TEMP_SENSOR_1_AS_REDUNDANT
if(e > EXTRUDERS) if (e > EXTRUDERS)
#else #else
if(e >= EXTRUDERS) if (e >= EXTRUDERS)
#endif #endif
{ {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERROR((int)e); SERIAL_ERROR((int)e);
SERIAL_ERRORLNPGM(" - Invalid extruder number !"); SERIAL_ERRORLNPGM(MSG_INVALID_EXTRUDER_NUM);
kill(); kill();
return 0.0; return 0.0;
} }
@ -799,54 +806,45 @@ static float analog2tempBed(int raw) {
/* Called to get the raw values into the the actual temperatures. The raw values are created in interrupt context, /* Called to get the raw values into the the actual temperatures. The raw values are created in interrupt context,
and this function is called from normal context as it is too slow to run in interrupts and will block the stepper routine otherwise */ and this function is called from normal context as it is too slow to run in interrupts and will block the stepper routine otherwise */
static void updateTemperaturesFromRawValues() static void updateTemperaturesFromRawValues() {
{ #ifdef HEATER_0_USES_MAX6675
#ifdef HEATER_0_USES_MAX6675 current_temperature_raw[0] = read_max6675();
current_temperature_raw[0] = read_max6675(); #endif
#endif for(uint8_t e = 0; e < EXTRUDERS; e++) {
for(uint8_t e=0;e<EXTRUDERS;e++) current_temperature[e] = analog2temp(current_temperature_raw[e], e);
{ }
current_temperature[e] = analog2temp(current_temperature_raw[e], e); current_temperature_bed = analog2tempBed(current_temperature_bed_raw);
} #ifdef TEMP_SENSOR_1_AS_REDUNDANT
current_temperature_bed = analog2tempBed(current_temperature_bed_raw); redundant_temperature = analog2temp(redundant_temperature_raw, 1);
#ifdef TEMP_SENSOR_1_AS_REDUNDANT #endif
redundant_temperature = analog2temp(redundant_temperature_raw, 1); #if HAS_FILAMENT_SENSOR
#endif filament_width_meas = analog2widthFil();
#if defined (FILAMENT_SENSOR) && (FILWIDTH_PIN > -1) //check if a sensor is supported #endif
filament_width_meas = analog2widthFil(); //Reset the watchdog after we know we have a temperature measurement.
#endif watchdog_reset();
//Reset the watchdog after we know we have a temperature measurement.
watchdog_reset();
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
temp_meas_ready = false; temp_meas_ready = false;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
// For converting raw Filament Width to milimeters
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
float analog2widthFil() {
return current_raw_filwidth/16383.0*5.0;
//return current_raw_filwidth;
}
// For converting raw Filament Width to a ratio
int widthFil_to_size_ratio() {
float temp;
temp=filament_width_meas;
if(filament_width_meas<MEASURED_LOWER_LIMIT)
temp=filament_width_nominal; //assume sensor cut out
else if (filament_width_meas>MEASURED_UPPER_LIMIT)
temp= MEASURED_UPPER_LIMIT;
// Convert raw Filament Width to millimeters
float analog2widthFil() {
return current_raw_filwidth / 16383.0 * 5.0;
//return current_raw_filwidth;
}
return(filament_width_nominal/temp*100); // Convert raw Filament Width to a ratio
int widthFil_to_size_ratio() {
float temp = filament_width_meas;
if (temp < MEASURED_LOWER_LIMIT) temp = filament_width_nominal; //assume sensor cut out
else if (temp > MEASURED_UPPER_LIMIT) temp = MEASURED_UPPER_LIMIT;
return filament_width_nominal / temp * 100;
}
}
#endif #endif
@ -855,123 +853,95 @@ return(filament_width_nominal/temp*100);
void tp_init() void tp_init()
{ {
#if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1)) #if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))
//disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector //disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
MCUCR=(1<<JTD); MCUCR=(1<<JTD);
MCUCR=(1<<JTD); MCUCR=(1<<JTD);
#endif #endif
// Finish init of mult extruder arrays // Finish init of mult extruder arrays
for(int e = 0; e < EXTRUDERS; e++) { for (int e = 0; e < EXTRUDERS; e++) {
// populate with the first value // populate with the first value
maxttemp[e] = maxttemp[0]; maxttemp[e] = maxttemp[0];
#ifdef PIDTEMP #ifdef PIDTEMP
temp_iState_min[e] = 0.0; temp_iState_min[e] = 0.0;
temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / PID_PARAM(Ki,e); temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / PID_PARAM(Ki,e);
#endif //PIDTEMP #endif //PIDTEMP
#ifdef PIDTEMPBED #ifdef PIDTEMPBED
temp_iState_min_bed = 0.0; temp_iState_min_bed = 0.0;
temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi; temp_iState_max_bed = PID_INTEGRAL_DRIVE_MAX / bedKi;
#endif //PIDTEMPBED #endif //PIDTEMPBED
} }
#if defined(HEATER_0_PIN) && (HEATER_0_PIN > -1) #if HAS_HEATER_0
SET_OUTPUT(HEATER_0_PIN); SET_OUTPUT(HEATER_0_PIN);
#endif #endif
#if defined(HEATER_1_PIN) && (HEATER_1_PIN > -1) #if HAS_HEATER_1
SET_OUTPUT(HEATER_1_PIN); SET_OUTPUT(HEATER_1_PIN);
#endif #endif
#if defined(HEATER_2_PIN) && (HEATER_2_PIN > -1) #if HAS_HEATER_2
SET_OUTPUT(HEATER_2_PIN); SET_OUTPUT(HEATER_2_PIN);
#endif #endif
#if defined(HEATER_3_PIN) && (HEATER_3_PIN > -1) #if HAS_HEATER_3
SET_OUTPUT(HEATER_3_PIN); SET_OUTPUT(HEATER_3_PIN);
#endif #endif
#if defined(HEATER_BED_PIN) && (HEATER_BED_PIN > -1) #if HAS_HEATER_BED
SET_OUTPUT(HEATER_BED_PIN); SET_OUTPUT(HEATER_BED_PIN);
#endif #endif
#if defined(FAN_PIN) && (FAN_PIN > -1) #if HAS_FAN
SET_OUTPUT(FAN_PIN); SET_OUTPUT(FAN_PIN);
#ifdef FAST_PWM_FAN #ifdef FAST_PWM_FAN
setPwmFrequency(FAN_PIN, 1); // No prescaling. Pwm frequency = F_CPU/256/8 setPwmFrequency(FAN_PIN, 1); // No prescaling. Pwm frequency = F_CPU/256/8
#endif #endif
#ifdef FAN_SOFT_PWM #ifdef FAN_SOFT_PWM
soft_pwm_fan = fanSpeedSoftPwm / 2; soft_pwm_fan = fanSpeedSoftPwm / 2;
#endif #endif
#endif #endif
#ifdef HEATER_0_USES_MAX6675 #ifdef HEATER_0_USES_MAX6675
#ifndef SDSUPPORT #ifndef SDSUPPORT
SET_OUTPUT(SCK_PIN); OUT_WRITE(SCK_PIN, LOW);
WRITE(SCK_PIN,0); OUT_WRITE(MOSI_PIN, HIGH);
OUT_WRITE(MISO_PIN, HIGH);
SET_OUTPUT(MOSI_PIN);
WRITE(MOSI_PIN,1);
SET_INPUT(MISO_PIN);
WRITE(MISO_PIN,1);
#else #else
pinMode(SS_PIN, OUTPUT); pinMode(SS_PIN, OUTPUT);
digitalWrite(SS_PIN, HIGH); digitalWrite(SS_PIN, HIGH);
#endif #endif
SET_OUTPUT(MAX6675_SS); OUT_WRITE(MAX6675_SS,HIGH);
WRITE(MAX6675_SS,1);
#endif //HEATER_0_USES_MAX6675 #endif //HEATER_0_USES_MAX6675
#ifdef DIDR2
#define ANALOG_SELECT(pin) do{ if (pin < 8) DIDR0 |= 1 << pin; else DIDR2 |= 1 << (pin - 8); }while(0)
#else
#define ANALOG_SELECT(pin) do{ DIDR0 |= 1 << pin; }while(0)
#endif
// Set analog inputs // Set analog inputs
ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07; ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;
DIDR0 = 0; DIDR0 = 0;
#ifdef DIDR2 #ifdef DIDR2
DIDR2 = 0; DIDR2 = 0;
#endif #endif
#if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1) #if HAS_TEMP_0
#if TEMP_0_PIN < 8 ANALOG_SELECT(TEMP_0_PIN);
DIDR0 |= 1 << TEMP_0_PIN;
#else
DIDR2 |= 1<<(TEMP_0_PIN - 8);
#endif
#endif #endif
#if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1) #if HAS_TEMP_1
#if TEMP_1_PIN < 8 ANALOG_SELECT(TEMP_1_PIN);
DIDR0 |= 1<<TEMP_1_PIN;
#else
DIDR2 |= 1<<(TEMP_1_PIN - 8);
#endif
#endif #endif
#if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1) #if HAS_TEMP_2
#if TEMP_2_PIN < 8 ANALOG_SELECT(TEMP_2_PIN);
DIDR0 |= 1 << TEMP_2_PIN;
#else
DIDR2 |= 1<<(TEMP_2_PIN - 8);
#endif
#endif #endif
#if defined(TEMP_3_PIN) && (TEMP_3_PIN > -1) #if HAS_TEMP_3
#if TEMP_3_PIN < 8 ANALOG_SELECT(TEMP_3_PIN);
DIDR0 |= 1 << TEMP_3_PIN;
#else
DIDR2 |= 1<<(TEMP_3_PIN - 8);
#endif
#endif #endif
#if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1) #if HAS_TEMP_BED
#if TEMP_BED_PIN < 8 ANALOG_SELECT(TEMP_BED_PIN);
DIDR0 |= 1<<TEMP_BED_PIN;
#else
DIDR2 |= 1<<(TEMP_BED_PIN - 8);
#endif
#endif #endif
#if HAS_FILAMENT_SENSOR
//Added for Filament Sensor ANALOG_SELECT(FILWIDTH_PIN);
#ifdef FILAMENT_SENSOR
#if defined(FILWIDTH_PIN) && (FILWIDTH_PIN > -1)
#if FILWIDTH_PIN < 8
DIDR0 |= 1<<FILWIDTH_PIN;
#else
DIDR2 |= 1<<(FILWIDTH_PIN - 8);
#endif
#endif
#endif #endif
// Use timer0 for temperature measurement // Use timer0 for temperature measurement
@ -982,128 +952,89 @@ void tp_init()
// Wait for temperature measurement to settle // Wait for temperature measurement to settle
delay(250); delay(250);
#ifdef HEATER_0_MINTEMP #define TEMP_MIN_ROUTINE(NR) \
minttemp[0] = HEATER_0_MINTEMP; minttemp[NR] = HEATER_ ## NR ## _MINTEMP; \
while(analog2temp(minttemp_raw[0], 0) < HEATER_0_MINTEMP) { while(analog2temp(minttemp_raw[NR], NR) < HEATER_ ## NR ## _MINTEMP) { \
#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP if (HEATER_ ## NR ## _RAW_LO_TEMP < HEATER_ ## NR ## _RAW_HI_TEMP) \
minttemp_raw[0] += OVERSAMPLENR; minttemp_raw[NR] += OVERSAMPLENR; \
#else else \
minttemp_raw[0] -= OVERSAMPLENR; minttemp_raw[NR] -= OVERSAMPLENR; \
#endif }
} #define TEMP_MAX_ROUTINE(NR) \
#endif //MINTEMP maxttemp[NR] = HEATER_ ## NR ## _MAXTEMP; \
#ifdef HEATER_0_MAXTEMP while(analog2temp(maxttemp_raw[NR], NR) > HEATER_ ## NR ## _MAXTEMP) { \
maxttemp[0] = HEATER_0_MAXTEMP; if (HEATER_ ## NR ## _RAW_LO_TEMP < HEATER_ ## NR ## _RAW_HI_TEMP) \
while(analog2temp(maxttemp_raw[0], 0) > HEATER_0_MAXTEMP) { maxttemp_raw[NR] -= OVERSAMPLENR; \
#if HEATER_0_RAW_LO_TEMP < HEATER_0_RAW_HI_TEMP else \
maxttemp_raw[0] -= OVERSAMPLENR; maxttemp_raw[NR] += OVERSAMPLENR; \
#else }
maxttemp_raw[0] += OVERSAMPLENR;
#endif
}
#endif //MAXTEMP
#if (EXTRUDERS > 1) && defined(HEATER_1_MINTEMP) #ifdef HEATER_0_MINTEMP
minttemp[1] = HEATER_1_MINTEMP; TEMP_MIN_ROUTINE(0);
while(analog2temp(minttemp_raw[1], 1) < HEATER_1_MINTEMP) { #endif
#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP #ifdef HEATER_0_MAXTEMP
minttemp_raw[1] += OVERSAMPLENR; TEMP_MAX_ROUTINE(0);
#else #endif
minttemp_raw[1] -= OVERSAMPLENR; #if EXTRUDERS > 1
#endif #ifdef HEATER_1_MINTEMP
} TEMP_MIN_ROUTINE(1);
#endif // MINTEMP 1 #endif
#if (EXTRUDERS > 1) && defined(HEATER_1_MAXTEMP) #ifdef HEATER_1_MAXTEMP
maxttemp[1] = HEATER_1_MAXTEMP; TEMP_MAX_ROUTINE(1);
while(analog2temp(maxttemp_raw[1], 1) > HEATER_1_MAXTEMP) { #endif
#if HEATER_1_RAW_LO_TEMP < HEATER_1_RAW_HI_TEMP #if EXTRUDERS > 2
maxttemp_raw[1] -= OVERSAMPLENR; #ifdef HEATER_2_MINTEMP
#else TEMP_MIN_ROUTINE(2);
maxttemp_raw[1] += OVERSAMPLENR; #endif
#endif #ifdef HEATER_2_MAXTEMP
} TEMP_MAX_ROUTINE(2);
#endif //MAXTEMP 1 #endif
#if EXTRUDERS > 3
#ifdef HEATER_3_MINTEMP
TEMP_MIN_ROUTINE(3);
#endif
#ifdef HEATER_3_MAXTEMP
TEMP_MAX_ROUTINE(3);
#endif
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
#if (EXTRUDERS > 2) && defined(HEATER_2_MINTEMP) #ifdef BED_MINTEMP
minttemp[2] = HEATER_2_MINTEMP; /* No bed MINTEMP error implemented?!? */ /*
while(analog2temp(minttemp_raw[2], 2) < HEATER_2_MINTEMP) { while(analog2tempBed(bed_minttemp_raw) < BED_MINTEMP) {
#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP #if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
minttemp_raw[2] += OVERSAMPLENR; bed_minttemp_raw += OVERSAMPLENR;
#else #else
minttemp_raw[2] -= OVERSAMPLENR; bed_minttemp_raw -= OVERSAMPLENR;
#endif #endif
} }
#endif //MINTEMP 2 */
#if (EXTRUDERS > 2) && defined(HEATER_2_MAXTEMP) #endif //BED_MINTEMP
maxttemp[2] = HEATER_2_MAXTEMP; #ifdef BED_MAXTEMP
while(analog2temp(maxttemp_raw[2], 2) > HEATER_2_MAXTEMP) { while(analog2tempBed(bed_maxttemp_raw) > BED_MAXTEMP) {
#if HEATER_2_RAW_LO_TEMP < HEATER_2_RAW_HI_TEMP #if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
maxttemp_raw[2] -= OVERSAMPLENR; bed_maxttemp_raw -= OVERSAMPLENR;
#else #else
maxttemp_raw[2] += OVERSAMPLENR; bed_maxttemp_raw += OVERSAMPLENR;
#endif #endif
} }
#endif //MAXTEMP 2 #endif //BED_MAXTEMP
#if (EXTRUDERS > 3) && defined(HEATER_3_MINTEMP)
minttemp[3] = HEATER_3_MINTEMP;
while(analog2temp(minttemp_raw[3], 3) < HEATER_3_MINTEMP) {
#if HEATER_3_RAW_LO_TEMP < HEATER_3_RAW_HI_TEMP
minttemp_raw[3] += OVERSAMPLENR;
#else
minttemp_raw[3] -= OVERSAMPLENR;
#endif
}
#endif //MINTEMP 3
#if (EXTRUDERS > 3) && defined(HEATER_3_MAXTEMP)
maxttemp[3] = HEATER_3_MAXTEMP;
while(analog2temp(maxttemp_raw[3], 3) > HEATER_3_MAXTEMP) {
#if HEATER_3_RAW_LO_TEMP < HEATER_3_RAW_HI_TEMP
maxttemp_raw[3] -= OVERSAMPLENR;
#else
maxttemp_raw[3] += OVERSAMPLENR;
#endif
}
#endif // MAXTEMP 3
#ifdef BED_MINTEMP
/* No bed MINTEMP error implemented?!? */ /*
while(analog2tempBed(bed_minttemp_raw) < BED_MINTEMP) {
#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
bed_minttemp_raw += OVERSAMPLENR;
#else
bed_minttemp_raw -= OVERSAMPLENR;
#endif
}
*/
#endif //BED_MINTEMP
#ifdef BED_MAXTEMP
while(analog2tempBed(bed_maxttemp_raw) > BED_MAXTEMP) {
#if HEATER_BED_RAW_LO_TEMP < HEATER_BED_RAW_HI_TEMP
bed_maxttemp_raw -= OVERSAMPLENR;
#else
bed_maxttemp_raw += OVERSAMPLENR;
#endif
}
#endif //BED_MAXTEMP
} }
void setWatch() void setWatch() {
{ #ifdef WATCH_TEMP_PERIOD
#ifdef WATCH_TEMP_PERIOD unsigned long ms = millis();
for (int e = 0; e < EXTRUDERS; e++) for (int e = 0; e < EXTRUDERS; e++) {
{ if (degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2)) {
if(degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2)) watch_start_temp[e] = degHotend(e);
{ watchmillis[e] = ms;
watch_start_temp[e] = degHotend(e); }
watchmillis[e] = millis(); }
} #endif
}
#endif
} }
#if defined (THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 #if defined(THERMAL_RUNAWAY_PROTECTION_PERIOD) && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0
void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc)
{ {
/* /*
@ -1135,16 +1066,18 @@ void thermal_runaway_protection(int *state, unsigned long *timer, float temperat
if (temperature >= target_temperature) *state = 2; if (temperature >= target_temperature) *state = 2;
break; break;
case 2: // "Temperature Stable" state case 2: // "Temperature Stable" state
{
unsigned long ms = millis();
if (temperature >= (target_temperature - hysteresis_degc)) if (temperature >= (target_temperature - hysteresis_degc))
{ {
*timer = millis(); *timer = ms;
} }
else if ( (millis() - *timer) > ((unsigned long) period_seconds) * 1000) else if ( (ms - *timer) > ((unsigned long) period_seconds) * 1000)
{ {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Thermal Runaway, system stopped! Heater_ID: "); SERIAL_ERRORLNPGM(MSG_THERMAL_RUNAWAY_STOP);
SERIAL_ERRORLN((int)heater_id); SERIAL_ERRORLN((int)heater_id);
LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY"); LCD_ALERTMESSAGEPGM(MSG_THERMAL_RUNAWAY); // translatable
thermal_runaway = true; thermal_runaway = true;
while(1) while(1)
{ {
@ -1160,56 +1093,47 @@ void thermal_runaway_protection(int *state, unsigned long *timer, float temperat
lcd_update(); lcd_update();
} }
} }
break; } break;
} }
} }
#endif #endif //THERMAL_RUNAWAY_PROTECTION_PERIOD
void disable_heater()
{ void disable_heater() {
for(int i=0;i<EXTRUDERS;i++) for (int i=0; i<EXTRUDERS; i++) setTargetHotend(0, i);
setTargetHotend(0,i);
setTargetBed(0); setTargetBed(0);
#if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
target_temperature[0]=0; #if HAS_TEMP_0
soft_pwm[0]=0; target_temperature[0] = 0;
#if defined(HEATER_0_PIN) && HEATER_0_PIN > -1 soft_pwm[0] = 0;
WRITE(HEATER_0_PIN,LOW); WRITE_HEATER_0P(LOW); // If HEATERS_PARALLEL should apply, change to WRITE_HEATER_0
#endif
#endif
#if defined(TEMP_1_PIN) && TEMP_1_PIN > -1 && EXTRUDERS > 1
target_temperature[1]=0;
soft_pwm[1]=0;
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
WRITE(HEATER_1_PIN,LOW);
#endif
#endif
#if defined(TEMP_2_PIN) && TEMP_2_PIN > -1 && EXTRUDERS > 2
target_temperature[2]=0;
soft_pwm[2]=0;
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
WRITE(HEATER_2_PIN,LOW);
#endif
#endif #endif
#if defined(TEMP_3_PIN) && TEMP_3_PIN > -1 && EXTRUDERS > 3 #if EXTRUDERS > 1 && HAS_TEMP_1
target_temperature[3]=0; target_temperature[1] = 0;
soft_pwm[3]=0; soft_pwm[1] = 0;
#if defined(HEATER_3_PIN) && HEATER_3_PIN > -1 WRITE_HEATER_1(LOW);
WRITE(HEATER_3_PIN,LOW); #endif
#endif
#endif
#if EXTRUDERS > 2 && HAS_TEMP_2
target_temperature[2] = 0;
soft_pwm[2] = 0;
WRITE_HEATER_2(LOW);
#endif
#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1 #if EXTRUDERS > 3 && HAS_TEMP_3
target_temperature_bed=0; target_temperature[3] = 0;
soft_pwm_bed=0; soft_pwm[3] = 0;
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1 WRITE_HEATER_3(LOW);
WRITE(HEATER_BED_PIN,LOW); #endif
#if HAS_TEMP_BED
target_temperature_bed = 0;
soft_pwm_bed = 0;
#if HAS_HEATER_BED
WRITE_HEATER_BED(LOW);
#endif #endif
#endif #endif
} }
void max_temp_error(uint8_t e) { void max_temp_error(uint8_t e) {
@ -1217,8 +1141,8 @@ void max_temp_error(uint8_t e) {
if(IsStopped() == false) { if(IsStopped() == false) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLN((int)e); SERIAL_ERRORLN((int)e);
SERIAL_ERRORLNPGM(": Extruder switched off. MAXTEMP triggered !"); SERIAL_ERRORLNPGM(MSG_MAXTEMP_EXTRUDER_OFF);
LCD_ALERTMESSAGEPGM("Err: MAXTEMP"); LCD_ALERTMESSAGEPGM(MSG_ERR_MAXTEMP); // translatable
} }
#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
Stop(); Stop();
@ -1230,8 +1154,8 @@ void min_temp_error(uint8_t e) {
if(IsStopped() == false) { if(IsStopped() == false) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLN((int)e); SERIAL_ERRORLN((int)e);
SERIAL_ERRORLNPGM(": Extruder switched off. MINTEMP triggered !"); SERIAL_ERRORLNPGM(MSG_MINTEMP_EXTRUDER_OFF);
LCD_ALERTMESSAGEPGM("Err: MINTEMP"); LCD_ALERTMESSAGEPGM(MSG_ERR_MINTEMP); // translatable
} }
#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
Stop(); Stop();
@ -1239,13 +1163,13 @@ void min_temp_error(uint8_t e) {
} }
void bed_max_temp_error(void) { void bed_max_temp_error(void) {
#if HEATER_BED_PIN > -1 #if HAS_HEATER_BED
WRITE(HEATER_BED_PIN, 0); WRITE_HEATER_BED(0);
#endif #endif
if(IsStopped() == false) { if (IsStopped() == false) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !!"); SERIAL_ERRORLNPGM(MSG_MAXTEMP_BED_OFF);
LCD_ALERTMESSAGEPGM("Err: MAXTEMP BED"); LCD_ALERTMESSAGEPGM(MSG_ERR_MAXTEMP_BED); // translatable
} }
#ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
Stop(); Stop();
@ -1253,66 +1177,84 @@ void bed_max_temp_error(void) {
} }
#ifdef HEATER_0_USES_MAX6675 #ifdef HEATER_0_USES_MAX6675
#define MAX6675_HEAT_INTERVAL 250 #define MAX6675_HEAT_INTERVAL 250
long max6675_previous_millis = MAX6675_HEAT_INTERVAL; long max6675_previous_millis = MAX6675_HEAT_INTERVAL;
int max6675_temp = 2000; int max6675_temp = 2000;
static int read_max6675() static int read_max6675() {
{
if (millis() - max6675_previous_millis < MAX6675_HEAT_INTERVAL) unsigned long ms = millis();
return max6675_temp; if (ms < max6675_previous_millis + MAX6675_HEAT_INTERVAL)
return max6675_temp;
max6675_previous_millis = millis();
max6675_temp = 0;
#ifdef PRR max6675_previous_millis = ms;
PRR &= ~(1<<PRSPI); max6675_temp = 0;
#elif defined(PRR0)
PRR0 &= ~(1<<PRSPI);
#endif
SPCR = (1<<MSTR) | (1<<SPE) | (1<<SPR0);
// enable TT_MAX6675
WRITE(MAX6675_SS, 0);
// ensure 100ns delay - a bit extra is fine
asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
// read MSB
SPDR = 0;
for (;(SPSR & (1<<SPIF)) == 0;);
max6675_temp = SPDR;
max6675_temp <<= 8;
// read LSB
SPDR = 0;
for (;(SPSR & (1<<SPIF)) == 0;);
max6675_temp |= SPDR;
// disable TT_MAX6675
WRITE(MAX6675_SS, 1);
if (max6675_temp & 4) #ifdef PRR
{ PRR &= ~(1<<PRSPI);
// thermocouple open #elif defined(PRR0)
max6675_temp = 4000; PRR0 &= ~(1<<PRSPI);
} #endif
else
{
max6675_temp = max6675_temp >> 3;
}
return max6675_temp; SPCR = (1<<MSTR) | (1<<SPE) | (1<<SPR0);
}
// enable TT_MAX6675
WRITE(MAX6675_SS, 0);
// ensure 100ns delay - a bit extra is fine
asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
// read MSB
SPDR = 0;
for (;(SPSR & (1<<SPIF)) == 0;);
max6675_temp = SPDR;
max6675_temp <<= 8;
// read LSB
SPDR = 0;
for (;(SPSR & (1<<SPIF)) == 0;);
max6675_temp |= SPDR;
// disable TT_MAX6675
WRITE(MAX6675_SS, 1);
if (max6675_temp & 4) {
// thermocouple open
max6675_temp = 4000;
}
else {
max6675_temp = max6675_temp >> 3;
}
return max6675_temp;
}
#endif //HEATER_0_USES_MAX6675 #endif //HEATER_0_USES_MAX6675
/**
* Stages in the ISR loop
*/
enum TempState {
PrepareTemp_0,
MeasureTemp_0,
PrepareTemp_BED,
MeasureTemp_BED,
PrepareTemp_1,
MeasureTemp_1,
PrepareTemp_2,
MeasureTemp_2,
PrepareTemp_3,
MeasureTemp_3,
Prepare_FILWIDTH,
Measure_FILWIDTH,
StartupDelay // Startup, delay initial temp reading a tiny bit so the hardware can settle
};
//
// Timer 0 is shared with millies // Timer 0 is shared with millies
ISR(TIMER0_COMPB_vect) //
{ ISR(TIMER0_COMPB_vect) {
//these variables are only accesible from the ISR, but static, so they don't lose their value //these variables are only accesible from the ISR, but static, so they don't lose their value
static unsigned char temp_count = 0; static unsigned char temp_count = 0;
static unsigned long raw_temp_0_value = 0; static unsigned long raw_temp_0_value = 0;
@ -1320,542 +1262,324 @@ ISR(TIMER0_COMPB_vect)
static unsigned long raw_temp_2_value = 0; static unsigned long raw_temp_2_value = 0;
static unsigned long raw_temp_3_value = 0; static unsigned long raw_temp_3_value = 0;
static unsigned long raw_temp_bed_value = 0; static unsigned long raw_temp_bed_value = 0;
static unsigned char temp_state = 12; static TempState temp_state = StartupDelay;
static unsigned char pwm_count = (1 << SOFT_PWM_SCALE); static unsigned char pwm_count = (1 << SOFT_PWM_SCALE);
static unsigned char soft_pwm_0;
#ifdef SLOW_PWM_HEATERS
static unsigned char slow_pwm_count = 0;
static unsigned char state_heater_0 = 0;
static unsigned char state_timer_heater_0 = 0;
#endif
#if (EXTRUDERS > 1) || defined(HEATERS_PARALLEL) // Static members for each heater
static unsigned char soft_pwm_1; #ifdef SLOW_PWM_HEATERS
#ifdef SLOW_PWM_HEATERS static unsigned char slow_pwm_count = 0;
static unsigned char state_heater_1 = 0; #define ISR_STATICS(n) \
static unsigned char state_timer_heater_1 = 0; static unsigned char soft_pwm_ ## n; \
#endif static unsigned char state_heater_ ## n = 0; \
#endif static unsigned char state_timer_heater_ ## n = 0
#if EXTRUDERS > 2 #else
static unsigned char soft_pwm_2; #define ISR_STATICS(n) static unsigned char soft_pwm_ ## n
#ifdef SLOW_PWM_HEATERS #endif
static unsigned char state_heater_2 = 0;
static unsigned char state_timer_heater_2 = 0;
#endif
#endif
#if EXTRUDERS > 3
static unsigned char soft_pwm_3;
#ifdef SLOW_PWM_HEATERS
static unsigned char state_heater_3 = 0;
static unsigned char state_timer_heater_3 = 0;
#endif
#endif
#if HEATER_BED_PIN > -1 // Statics per heater
static unsigned char soft_pwm_b; ISR_STATICS(0);
#ifdef SLOW_PWM_HEATERS #if (EXTRUDERS > 1) || defined(HEATERS_PARALLEL)
static unsigned char state_heater_b = 0; ISR_STATICS(1);
static unsigned char state_timer_heater_b = 0; #if EXTRUDERS > 2
#endif ISR_STATICS(2);
#endif #if EXTRUDERS > 3
ISR_STATICS(3);
#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1) #endif
static unsigned long raw_filwidth_value = 0; //added for filament width sensor #endif
#endif #endif
#if HAS_HEATER_BED
#ifndef SLOW_PWM_HEATERS ISR_STATICS(BED);
/* #endif
* standard PWM modulation
*/
if(pwm_count == 0){
soft_pwm_0 = soft_pwm[0];
if(soft_pwm_0 > 0) {
WRITE(HEATER_0_PIN,1);
#ifdef HEATERS_PARALLEL
WRITE(HEATER_1_PIN,1);
#endif
} else WRITE(HEATER_0_PIN,0);
#if EXTRUDERS > 1 #if HAS_FILAMENT_SENSOR
soft_pwm_1 = soft_pwm[1]; static unsigned long raw_filwidth_value = 0;
if(soft_pwm_1 > 0) WRITE(HEATER_1_PIN,1); else WRITE(HEATER_1_PIN,0); #endif
#endif
#if EXTRUDERS > 2 #ifndef SLOW_PWM_HEATERS
soft_pwm_2 = soft_pwm[2]; /**
if(soft_pwm_2 > 0) WRITE(HEATER_2_PIN,1); else WRITE(HEATER_2_PIN,0); * standard PWM modulation
#endif */
#if EXTRUDERS > 3 if (pwm_count == 0) {
soft_pwm_3 = soft_pwm[3]; soft_pwm_0 = soft_pwm[0];
if(soft_pwm_3 > 0) WRITE(HEATER_3_PIN,1); else WRITE(HEATER_3_PIN,0); if (soft_pwm_0 > 0) {
#endif WRITE_HEATER_0(1);
}
else WRITE_HEATER_0P(0); // If HEATERS_PARALLEL should apply, change to WRITE_HEATER_0
#if EXTRUDERS > 1
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1 soft_pwm_1 = soft_pwm[1];
soft_pwm_b = soft_pwm_bed; WRITE_HEATER_1(soft_pwm_1 > 0 ? 1 : 0);
if(soft_pwm_b > 0) WRITE(HEATER_BED_PIN,1); else WRITE(HEATER_BED_PIN,0); #if EXTRUDERS > 2
#endif soft_pwm_2 = soft_pwm[2];
#ifdef FAN_SOFT_PWM WRITE_HEATER_2(soft_pwm_2 > 0 ? 1 : 0);
soft_pwm_fan = fanSpeedSoftPwm / 2; #if EXTRUDERS > 3
if(soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0); soft_pwm_3 = soft_pwm[3];
#endif WRITE_HEATER_3(soft_pwm_3 > 0 ? 1 : 0);
} #endif
if(soft_pwm_0 < pwm_count) {
WRITE(HEATER_0_PIN,0);
#ifdef HEATERS_PARALLEL
WRITE(HEATER_1_PIN,0);
#endif
}
#if EXTRUDERS > 1
if(soft_pwm_1 < pwm_count) WRITE(HEATER_1_PIN,0);
#endif
#if EXTRUDERS > 2
if(soft_pwm_2 < pwm_count) WRITE(HEATER_2_PIN,0);
#endif
#if EXTRUDERS > 3
if(soft_pwm_3 < pwm_count) WRITE(HEATER_3_PIN,0);
#endif
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
if(soft_pwm_b < pwm_count) WRITE(HEATER_BED_PIN,0);
#endif
#ifdef FAN_SOFT_PWM
if(soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
#endif
pwm_count += (1 << SOFT_PWM_SCALE);
pwm_count &= 0x7f;
#else //ifndef SLOW_PWM_HEATERS
/*
* SLOW PWM HEATERS
*
* for heaters drived by relay
*/
#ifndef MIN_STATE_TIME
#define MIN_STATE_TIME 16 // MIN_STATE_TIME * 65.5 = time in milliseconds
#endif
if (slow_pwm_count == 0) {
// EXTRUDER 0
soft_pwm_0 = soft_pwm[0];
if (soft_pwm_0 > 0) {
// turn ON heather only if the minimum time is up
if (state_timer_heater_0 == 0) {
// if change state set timer
if (state_heater_0 == 0) {
state_timer_heater_0 = MIN_STATE_TIME;
}
state_heater_0 = 1;
WRITE(HEATER_0_PIN, 1);
#ifdef HEATERS_PARALLEL
WRITE(HEATER_1_PIN, 1);
#endif
}
} else {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_0 == 0) {
// if change state set timer
if (state_heater_0 == 1) {
state_timer_heater_0 = MIN_STATE_TIME;
}
state_heater_0 = 0;
WRITE(HEATER_0_PIN, 0);
#ifdef HEATERS_PARALLEL
WRITE(HEATER_1_PIN, 0);
#endif
}
}
#if EXTRUDERS > 1
// EXTRUDER 1
soft_pwm_1 = soft_pwm[1];
if (soft_pwm_1 > 0) {
// turn ON heather only if the minimum time is up
if (state_timer_heater_1 == 0) {
// if change state set timer
if (state_heater_1 == 0) {
state_timer_heater_1 = MIN_STATE_TIME;
}
state_heater_1 = 1;
WRITE(HEATER_1_PIN, 1);
}
} else {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_1 == 0) {
// if change state set timer
if (state_heater_1 == 1) {
state_timer_heater_1 = MIN_STATE_TIME;
}
state_heater_1 = 0;
WRITE(HEATER_1_PIN, 0);
}
}
#endif
#if EXTRUDERS > 2
// EXTRUDER 2
soft_pwm_2 = soft_pwm[2];
if (soft_pwm_2 > 0) {
// turn ON heather only if the minimum time is up
if (state_timer_heater_2 == 0) {
// if change state set timer
if (state_heater_2 == 0) {
state_timer_heater_2 = MIN_STATE_TIME;
}
state_heater_2 = 1;
WRITE(HEATER_2_PIN, 1);
}
} else {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_2 == 0) {
// if change state set timer
if (state_heater_2 == 1) {
state_timer_heater_2 = MIN_STATE_TIME;
}
state_heater_2 = 0;
WRITE(HEATER_2_PIN, 0);
}
}
#endif
#if EXTRUDERS > 3
// EXTRUDER 3
soft_pwm_3 = soft_pwm[3];
if (soft_pwm_3 > 0) {
// turn ON heather only if the minimum time is up
if (state_timer_heater_3 == 0) {
// if change state set timer
if (state_heater_3 == 0) {
state_timer_heater_3 = MIN_STATE_TIME;
}
state_heater_3 = 1;
WRITE(HEATER_3_PIN, 1);
}
} else {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_3 == 0) {
// if change state set timer
if (state_heater_3 == 1) {
state_timer_heater_3 = MIN_STATE_TIME;
}
state_heater_3 = 0;
WRITE(HEATER_3_PIN, 0);
}
}
#endif
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
// BED
soft_pwm_b = soft_pwm_bed;
if (soft_pwm_b > 0) {
// turn ON heather only if the minimum time is up
if (state_timer_heater_b == 0) {
// if change state set timer
if (state_heater_b == 0) {
state_timer_heater_b = MIN_STATE_TIME;
}
state_heater_b = 1;
WRITE(HEATER_BED_PIN, 1);
}
} else {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_b == 0) {
// if change state set timer
if (state_heater_b == 1) {
state_timer_heater_b = MIN_STATE_TIME;
}
state_heater_b = 0;
WRITE(HEATER_BED_PIN, 0);
}
}
#endif
} // if (slow_pwm_count == 0)
// EXTRUDER 0
if (soft_pwm_0 < slow_pwm_count) {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_0 == 0) {
// if change state set timer
if (state_heater_0 == 1) {
state_timer_heater_0 = MIN_STATE_TIME;
}
state_heater_0 = 0;
WRITE(HEATER_0_PIN, 0);
#ifdef HEATERS_PARALLEL
WRITE(HEATER_1_PIN, 0);
#endif
}
}
#if EXTRUDERS > 1
// EXTRUDER 1
if (soft_pwm_1 < slow_pwm_count) {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_1 == 0) {
// if change state set timer
if (state_heater_1 == 1) {
state_timer_heater_1 = MIN_STATE_TIME;
}
state_heater_1 = 0;
WRITE(HEATER_1_PIN, 0);
}
}
#endif
#if EXTRUDERS > 2
// EXTRUDER 2
if (soft_pwm_2 < slow_pwm_count) {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_2 == 0) {
// if change state set timer
if (state_heater_2 == 1) {
state_timer_heater_2 = MIN_STATE_TIME;
}
state_heater_2 = 0;
WRITE(HEATER_2_PIN, 0);
}
}
#endif
#if EXTRUDERS > 3
// EXTRUDER 3
if (soft_pwm_3 < slow_pwm_count) {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_3 == 0) {
// if change state set timer
if (state_heater_3 == 1) {
state_timer_heater_3 = MIN_STATE_TIME;
}
state_heater_3 = 0;
WRITE(HEATER_3_PIN, 0);
}
}
#endif
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
// BED
if (soft_pwm_b < slow_pwm_count) {
// turn OFF heather only if the minimum time is up
if (state_timer_heater_b == 0) {
// if change state set timer
if (state_heater_b == 1) {
state_timer_heater_b = MIN_STATE_TIME;
}
state_heater_b = 0;
WRITE(HEATER_BED_PIN, 0);
}
}
#endif
#ifdef FAN_SOFT_PWM
if (pwm_count == 0){
soft_pwm_fan = fanSpeedSoftPwm / 2;
if (soft_pwm_fan > 0) WRITE(FAN_PIN,1); else WRITE(FAN_PIN,0);
}
if (soft_pwm_fan < pwm_count) WRITE(FAN_PIN,0);
#endif
pwm_count += (1 << SOFT_PWM_SCALE);
pwm_count &= 0x7f;
// increment slow_pwm_count only every 64 pwm_count circa 65.5ms
if ((pwm_count % 64) == 0) {
slow_pwm_count++;
slow_pwm_count &= 0x7f;
// Extruder 0
if (state_timer_heater_0 > 0) {
state_timer_heater_0--;
}
#if EXTRUDERS > 1
// Extruder 1
if (state_timer_heater_1 > 0)
state_timer_heater_1--;
#endif
#if EXTRUDERS > 2
// Extruder 2
if (state_timer_heater_2 > 0)
state_timer_heater_2--;
#endif
#if EXTRUDERS > 3
// Extruder 3
if (state_timer_heater_3 > 0)
state_timer_heater_3--;
#endif
#if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
// Bed
if (state_timer_heater_b > 0)
state_timer_heater_b--;
#endif
} //if ((pwm_count % 64) == 0) {
#endif //ifndef SLOW_PWM_HEATERS
switch(temp_state) {
case 0: // Prepare TEMP_0
#if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
#if TEMP_0_PIN > 7
ADCSRB = 1<<MUX5;
#else
ADCSRB = 0;
#endif #endif
ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07)); #endif
ADCSRA |= 1<<ADSC; // Start conversion
#if HAS_HEATER_BED
soft_pwm_BED = soft_pwm_bed;
WRITE_HEATER_BED(soft_pwm_BED > 0 ? 1 : 0);
#endif
#ifdef FAN_SOFT_PWM
soft_pwm_fan = fanSpeedSoftPwm / 2;
WRITE_FAN(soft_pwm_fan > 0 ? 1 : 0);
#endif
}
if (soft_pwm_0 < pwm_count) { WRITE_HEATER_0(0); }
#if EXTRUDERS > 1
if (soft_pwm_1 < pwm_count) WRITE_HEATER_1(0);
#if EXTRUDERS > 2
if (soft_pwm_2 < pwm_count) WRITE_HEATER_2(0);
#if EXTRUDERS > 3
if (soft_pwm_3 < pwm_count) WRITE_HEATER_3(0);
#endif
#endif
#endif
#if HAS_HEATER_BED
if (soft_pwm_BED < pwm_count) WRITE_HEATER_BED(0);
#endif
#ifdef FAN_SOFT_PWM
if (soft_pwm_fan < pwm_count) WRITE_FAN(0);
#endif
pwm_count += (1 << SOFT_PWM_SCALE);
pwm_count &= 0x7f;
#else // SLOW_PWM_HEATERS
/*
* SLOW PWM HEATERS
*
* for heaters drived by relay
*/
#ifndef MIN_STATE_TIME
#define MIN_STATE_TIME 16 // MIN_STATE_TIME * 65.5 = time in milliseconds
#endif
// Macros for Slow PWM timer logic - HEATERS_PARALLEL applies
#define _SLOW_PWM_ROUTINE(NR, src) \
soft_pwm_ ## NR = src; \
if (soft_pwm_ ## NR > 0) { \
if (state_timer_heater_ ## NR == 0) { \
if (state_heater_ ## NR == 0) state_timer_heater_ ## NR = MIN_STATE_TIME; \
state_heater_ ## NR = 1; \
WRITE_HEATER_ ## NR(1); \
} \
} \
else { \
if (state_timer_heater_ ## NR == 0) { \
if (state_heater_ ## NR == 1) state_timer_heater_ ## NR = MIN_STATE_TIME; \
state_heater_ ## NR = 0; \
WRITE_HEATER_ ## NR(0); \
} \
}
#define SLOW_PWM_ROUTINE(n) _SLOW_PWM_ROUTINE(n, soft_pwm[n])
#define PWM_OFF_ROUTINE(NR) \
if (soft_pwm_ ## NR < slow_pwm_count) { \
if (state_timer_heater_ ## NR == 0) { \
if (state_heater_ ## NR == 1) state_timer_heater_ ## NR = MIN_STATE_TIME; \
state_heater_ ## NR = 0; \
WRITE_HEATER_ ## NR (0); \
} \
}
if (slow_pwm_count == 0) {
SLOW_PWM_ROUTINE(0); // EXTRUDER 0
#if EXTRUDERS > 1
SLOW_PWM_ROUTINE(1); // EXTRUDER 1
#if EXTRUDERS > 2
SLOW_PWM_ROUTINE(2); // EXTRUDER 2
#if EXTRUDERS > 3
SLOW_PWM_ROUTINE(3); // EXTRUDER 3
#endif
#endif
#endif
#if HAS_HEATER_BED
_SLOW_PWM_ROUTINE(BED, soft_pwm_bed); // BED
#endif
} // slow_pwm_count == 0
PWM_OFF_ROUTINE(0); // EXTRUDER 0
#if EXTRUDERS > 1
PWM_OFF_ROUTINE(1); // EXTRUDER 1
#if EXTRUDERS > 2
PWM_OFF_ROUTINE(2); // EXTRUDER 2
#if EXTRUDERS > 3
PWM_OFF_ROUTINE(3); // EXTRUDER 3
#endif
#endif
#endif
#if HAS_HEATER_BED
PWM_OFF_ROUTINE(BED); // BED
#endif
#ifdef FAN_SOFT_PWM
if (pwm_count == 0) {
soft_pwm_fan = fanSpeedSoftPwm / 2;
WRITE_FAN(soft_pwm_fan > 0 ? 1 : 0);
}
if (soft_pwm_fan < pwm_count) WRITE_FAN(0);
#endif //FAN_SOFT_PWM
pwm_count += (1 << SOFT_PWM_SCALE);
pwm_count &= 0x7f;
// increment slow_pwm_count only every 64 pwm_count circa 65.5ms
if ((pwm_count % 64) == 0) {
slow_pwm_count++;
slow_pwm_count &= 0x7f;
// EXTRUDER 0
if (state_timer_heater_0 > 0) state_timer_heater_0--;
#if EXTRUDERS > 1 // EXTRUDER 1
if (state_timer_heater_1 > 0) state_timer_heater_1--;
#if EXTRUDERS > 2 // EXTRUDER 2
if (state_timer_heater_2 > 0) state_timer_heater_2--;
#if EXTRUDERS > 3 // EXTRUDER 3
if (state_timer_heater_3 > 0) state_timer_heater_3--;
#endif
#endif
#endif
#if HAS_HEATER_BED
if (state_timer_heater_BED > 0) state_timer_heater_BED--;
#endif
} // (pwm_count % 64) == 0
#endif // SLOW_PWM_HEATERS
#define SET_ADMUX_ADCSRA(pin) ADMUX = (1 << REFS0) | (pin & 0x07); ADCSRA |= 1<<ADSC
#ifdef MUX5
#define START_ADC(pin) if (pin > 7) ADCSRB = 1 << MUX5; else ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
#else
#define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
#endif
switch(temp_state) {
case PrepareTemp_0:
#if HAS_TEMP_0
START_ADC(TEMP_0_PIN);
#endif #endif
lcd_buttons_update(); lcd_buttons_update();
temp_state = 1; temp_state = MeasureTemp_0;
break; break;
case 1: // Measure TEMP_0 case MeasureTemp_0:
#if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1) #if HAS_TEMP_0
raw_temp_0_value += ADC; raw_temp_0_value += ADC;
#endif #endif
temp_state = 2; temp_state = PrepareTemp_BED;
break; break;
case 2: // Prepare TEMP_BED case PrepareTemp_BED:
#if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1) #if HAS_TEMP_BED
#if TEMP_BED_PIN > 7 START_ADC(TEMP_BED_PIN);
ADCSRB = 1<<MUX5;
#else
ADCSRB = 0;
#endif
ADMUX = ((1 << REFS0) | (TEMP_BED_PIN & 0x07));
ADCSRA |= 1<<ADSC; // Start conversion
#endif #endif
lcd_buttons_update(); lcd_buttons_update();
temp_state = 3; temp_state = MeasureTemp_BED;
break; break;
case 3: // Measure TEMP_BED case MeasureTemp_BED:
#if defined(TEMP_BED_PIN) && (TEMP_BED_PIN > -1) #if HAS_TEMP_BED
raw_temp_bed_value += ADC; raw_temp_bed_value += ADC;
#endif #endif
temp_state = 4; temp_state = PrepareTemp_1;
break; break;
case 4: // Prepare TEMP_1 case PrepareTemp_1:
#if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1) #if HAS_TEMP_1
#if TEMP_1_PIN > 7 START_ADC(TEMP_1_PIN);
ADCSRB = 1<<MUX5;
#else
ADCSRB = 0;
#endif
ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
ADCSRA |= 1<<ADSC; // Start conversion
#endif #endif
lcd_buttons_update(); lcd_buttons_update();
temp_state = 5; temp_state = MeasureTemp_1;
break; break;
case 5: // Measure TEMP_1 case MeasureTemp_1:
#if defined(TEMP_1_PIN) && (TEMP_1_PIN > -1) #if HAS_TEMP_1
raw_temp_1_value += ADC; raw_temp_1_value += ADC;
#endif #endif
temp_state = 6; temp_state = PrepareTemp_2;
break; break;
case 6: // Prepare TEMP_2 case PrepareTemp_2:
#if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1) #if HAS_TEMP_2
#if TEMP_2_PIN > 7 START_ADC(TEMP_2_PIN);
ADCSRB = 1<<MUX5;
#else
ADCSRB = 0;
#endif
ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
ADCSRA |= 1<<ADSC; // Start conversion
#endif #endif
lcd_buttons_update(); lcd_buttons_update();
temp_state = 7; temp_state = MeasureTemp_2;
break; break;
case 7: // Measure TEMP_2 case MeasureTemp_2:
#if defined(TEMP_2_PIN) && (TEMP_2_PIN > -1) #if HAS_TEMP_2
raw_temp_2_value += ADC; raw_temp_2_value += ADC;
#endif #endif
temp_state = 8; temp_state = PrepareTemp_3;
break; break;
case 8: // Prepare TEMP_3 case PrepareTemp_3:
#if defined(TEMP_3_PIN) && (TEMP_3_PIN > -1) #if HAS_TEMP_3
#if TEMP_3_PIN > 7 START_ADC(TEMP_3_PIN);
ADCSRB = 1<<MUX5;
#else
ADCSRB = 0;
#endif
ADMUX = ((1 << REFS0) | (TEMP_3_PIN & 0x07));
ADCSRA |= 1<<ADSC; // Start conversion
#endif #endif
lcd_buttons_update(); lcd_buttons_update();
temp_state = 9; temp_state = MeasureTemp_3;
break; break;
case 9: // Measure TEMP_3 case MeasureTemp_3:
#if defined(TEMP_3_PIN) && (TEMP_3_PIN > -1) #if HAS_TEMP_3
raw_temp_3_value += ADC; raw_temp_3_value += ADC;
#endif #endif
temp_state = 10; //change so that Filament Width is also measured temp_state = Prepare_FILWIDTH;
break; break;
case 10: //Prepare FILWIDTH case Prepare_FILWIDTH:
#if defined(FILWIDTH_PIN) && (FILWIDTH_PIN> -1) #if HAS_FILAMENT_SENSOR
#if FILWIDTH_PIN>7 START_ADC(FILWIDTH_PIN);
ADCSRB = 1<<MUX5; #endif
#else lcd_buttons_update();
ADCSRB = 0; temp_state = Measure_FILWIDTH;
#endif break;
ADMUX = ((1 << REFS0) | (FILWIDTH_PIN & 0x07)); case Measure_FILWIDTH:
ADCSRA |= 1<<ADSC; // Start conversion #if HAS_FILAMENT_SENSOR
#endif // raw_filwidth_value += ADC; //remove to use an IIR filter approach
lcd_buttons_update(); if (ADC > 102) { //check that ADC is reading a voltage > 0.5 volts, otherwise don't take in the data.
temp_state = 11; raw_filwidth_value -= (raw_filwidth_value>>7); //multiply raw_filwidth_value by 127/128
break; raw_filwidth_value += ((unsigned long)ADC<<7); //add new ADC reading
case 11: //Measure FILWIDTH
#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1)
//raw_filwidth_value += ADC; //remove to use an IIR filter approach
if(ADC>102) //check that ADC is reading a voltage > 0.5 volts, otherwise don't take in the data.
{
raw_filwidth_value= raw_filwidth_value-(raw_filwidth_value>>7); //multipliy raw_filwidth_value by 127/128
raw_filwidth_value= raw_filwidth_value + ((unsigned long)ADC<<7); //add new ADC reading
} }
#endif #endif
temp_state = 0; temp_state = PrepareTemp_0;
temp_count++;
temp_count++; break;
break; case StartupDelay:
temp_state = PrepareTemp_0;
case 12: //Startup, delay initial temp reading a tiny bit so the hardware can settle.
temp_state = 0;
break; break;
// default:
// SERIAL_ERROR_START;
// SERIAL_ERRORLNPGM("Temp measurement error!");
// break;
}
if(temp_count >= OVERSAMPLENR) // 10 * 16 * 1/(16000000/64/256) = 164ms.
{
if (!temp_meas_ready) //Only update the raw values if they have been read. Else we could be updating them during reading.
{
#ifndef HEATER_0_USES_MAX6675
current_temperature_raw[0] = raw_temp_0_value;
#endif
#if EXTRUDERS > 1
current_temperature_raw[1] = raw_temp_1_value;
#endif
#ifdef TEMP_SENSOR_1_AS_REDUNDANT
redundant_temperature_raw = raw_temp_1_value;
#endif
#if EXTRUDERS > 2
current_temperature_raw[2] = raw_temp_2_value;
#endif
#if EXTRUDERS > 3
current_temperature_raw[3] = raw_temp_3_value;
#endif
current_temperature_bed_raw = raw_temp_bed_value;
}
//Add similar code for Filament Sensor - can be read any time since IIR filtering is used // default:
#if defined(FILWIDTH_PIN) &&(FILWIDTH_PIN > -1) // SERIAL_ERROR_START;
current_raw_filwidth = raw_filwidth_value>>10; //need to divide to get to 0-16384 range since we used 1/128 IIR filter approach // SERIAL_ERRORLNPGM("Temp measurement error!");
#endif // break;
} // switch(temp_state)
if (temp_count >= OVERSAMPLENR) { // 10 * 16 * 1/(16000000/64/256) = 164ms.
if (!temp_meas_ready) { //Only update the raw values if they have been read. Else we could be updating them during reading.
#ifndef HEATER_0_USES_MAX6675
current_temperature_raw[0] = raw_temp_0_value;
#endif
#if EXTRUDERS > 1
current_temperature_raw[1] = raw_temp_1_value;
#if EXTRUDERS > 2
current_temperature_raw[2] = raw_temp_2_value;
#if EXTRUDERS > 3
current_temperature_raw[3] = raw_temp_3_value;
#endif
#endif
#endif
#ifdef TEMP_SENSOR_1_AS_REDUNDANT
redundant_temperature_raw = raw_temp_1_value;
#endif
current_temperature_bed_raw = raw_temp_bed_value;
} //!temp_meas_ready
// Filament Sensor - can be read any time since IIR filtering is used
#if HAS_FILAMENT_SENSOR
current_raw_filwidth = raw_filwidth_value >> 10; // Divide to get to 0-16384 range since we used 1/128 IIR filter approach
#endif
temp_meas_ready = true; temp_meas_ready = true;
temp_count = 0; temp_count = 0;
@ -1865,131 +1589,47 @@ ISR(TIMER0_COMPB_vect)
raw_temp_3_value = 0; raw_temp_3_value = 0;
raw_temp_bed_value = 0; raw_temp_bed_value = 0;
#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP #if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
if(current_temperature_raw[0] <= maxttemp_raw[0]) { #define MAXTEST <=
#else #define MINTEST >=
if(current_temperature_raw[0] >= maxttemp_raw[0]) { #else
#endif #define MAXTEST >=
#ifndef HEATER_0_USES_MAX6675 #define MINTEST <=
max_temp_error(0); #endif
#endif
}
#if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
if(current_temperature_raw[0] >= minttemp_raw[0]) {
#else
if(current_temperature_raw[0] <= minttemp_raw[0]) {
#endif
#ifndef HEATER_0_USES_MAX6675
min_temp_error(0);
#endif
}
for (int i=0; i<EXTRUDERS; i++) {
if (current_temperature_raw[i] MAXTEST maxttemp_raw[i]) max_temp_error(i);
else if (current_temperature_raw[i] MINTEST minttemp_raw[i]) min_temp_error(i);
}
/* No bed MINTEMP error? */
#if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
if (current_temperature_bed_raw MAXTEST bed_maxttemp_raw) {
target_temperature_bed = 0;
bed_max_temp_error();
}
#endif
} // temp_count >= OVERSAMPLENR
#if EXTRUDERS > 1 #ifdef BABYSTEPPING
#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP for (uint8_t axis=X_AXIS; axis<=Z_AXIS; axis++) {
if(current_temperature_raw[1] <= maxttemp_raw[1]) { int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
#else
if(current_temperature_raw[1] >= maxttemp_raw[1]) { if (curTodo > 0) {
#endif babystep(axis,/*fwd*/true);
max_temp_error(1); babystepsTodo[axis]--; //less to do next time
}
else if(curTodo < 0) {
babystep(axis,/*fwd*/false);
babystepsTodo[axis]++; //less to do next time
}
} }
#if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP #endif //BABYSTEPPING
if(current_temperature_raw[1] >= minttemp_raw[1]) {
#else
if(current_temperature_raw[1] <= minttemp_raw[1]) {
#endif
min_temp_error(1);
}
#endif
#if EXTRUDERS > 2
#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
if(current_temperature_raw[2] <= maxttemp_raw[2]) {
#else
if(current_temperature_raw[2] >= maxttemp_raw[2]) {
#endif
max_temp_error(2);
}
#if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
if(current_temperature_raw[2] >= minttemp_raw[2]) {
#else
if(current_temperature_raw[2] <= minttemp_raw[2]) {
#endif
min_temp_error(2);
}
#endif
#if EXTRUDERS > 3
#if HEATER_3_RAW_LO_TEMP > HEATER_3_RAW_HI_TEMP
if(current_temperature_raw[3] <= maxttemp_raw[3]) {
#else
if(current_temperature_raw[3] >= maxttemp_raw[3]) {
#endif
max_temp_error(3);
}
#if HEATER_3_RAW_LO_TEMP > HEATER_3_RAW_HI_TEMP
if(current_temperature_raw[3] >= minttemp_raw[3]) {
#else
if(current_temperature_raw[3] <= minttemp_raw[3]) {
#endif
min_temp_error(3);
}
#endif
/* No bed MINTEMP error? */
#if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
# if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
if(current_temperature_bed_raw <= bed_maxttemp_raw) {
#else
if(current_temperature_bed_raw >= bed_maxttemp_raw) {
#endif
target_temperature_bed = 0;
bed_max_temp_error();
}
#endif
}
#ifdef BABYSTEPPING
for(uint8_t axis=0;axis<3;axis++)
{
int curTodo=babystepsTodo[axis]; //get rid of volatile for performance
if(curTodo>0)
{
babystep(axis,/*fwd*/true);
babystepsTodo[axis]--; //less to do next time
}
else
if(curTodo<0)
{
babystep(axis,/*fwd*/false);
babystepsTodo[axis]++; //less to do next time
}
}
#endif //BABYSTEPPING
} }
#ifdef PIDTEMP #ifdef PIDTEMP
// Apply the scale factors to the PID values // Apply the scale factors to the PID values
float scalePID_i(float i) { return i * PID_dT; }
float unscalePID_i(float i) { return i / PID_dT; }
float scalePID_i(float i) float scalePID_d(float d) { return d / PID_dT; }
{ float unscalePID_d(float d) { return d * PID_dT; }
return i*PID_dT;
}
float unscalePID_i(float i)
{
return i/PID_dT;
}
float scalePID_d(float d)
{
return d/PID_dT;
}
float unscalePID_d(float d)
{
return d*PID_dT;
}
#endif //PIDTEMP #endif //PIDTEMP

View file

@ -85,55 +85,25 @@ extern float current_temperature_bed;
//inline so that there is no performance decrease. //inline so that there is no performance decrease.
//deg=degreeCelsius //deg=degreeCelsius
FORCE_INLINE float degHotend(uint8_t extruder) { FORCE_INLINE float degHotend(uint8_t extruder) { return current_temperature[extruder]; }
return current_temperature[extruder]; FORCE_INLINE float degBed() { return current_temperature_bed; }
};
#ifdef SHOW_TEMP_ADC_VALUES #ifdef SHOW_TEMP_ADC_VALUES
FORCE_INLINE float rawHotendTemp(uint8_t extruder) { FORCE_INLINE float rawHotendTemp(uint8_t extruder) { return current_temperature_raw[extruder]; }
return current_temperature_raw[extruder]; FORCE_INLINE float rawBedTemp() { return current_temperature_bed_raw; }
};
FORCE_INLINE float rawBedTemp() {
return current_temperature_bed_raw;
};
#endif #endif
FORCE_INLINE float degBed() { FORCE_INLINE float degTargetHotend(uint8_t extruder) { return target_temperature[extruder]; }
return current_temperature_bed; FORCE_INLINE float degTargetBed() { return target_temperature_bed; }
};
FORCE_INLINE float degTargetHotend(uint8_t extruder) { FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) { target_temperature[extruder] = celsius; }
return target_temperature[extruder]; FORCE_INLINE void setTargetBed(const float &celsius) { target_temperature_bed = celsius; }
};
FORCE_INLINE float degTargetBed() { FORCE_INLINE bool isHeatingHotend(uint8_t extruder) { return target_temperature[extruder] > current_temperature[extruder]; }
return target_temperature_bed; FORCE_INLINE bool isHeatingBed() { return target_temperature_bed > current_temperature_bed; }
};
FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) { FORCE_INLINE bool isCoolingHotend(uint8_t extruder) { return target_temperature[extruder] < current_temperature[extruder]; }
target_temperature[extruder] = celsius; FORCE_INLINE bool isCoolingBed() { return target_temperature_bed < current_temperature_bed; }
};
FORCE_INLINE void setTargetBed(const float &celsius) {
target_temperature_bed = celsius;
};
FORCE_INLINE bool isHeatingHotend(uint8_t extruder){
return target_temperature[extruder] > current_temperature[extruder];
};
FORCE_INLINE bool isHeatingBed() {
return target_temperature_bed > current_temperature_bed;
};
FORCE_INLINE bool isCoolingHotend(uint8_t extruder) {
return target_temperature[extruder] < current_temperature[extruder];
};
FORCE_INLINE bool isCoolingBed() {
return target_temperature_bed < current_temperature_bed;
};
#define degHotend0() degHotend(0) #define degHotend0() degHotend(0)
#define degTargetHotend0() degTargetHotend(0) #define degTargetHotend0() degTargetHotend(0)
@ -141,38 +111,36 @@ FORCE_INLINE bool isCoolingBed() {
#define isHeatingHotend0() isHeatingHotend(0) #define isHeatingHotend0() isHeatingHotend(0)
#define isCoolingHotend0() isCoolingHotend(0) #define isCoolingHotend0() isCoolingHotend(0)
#if EXTRUDERS > 1 #if EXTRUDERS > 1
#define degHotend1() degHotend(1) #define degHotend1() degHotend(1)
#define degTargetHotend1() degTargetHotend(1) #define degTargetHotend1() degTargetHotend(1)
#define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1) #define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1)
#define isHeatingHotend1() isHeatingHotend(1) #define isHeatingHotend1() isHeatingHotend(1)
#define isCoolingHotend1() isCoolingHotend(1) #define isCoolingHotend1() isCoolingHotend(1)
#else #else
#define setTargetHotend1(_celsius) do{}while(0) #define setTargetHotend1(_celsius) do{}while(0)
#endif #endif
#if EXTRUDERS > 2 #if EXTRUDERS > 2
#define degHotend2() degHotend(2) #define degHotend2() degHotend(2)
#define degTargetHotend2() degTargetHotend(2) #define degTargetHotend2() degTargetHotend(2)
#define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2) #define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2)
#define isHeatingHotend2() isHeatingHotend(2) #define isHeatingHotend2() isHeatingHotend(2)
#define isCoolingHotend2() isCoolingHotend(2) #define isCoolingHotend2() isCoolingHotend(2)
#else #else
#define setTargetHotend2(_celsius) do{}while(0) #define setTargetHotend2(_celsius) do{}while(0)
#endif #endif
#if EXTRUDERS > 3 #if EXTRUDERS > 3
#define degHotend3() degHotend(3) #define degHotend3() degHotend(3)
#define degTargetHotend3() degTargetHotend(3) #define degTargetHotend3() degTargetHotend(3)
#define setTargetHotend3(_celsius) setTargetHotend((_celsius), 3) #define setTargetHotend3(_celsius) setTargetHotend((_celsius), 3)
#define isHeatingHotend3() isHeatingHotend(3) #define isHeatingHotend3() isHeatingHotend(3)
#define isCoolingHotend3() isCoolingHotend(3) #define isCoolingHotend3() isCoolingHotend(3)
#else #else
#define setTargetHotend3(_celsius) do{}while(0) #define setTargetHotend3(_celsius) do{}while(0)
#endif #endif
#if EXTRUDERS > 4 #if EXTRUDERS > 4
#error Invalid number of extruders #error Invalid number of extruders
#endif #endif
int getHeaterPower(int heater); int getHeaterPower(int heater);
void disable_heater(); void disable_heater();
void setWatch(); void setWatch();
@ -189,15 +157,14 @@ static bool thermal_runaway = false;
#endif #endif
#endif #endif
FORCE_INLINE void autotempShutdown(){ FORCE_INLINE void autotempShutdown() {
#ifdef AUTOTEMP #ifdef AUTOTEMP
if(autotemp_enabled) if (autotemp_enabled) {
{ autotemp_enabled = false;
autotemp_enabled=false; if (degTargetHotend(active_extruder) > autotemp_min)
if(degTargetHotend(active_extruder)>autotemp_min) setTargetHotend(0, active_extruder);
setTargetHotend(0,active_extruder); }
} #endif
#endif
} }
void PID_autotune(float temp, int extruder, int ncycles); void PID_autotune(float temp, int extruder, int ncycles);

View file

@ -1095,6 +1095,29 @@ const short temptable_1047[][2] PROGMEM = {
}; };
#endif #endif
#if (THERMISTORHEATER_0 == 999) || (THERMISTORHEATER_1 == 999) || (THERMISTORHEATER_2 == 999) || (THERMISTORHEATER_3 == 999) || (THERMISTORBED == 999) //User defined table
// Dummy Thermistor table.. It will ALWAYS read a fixed value.
#ifndef DUMMY_THERMISTOR_999_VALUE
#define DUMMY_THERMISTOR_999_VALUE 25
#endif
const short temptable_999[][2] PROGMEM = {
{1*OVERSAMPLENR, DUMMY_THERMISTOR_999_VALUE},
{1023*OVERSAMPLENR, DUMMY_THERMISTOR_999_VALUE}
};
#endif
#if (THERMISTORHEATER_0 == 998) || (THERMISTORHEATER_1 == 998) || (THERMISTORHEATER_2 == 998) || (THERMISTORHEATER_3 == 998) || (THERMISTORBED == 998) //User defined table
// Dummy Thermistor table.. It will ALWAYS read a fixed value.
#ifndef DUMMY_THERMISTOR_998_VALUE
#define DUMMY_THERMISTOR_998_VALUE 25
#endif
const short temptable_998[][2] PROGMEM = {
{1*OVERSAMPLENR, DUMMY_THERMISTOR_998_VALUE},
{1023*OVERSAMPLENR, DUMMY_THERMISTOR_998_VALUE}
};
#endif
#define _TT_NAME(_N) temptable_ ## _N #define _TT_NAME(_N) temptable_ ## _N
#define TT_NAME(_N) _TT_NAME(_N) #define TT_NAME(_N) _TT_NAME(_N)

View file

@ -10,6 +10,9 @@
int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */ int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */
bool encoderRateMultiplierEnabled;
int32_t lastEncoderMovementMillis;
/* Configuration settings */ /* Configuration settings */
int plaPreheatHotendTemp; int plaPreheatHotendTemp;
int plaPreheatHPBTemp; int plaPreheatHPBTemp;
@ -41,11 +44,6 @@ char lcd_status_message[LCD_WIDTH+1] = WELCOME_MSG;
#include "ultralcd_implementation_hitachi_HD44780.h" #include "ultralcd_implementation_hitachi_HD44780.h"
#endif #endif
/** forward declarations **/
void copy_and_scalePID_i();
void copy_and_scalePID_d();
/* Different menus */ /* Different menus */
static void lcd_status_screen(); static void lcd_status_screen();
#ifdef ULTIPANEL #ifdef ULTIPANEL
@ -119,6 +117,7 @@ static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned l
/* Helper macros for menus */ /* Helper macros for menus */
#define START_MENU() do { \ #define START_MENU() do { \
encoderRateMultiplierEnabled = false; \
if (encoderPosition > 0x8000) encoderPosition = 0; \ if (encoderPosition > 0x8000) encoderPosition = 0; \
if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM < currentMenuViewOffset) currentMenuViewOffset = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\ if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM < currentMenuViewOffset) currentMenuViewOffset = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\
uint8_t _lineNr = currentMenuViewOffset, _menuItemNr; \ uint8_t _lineNr = currentMenuViewOffset, _menuItemNr; \
@ -143,9 +142,39 @@ static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned l
}\ }\
_menuItemNr++;\ _menuItemNr++;\
} while(0) } while(0)
#ifdef ENCODER_RATE_MULTIPLIER
#define MENU_MULTIPLIER_ITEM(type, label, args...) do { \
if (_menuItemNr == _lineNr) { \
if (lcdDrawUpdate) { \
const char* _label_pstr = PSTR(label); \
if ((encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) { \
lcd_implementation_drawmenu_ ## type ## _selected (_drawLineNr, _label_pstr , ## args ); \
} \
else { \
lcd_implementation_drawmenu_ ## type (_drawLineNr, _label_pstr , ## args ); \
} \
} \
if (wasClicked && (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) { \
lcd_quick_feedback(); \
encoderRateMultiplierEnabled = true; \
lastEncoderMovementMillis = 0; \
menu_action_ ## type ( args ); \
return; \
} \
} \
_menuItemNr++; \
} while(0)
#endif //ENCODER_RATE_MULTIPLIER
#define MENU_ITEM_DUMMY() do { _menuItemNr++; } while(0) #define MENU_ITEM_DUMMY() do { _menuItemNr++; } while(0)
#define MENU_ITEM_EDIT(type, label, args...) MENU_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args ) #define MENU_ITEM_EDIT(type, label, args...) MENU_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args )
#define MENU_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args ) #define MENU_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args )
#ifdef ENCODER_RATE_MULTIPLIER
#define MENU_MULTIPLIER_ITEM_EDIT(type, label, args...) MENU_MULTIPLIER_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args )
#define MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(type, label, args...) MENU_MULTIPLIER_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args )
#else //!ENCODER_RATE_MULTIPLIER
#define MENU_MULTIPLIER_ITEM_EDIT(type, label, args...) MENU_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args )
#define MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args )
#endif //!ENCODER_RATE_MULTIPLIER
#define END_MENU() \ #define END_MENU() \
if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; \ if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; \
if ((uint8_t)(encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \ if ((uint8_t)(encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \
@ -185,9 +214,8 @@ void* editValue;
int32_t minEditValue, maxEditValue; int32_t minEditValue, maxEditValue;
menuFunc_t callbackFunc; menuFunc_t callbackFunc;
// place-holders for Ki and Kd edits, and the extruder # being edited // place-holders for Ki and Kd edits
float raw_Ki, raw_Kd; float raw_Ki, raw_Kd;
int pid_current_extruder;
static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool feedback=true) { static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool feedback=true) {
if (currentMenu != menu) { if (currentMenu != menu) {
@ -205,6 +233,7 @@ static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool
/* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */ /* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */
static void lcd_status_screen() static void lcd_status_screen()
{ {
encoderRateMultiplierEnabled = false;
#if defined(LCD_PROGRESS_BAR) && defined(SDSUPPORT) && !defined(DOGLCD) #if defined(LCD_PROGRESS_BAR) && defined(SDSUPPORT) && !defined(DOGLCD)
uint16_t mil = millis(); uint16_t mil = millis();
#ifndef PROGRESS_MSG_ONCE #ifndef PROGRESS_MSG_ONCE
@ -233,15 +262,15 @@ static void lcd_status_screen()
#endif #endif
#endif //LCD_PROGRESS_BAR #endif //LCD_PROGRESS_BAR
if (lcd_status_update_delay) if (lcd_status_update_delay)
lcd_status_update_delay--; lcd_status_update_delay--;
else else
lcdDrawUpdate = 1; lcdDrawUpdate = 1;
if (lcdDrawUpdate) { if (lcdDrawUpdate) {
lcd_implementation_status_screen(); lcd_implementation_status_screen();
lcd_status_update_delay = 10; /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */ lcd_status_update_delay = 10; /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */
} }
#ifdef ULTIPANEL #ifdef ULTIPANEL
@ -317,86 +346,82 @@ static void lcd_sdcard_pause() { card.pauseSDPrint(); }
static void lcd_sdcard_resume() { card.startFileprint(); } static void lcd_sdcard_resume() { card.startFileprint(); }
static void lcd_sdcard_stop() static void lcd_sdcard_stop() {
{ card.sdprinting = false;
card.sdprinting = false; card.closefile();
card.closefile(); quickStop();
quickStop(); if (SD_FINISHED_STEPPERRELEASE) {
if(SD_FINISHED_STEPPERRELEASE) enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
{ }
enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND)); autotempShutdown();
}
autotempShutdown();
cancel_heatup = true; cancel_heatup = true;
lcd_setstatus(MSG_PRINT_ABORTED); lcd_setstatus(MSG_PRINT_ABORTED);
} }
/* Menu implementation */ /* Menu implementation */
static void lcd_main_menu() static void lcd_main_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_WATCH, lcd_status_screen);
MENU_ITEM(back, MSG_WATCH, lcd_status_screen); if (movesplanned() || IS_SD_PRINTING) {
if (movesplanned() || IS_SD_PRINTING) MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu);
{ }
MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu); else {
}else{ MENU_ITEM(submenu, MSG_PREPARE, lcd_prepare_menu);
MENU_ITEM(submenu, MSG_PREPARE, lcd_prepare_menu); #ifdef DELTA_CALIBRATION_MENU
#ifdef DELTA_CALIBRATION_MENU MENU_ITEM(submenu, MSG_DELTA_CALIBRATE, lcd_delta_calibrate_menu);
MENU_ITEM(submenu, MSG_DELTA_CALIBRATE, lcd_delta_calibrate_menu); #endif
#endif // DELTA_CALIBRATION_MENU }
} MENU_ITEM(submenu, MSG_CONTROL, lcd_control_menu);
MENU_ITEM(submenu, MSG_CONTROL, lcd_control_menu);
#ifdef SDSUPPORT
if (card.cardOK)
{
if (card.isFileOpen())
{
if (card.sdprinting)
MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause);
else
MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume);
MENU_ITEM(function, MSG_STOP_PRINT, lcd_sdcard_stop);
}else{
MENU_ITEM(submenu, MSG_CARD_MENU, lcd_sdcard_menu);
#if SDCARDDETECT < 1
MENU_ITEM(gcode, MSG_CNG_SDCARD, PSTR("M21")); // SD-card changed by user
#endif
}
}else{
MENU_ITEM(submenu, MSG_NO_CARD, lcd_sdcard_menu);
#if SDCARDDETECT < 1
MENU_ITEM(gcode, MSG_INIT_SDCARD, PSTR("M21")); // Manually initialize the SD-card via user interface
#endif
}
#endif
END_MENU();
}
#ifdef SDSUPPORT #ifdef SDSUPPORT
static void lcd_autostart_sd() if (card.cardOK) {
{ if (card.isFileOpen()) {
card.autostart_index=0; if (card.sdprinting)
card.setroot(); MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause);
card.checkautostart(true); else
} MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume);
#endif MENU_ITEM(function, MSG_STOP_PRINT, lcd_sdcard_stop);
}
void lcd_set_home_offsets() else {
{ MENU_ITEM(submenu, MSG_CARD_MENU, lcd_sdcard_menu);
for(int8_t i=0; i < NUM_AXIS; i++) { #if SDCARDDETECT < 1
if (i != E_AXIS) { MENU_ITEM(gcode, MSG_CNG_SDCARD, PSTR("M21")); // SD-card changed by user
add_homing[i] -= current_position[i]; #endif
current_position[i] = 0.0;
} }
} }
plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]); else {
MENU_ITEM(submenu, MSG_NO_CARD, lcd_sdcard_menu);
#if SDCARDDETECT < 1
MENU_ITEM(gcode, MSG_INIT_SDCARD, PSTR("M21")); // Manually initialize the SD-card via user interface
#endif
}
#endif //SDSUPPORT
// Audio feedback END_MENU();
enquecommand_P(PSTR("M300 S659 P200")); }
enquecommand_P(PSTR("M300 S698 P200"));
lcd_return_to_status(); #ifdef SDSUPPORT
static void lcd_autostart_sd() {
card.autostart_index = 0;
card.setroot();
card.checkautostart(true);
}
#endif
void lcd_set_home_offsets() {
for(int8_t i=0; i < NUM_AXIS; i++) {
if (i != E_AXIS) {
add_homing[i] -= current_position[i];
current_position[i] = 0.0;
}
}
plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]);
// Audio feedback
enquecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
lcd_return_to_status();
} }
@ -417,267 +442,181 @@ void lcd_set_home_offsets()
#endif //BABYSTEPPING #endif //BABYSTEPPING
static void lcd_tune_menu() static void lcd_tune_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
MENU_ITEM(back, MSG_MAIN, lcd_main_menu); MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999);
MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999); #if TEMP_SENSOR_0 != 0
#if TEMP_SENSOR_0 != 0 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); #endif
#endif #if TEMP_SENSOR_1 != 0
#if TEMP_SENSOR_1 != 0 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N2, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15);
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 2", &target_temperature[1], 0, HEATER_1_MAXTEMP - 15); #endif
#endif #if TEMP_SENSOR_2 != 0
#if TEMP_SENSOR_2 != 0 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N3, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15);
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 3", &target_temperature[2], 0, HEATER_2_MAXTEMP - 15); #endif
#endif #if TEMP_SENSOR_3 != 0
#if TEMP_SENSOR_3 != 0 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N4, &target_temperature[3], 0, HEATER_3_MAXTEMP - 15);
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 4", &target_temperature[3], 0, HEATER_3_MAXTEMP - 15); #endif
#endif #if TEMP_SENSOR_BED != 0
MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
#endif
#if TEMP_SENSOR_BED != 0 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
#endif
MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999); MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999);
MENU_ITEM_EDIT(int3, MSG_FLOW " 0", &extruder_multiply[0], 10, 999); MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F0, &extruder_multiply[0], 10, 999);
#if TEMP_SENSOR_1 != 0 #if TEMP_SENSOR_1 != 0
MENU_ITEM_EDIT(int3, MSG_FLOW " 1", &extruder_multiply[1], 10, 999); MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F1, &extruder_multiply[1], 10, 999);
#endif #endif
#if TEMP_SENSOR_2 != 0 #if TEMP_SENSOR_2 != 0
MENU_ITEM_EDIT(int3, MSG_FLOW " 2", &extruder_multiply[2], 10, 999); MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F2, &extruder_multiply[2], 10, 999);
#endif #endif
#if TEMP_SENSOR_3 != 0 #if TEMP_SENSOR_3 != 0
MENU_ITEM_EDIT(int3, MSG_FLOW " 3", &extruder_multiply[3], 10, 999); MENU_ITEM_EDIT(int3, MSG_FLOW MSG_F3, &extruder_multiply[3], 10, 999);
#endif #endif
#ifdef BABYSTEPPING
#ifdef BABYSTEPPING
#ifdef BABYSTEP_XY #ifdef BABYSTEP_XY
MENU_ITEM(submenu, MSG_BABYSTEP_X, lcd_babystep_x); MENU_ITEM(submenu, MSG_BABYSTEP_X, lcd_babystep_x);
MENU_ITEM(submenu, MSG_BABYSTEP_Y, lcd_babystep_y); MENU_ITEM(submenu, MSG_BABYSTEP_Y, lcd_babystep_y);
#endif //BABYSTEP_XY #endif //BABYSTEP_XY
MENU_ITEM(submenu, MSG_BABYSTEP_Z, lcd_babystep_z); MENU_ITEM(submenu, MSG_BABYSTEP_Z, lcd_babystep_z);
#endif #endif
#ifdef FILAMENTCHANGEENABLE #ifdef FILAMENTCHANGEENABLE
MENU_ITEM(gcode, MSG_FILAMENTCHANGE, PSTR("M600")); MENU_ITEM(gcode, MSG_FILAMENTCHANGE, PSTR("M600"));
#endif #endif
END_MENU(); END_MENU();
} }
void lcd_preheat_pla0() void _lcd_preheat(int endnum, const float temph, const float tempb, const int fan) {
{ if (temph > 0) setTargetHotend(temph, endnum);
setTargetHotend0(plaPreheatHotendTemp); setTargetBed(tempb);
setTargetBed(plaPreheatHPBTemp); fanSpeed = fan;
fanSpeed = plaPreheatFanSpeed; lcd_return_to_status();
lcd_return_to_status(); setWatch(); // heater sanity check timer
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs0()
{
setTargetHotend0(absPreheatHotendTemp);
setTargetBed(absPreheatHPBTemp);
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
} }
void lcd_preheat_pla0() { _lcd_preheat(0, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed); }
void lcd_preheat_abs0() { _lcd_preheat(0, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed); }
#if TEMP_SENSOR_1 != 0 //2nd extruder preheat #if TEMP_SENSOR_1 != 0 //2nd extruder preheat
void lcd_preheat_pla1() void lcd_preheat_pla1() { _lcd_preheat(1, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed); }
{ void lcd_preheat_abs1() { _lcd_preheat(1, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed); }
setTargetHotend1(plaPreheatHotendTemp);
setTargetBed(plaPreheatHPBTemp);
fanSpeed = plaPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs1()
{
setTargetHotend1(absPreheatHotendTemp);
setTargetBed(absPreheatHPBTemp);
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
#endif //2nd extruder preheat #endif //2nd extruder preheat
#if TEMP_SENSOR_2 != 0 //3 extruder preheat #if TEMP_SENSOR_2 != 0 //3 extruder preheat
void lcd_preheat_pla2() void lcd_preheat_pla2() { _lcd_preheat(2, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed); }
{ void lcd_preheat_abs2() { _lcd_preheat(2, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed); }
setTargetHotend2(plaPreheatHotendTemp);
setTargetBed(plaPreheatHPBTemp);
fanSpeed = plaPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs2()
{
setTargetHotend2(absPreheatHotendTemp);
setTargetBed(absPreheatHPBTemp);
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
#endif //3 extruder preheat #endif //3 extruder preheat
#if TEMP_SENSOR_3 != 0 //4 extruder preheat #if TEMP_SENSOR_3 != 0 //4 extruder preheat
void lcd_preheat_pla3() void lcd_preheat_pla3() { _lcd_preheat(3, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed); }
{ void lcd_preheat_abs3() { _lcd_preheat(3, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed); }
setTargetHotend3(plaPreheatHotendTemp);
setTargetBed(plaPreheatHPBTemp);
fanSpeed = plaPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs3()
{
setTargetHotend3(absPreheatHotendTemp);
setTargetBed(absPreheatHPBTemp);
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
#endif //4 extruder preheat #endif //4 extruder preheat
#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //more than one extruder present #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //more than one extruder present
void lcd_preheat_pla0123() void lcd_preheat_pla0123() {
{
setTargetHotend0(plaPreheatHotendTemp); setTargetHotend0(plaPreheatHotendTemp);
setTargetHotend1(plaPreheatHotendTemp); setTargetHotend1(plaPreheatHotendTemp);
setTargetHotend2(plaPreheatHotendTemp); setTargetHotend2(plaPreheatHotendTemp);
setTargetHotend3(plaPreheatHotendTemp); _lcd_preheat(3, plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed);
setTargetBed(plaPreheatHPBTemp); }
fanSpeed = plaPreheatFanSpeed; void lcd_preheat_abs0123() {
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs0123()
{
setTargetHotend0(absPreheatHotendTemp); setTargetHotend0(absPreheatHotendTemp);
setTargetHotend1(absPreheatHotendTemp); setTargetHotend1(absPreheatHotendTemp);
setTargetHotend2(absPreheatHotendTemp); setTargetHotend2(absPreheatHotendTemp);
setTargetHotend3(absPreheatHotendTemp); _lcd_preheat(3, absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed);
setTargetBed(absPreheatHPBTemp); }
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
#endif //more than one extruder present #endif //more than one extruder present
void lcd_preheat_pla_bedonly() void lcd_preheat_pla_bedonly() { _lcd_preheat(0, 0, plaPreheatHPBTemp, plaPreheatFanSpeed); }
{ void lcd_preheat_abs_bedonly() { _lcd_preheat(0, 0, absPreheatHPBTemp, absPreheatFanSpeed); }
setTargetBed(plaPreheatHPBTemp);
fanSpeed = plaPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
void lcd_preheat_abs_bedonly() static void lcd_preheat_pla_menu() {
{
setTargetBed(absPreheatHPBTemp);
fanSpeed = absPreheatFanSpeed;
lcd_return_to_status();
setWatch(); // heater sanity check timer
}
static void lcd_preheat_pla_menu()
{
START_MENU(); START_MENU();
MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu);
MENU_ITEM(function, MSG_PREHEAT_PLA_N "1", lcd_preheat_pla0); MENU_ITEM(function, MSG_PREHEAT_PLA_N MSG_H1, lcd_preheat_pla0);
#if TEMP_SENSOR_1 != 0 //2 extruder preheat #if TEMP_SENSOR_1 != 0 //2 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_PLA_N "2", lcd_preheat_pla1); MENU_ITEM(function, MSG_PREHEAT_PLA_N MSG_H2, lcd_preheat_pla1);
#endif //2 extruder preheat #endif //2 extruder preheat
#if TEMP_SENSOR_2 != 0 //3 extruder preheat #if TEMP_SENSOR_2 != 0 //3 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_PLA_N "3", lcd_preheat_pla2); MENU_ITEM(function, MSG_PREHEAT_PLA_N MSG_H3, lcd_preheat_pla2);
#endif //3 extruder preheat #endif //3 extruder preheat
#if TEMP_SENSOR_3 != 0 //4 extruder preheat #if TEMP_SENSOR_3 != 0 //4 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_PLA_N "4", lcd_preheat_pla3); MENU_ITEM(function, MSG_PREHEAT_PLA_N MSG_H4, lcd_preheat_pla3);
#endif //4 extruder preheat #endif //4 extruder preheat
#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //all extruder preheat #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //all extruder preheat
MENU_ITEM(function, MSG_PREHEAT_PLA_ALL, lcd_preheat_pla0123); MENU_ITEM(function, MSG_PREHEAT_PLA_ALL, lcd_preheat_pla0123);
#endif //all extruder preheat #endif //all extruder preheat
#if TEMP_SENSOR_BED != 0 #if TEMP_SENSOR_BED != 0
MENU_ITEM(function, MSG_PREHEAT_PLA_BEDONLY, lcd_preheat_pla_bedonly); MENU_ITEM(function, MSG_PREHEAT_PLA_BEDONLY, lcd_preheat_pla_bedonly);
#endif #endif
END_MENU(); END_MENU();
} }
static void lcd_preheat_abs_menu() static void lcd_preheat_abs_menu() {
{
START_MENU(); START_MENU();
MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu);
MENU_ITEM(function, MSG_PREHEAT_ABS_N "1", lcd_preheat_abs0); MENU_ITEM(function, MSG_PREHEAT_ABS_N MSG_H1, lcd_preheat_abs0);
#if TEMP_SENSOR_1 != 0 //2 extruder preheat #if TEMP_SENSOR_1 != 0 //2 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_ABS_N "2", lcd_preheat_abs1); MENU_ITEM(function, MSG_PREHEAT_ABS_N MSG_H2, lcd_preheat_abs1);
#endif //2 extruder preheat #endif //2 extruder preheat
#if TEMP_SENSOR_2 != 0 //3 extruder preheat #if TEMP_SENSOR_2 != 0 //3 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_ABS_N "3", lcd_preheat_abs2); MENU_ITEM(function, MSG_PREHEAT_ABS_N MSG_H3, lcd_preheat_abs2);
#endif //3 extruder preheat #endif //3 extruder preheat
#if TEMP_SENSOR_3 != 0 //4 extruder preheat #if TEMP_SENSOR_3 != 0 //4 extruder preheat
MENU_ITEM(function, MSG_PREHEAT_ABS_N "4", lcd_preheat_abs3); MENU_ITEM(function, MSG_PREHEAT_ABS_N MSG_H4, lcd_preheat_abs3);
#endif //4 extruder preheat #endif //4 extruder preheat
#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //all extruder preheat #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_3 != 0 //all extruder preheat
MENU_ITEM(function, MSG_PREHEAT_ABS_ALL, lcd_preheat_abs0123); MENU_ITEM(function, MSG_PREHEAT_ABS_ALL, lcd_preheat_abs0123);
#endif //all extruder preheat #endif //all extruder preheat
#if TEMP_SENSOR_BED != 0
#if TEMP_SENSOR_BED != 0 MENU_ITEM(function, MSG_PREHEAT_ABS_BEDONLY, lcd_preheat_abs_bedonly);
MENU_ITEM(function, MSG_PREHEAT_ABS_BEDONLY, lcd_preheat_abs_bedonly); #endif
#endif
END_MENU(); END_MENU();
} }
void lcd_cooldown() void lcd_cooldown() {
{ setTargetHotend0(0);
setTargetHotend0(0); setTargetHotend1(0);
setTargetHotend1(0); setTargetHotend2(0);
setTargetHotend2(0); setTargetHotend3(0);
setTargetHotend3(0); setTargetBed(0);
setTargetBed(0); fanSpeed = 0;
fanSpeed = 0; lcd_return_to_status();
lcd_return_to_status();
} }
static void lcd_prepare_menu() static void lcd_prepare_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
MENU_ITEM(back, MSG_MAIN, lcd_main_menu); #ifdef SDSUPPORT
#ifdef SDSUPPORT
#ifdef MENU_ADDAUTOSTART #ifdef MENU_ADDAUTOSTART
MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd); MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd);
#endif #endif
#endif
MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
MENU_ITEM(function, MSG_SET_HOME_OFFSETS, lcd_set_home_offsets);
//MENU_ITEM(gcode, MSG_SET_ORIGIN, PSTR("G92 X0 Y0 Z0"));
#if TEMP_SENSOR_0 != 0
#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_BED != 0
MENU_ITEM(submenu, MSG_PREHEAT_PLA, lcd_preheat_pla_menu);
MENU_ITEM(submenu, MSG_PREHEAT_ABS, lcd_preheat_abs_menu);
#else
MENU_ITEM(function, MSG_PREHEAT_PLA, lcd_preheat_pla0);
MENU_ITEM(function, MSG_PREHEAT_ABS, lcd_preheat_abs0);
#endif #endif
#endif MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown); MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
#if PS_ON_PIN > -1 MENU_ITEM(function, MSG_SET_HOME_OFFSETS, lcd_set_home_offsets);
if (powersupply) //MENU_ITEM(gcode, MSG_SET_ORIGIN, PSTR("G92 X0 Y0 Z0"));
{ #if TEMP_SENSOR_0 != 0
MENU_ITEM(gcode, MSG_SWITCH_PS_OFF, PSTR("M81")); #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_BED != 0
}else{ MENU_ITEM(submenu, MSG_PREHEAT_PLA, lcd_preheat_pla_menu);
MENU_ITEM(gcode, MSG_SWITCH_PS_ON, PSTR("M80")); MENU_ITEM(submenu, MSG_PREHEAT_ABS, lcd_preheat_abs_menu);
#else
MENU_ITEM(function, MSG_PREHEAT_PLA, lcd_preheat_pla0);
MENU_ITEM(function, MSG_PREHEAT_ABS, lcd_preheat_abs0);
#endif
#endif
MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown);
#if defined(POWER_SUPPLY) && POWER_SUPPLY > 0 && defined(PS_ON_PIN) && PS_ON_PIN > -1
if (powersupply) {
MENU_ITEM(gcode, MSG_SWITCH_PS_OFF, PSTR("M81"));
} }
#endif else {
MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu); MENU_ITEM(gcode, MSG_SWITCH_PS_ON, PSTR("M80"));
END_MENU(); }
#endif
MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu);
END_MENU();
} }
#ifdef DELTA_CALIBRATION_MENU #ifdef DELTA_CALIBRATION_MENU
@ -719,394 +658,409 @@ static void lcd_move_x() { _lcd_move(PSTR("X"), X_AXIS, X_MIN_POS, X_MAX_POS); }
static void lcd_move_y() { _lcd_move(PSTR("Y"), Y_AXIS, Y_MIN_POS, Y_MAX_POS); } static void lcd_move_y() { _lcd_move(PSTR("Y"), Y_AXIS, Y_MIN_POS, Y_MAX_POS); }
static void lcd_move_z() { _lcd_move(PSTR("Z"), Z_AXIS, Z_MIN_POS, Z_MAX_POS); } static void lcd_move_z() { _lcd_move(PSTR("Z"), Z_AXIS, Z_MIN_POS, Z_MAX_POS); }
static void lcd_move_e() static void lcd_move_e() {
{ if (encoderPosition != 0) {
if (encoderPosition != 0) current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
{ encoderPosition = 0;
current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale; #ifdef DELTA
encoderPosition = 0; calculate_delta(current_position);
#ifdef DELTA plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
calculate_delta(current_position); #else
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
#else #endif
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); lcdDrawUpdate = 1;
#endif }
lcdDrawUpdate = 1; if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS]));
} if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
if (lcdDrawUpdate)
{
lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS]));
}
if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
} }
static void lcd_move_menu_axis() static void lcd_move_menu_axis() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_MOVE_AXIS, lcd_move_menu);
MENU_ITEM(back, MSG_MOVE_AXIS, lcd_move_menu); MENU_ITEM(submenu, MSG_MOVE_X, lcd_move_x);
MENU_ITEM(submenu, MSG_MOVE_X, lcd_move_x); MENU_ITEM(submenu, MSG_MOVE_Y, lcd_move_y);
MENU_ITEM(submenu, MSG_MOVE_Y, lcd_move_y); if (move_menu_scale < 10.0) {
if (move_menu_scale < 10.0) MENU_ITEM(submenu, MSG_MOVE_Z, lcd_move_z);
{ MENU_ITEM(submenu, MSG_MOVE_E, lcd_move_e);
MENU_ITEM(submenu, MSG_MOVE_Z, lcd_move_z); }
MENU_ITEM(submenu, MSG_MOVE_E, lcd_move_e); END_MENU();
}
END_MENU();
} }
static void lcd_move_menu_10mm() static void lcd_move_menu_10mm() {
{ move_menu_scale = 10.0;
move_menu_scale = 10.0; lcd_move_menu_axis();
lcd_move_menu_axis();
} }
static void lcd_move_menu_1mm() static void lcd_move_menu_1mm() {
{ move_menu_scale = 1.0;
move_menu_scale = 1.0; lcd_move_menu_axis();
lcd_move_menu_axis();
} }
static void lcd_move_menu_01mm() static void lcd_move_menu_01mm() {
{ move_menu_scale = 0.1;
move_menu_scale = 0.1; lcd_move_menu_axis();
lcd_move_menu_axis();
} }
static void lcd_move_menu() static void lcd_move_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu);
MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); MENU_ITEM(submenu, MSG_MOVE_10MM, lcd_move_menu_10mm);
MENU_ITEM(submenu, MSG_MOVE_10MM, lcd_move_menu_10mm); MENU_ITEM(submenu, MSG_MOVE_1MM, lcd_move_menu_1mm);
MENU_ITEM(submenu, MSG_MOVE_1MM, lcd_move_menu_1mm); MENU_ITEM(submenu, MSG_MOVE_01MM, lcd_move_menu_01mm);
MENU_ITEM(submenu, MSG_MOVE_01MM, lcd_move_menu_01mm); //TODO:X,Y,Z,E
//TODO:X,Y,Z,E END_MENU();
END_MENU();
} }
static void lcd_control_menu() static void lcd_control_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
MENU_ITEM(back, MSG_MAIN, lcd_main_menu); MENU_ITEM(submenu, MSG_TEMPERATURE, lcd_control_temperature_menu);
MENU_ITEM(submenu, MSG_TEMPERATURE, lcd_control_temperature_menu); MENU_ITEM(submenu, MSG_MOTION, lcd_control_motion_menu);
MENU_ITEM(submenu, MSG_MOTION, lcd_control_motion_menu); MENU_ITEM(submenu, MSG_VOLUMETRIC, lcd_control_volumetric_menu);
MENU_ITEM(submenu, MSG_VOLUMETRIC, lcd_control_volumetric_menu);
#ifdef DOGLCD #ifdef DOGLCD
// MENU_ITEM_EDIT(int3, MSG_CONTRAST, &lcd_contrast, 0, 63); //MENU_ITEM_EDIT(int3, MSG_CONTRAST, &lcd_contrast, 0, 63);
MENU_ITEM(submenu, MSG_CONTRAST, lcd_set_contrast); MENU_ITEM(submenu, MSG_CONTRAST, lcd_set_contrast);
#endif #endif
#ifdef FWRETRACT #ifdef FWRETRACT
MENU_ITEM(submenu, MSG_RETRACT, lcd_control_retract_menu); MENU_ITEM(submenu, MSG_RETRACT, lcd_control_retract_menu);
#endif #endif
#ifdef EEPROM_SETTINGS #ifdef EEPROM_SETTINGS
MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings);
MENU_ITEM(function, MSG_LOAD_EPROM, Config_RetrieveSettings); MENU_ITEM(function, MSG_LOAD_EPROM, Config_RetrieveSettings);
#endif #endif
MENU_ITEM(function, MSG_RESTORE_FAILSAFE, Config_ResetDefault); MENU_ITEM(function, MSG_RESTORE_FAILSAFE, Config_ResetDefault);
END_MENU(); END_MENU();
} }
static void lcd_control_temperature_menu() #ifdef PIDTEMP
{
// Helpers for editing PID Ki & Kd values
// grab the PID value out of the temp variable; scale it; then update the PID driver
void copy_and_scalePID_i(int e) {
PID_PARAM(Ki, e) = scalePID_i(raw_Ki);
updatePID();
}
void copy_and_scalePID_d(int e) {
PID_PARAM(Kd, e) = scalePID_d(raw_Kd);
updatePID();
}
void copy_and_scalePID_i_E1() { copy_and_scalePID_i(0); }
void copy_and_scalePID_d_E1() { copy_and_scalePID_d(0); }
#ifdef PID_PARAMS_PER_EXTRUDER
#if EXTRUDERS > 1
void copy_and_scalePID_i_E2() { copy_and_scalePID_i(1); }
void copy_and_scalePID_d_E2() { copy_and_scalePID_d(1); }
#if EXTRUDERS > 2
void copy_and_scalePID_i_E3() { copy_and_scalePID_i(2); }
void copy_and_scalePID_d_E3() { copy_and_scalePID_d(2); }
#if EXTRUDERS > 3
void copy_and_scalePID_i_E4() { copy_and_scalePID_i(3); }
void copy_and_scalePID_d_E4() { copy_and_scalePID_d(3); }
#endif //EXTRUDERS > 3
#endif //EXTRUDERS > 2
#endif //EXTRUDERS > 1
#endif //PID_PARAMS_PER_EXTRUDER
#endif //PIDTEMP
static void lcd_control_temperature_menu() {
START_MENU(); START_MENU();
MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
#if TEMP_SENSOR_0 != 0 #if TEMP_SENSOR_0 != 0
MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15);
#endif #endif
#if TEMP_SENSOR_1 != 0 && EXTRUDERS > 1
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 2", &target_temperature[1], 0, HEATER_1_MAXTEMP - 15);
#endif
#if TEMP_SENSOR_2 != 0 && EXTRUDERS > 2
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 3", &target_temperature[2], 0, HEATER_2_MAXTEMP - 15);
#endif
#if TEMP_SENSOR_3 != 0 && EXTRUDERS > 3
MENU_ITEM_EDIT(int3, MSG_NOZZLE " 4", &target_temperature[3], 0, HEATER_3_MAXTEMP - 15);
#endif
#if TEMP_SENSOR_BED != 0
MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
#endif
MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
#if defined AUTOTEMP && (TEMP_SENSOR_0 != 0)
MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled);
MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0);
#endif
#ifdef PIDTEMP
// set up temp variables - undo the default scaling
pid_current_extruder = 0;
raw_Ki = unscalePID_i(PID_PARAM(Ki,0));
raw_Kd = unscalePID_d(PID_PARAM(Kd,0));
MENU_ITEM_EDIT(float52, MSG_PID_P, &PID_PARAM(Kp,0), 1, 9990);
// i is typically a small value so allows values below 1
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I, &raw_Ki, 0.01, 9990, copy_and_scalePID_i);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D, &raw_Kd, 1, 9990, copy_and_scalePID_d);
#ifdef PID_ADD_EXTRUSION_RATE
MENU_ITEM_EDIT(float3, MSG_PID_C, &PID_PARAM(Kc,0), 1, 9990);
#endif//PID_ADD_EXTRUSION_RATE
#ifdef PID_PARAMS_PER_EXTRUDER
#if EXTRUDERS > 1 #if EXTRUDERS > 1
// set up temp variables - undo the default scaling #if TEMP_SENSOR_1 != 0
pid_current_extruder = 0; MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N2, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15);
raw_Ki = unscalePID_i(PID_PARAM(Ki,1)); #endif
raw_Kd = unscalePID_d(PID_PARAM(Kd,1)); #if EXTRUDERS > 2
MENU_ITEM_EDIT(float52, MSG_PID_P " E2", &PID_PARAM(Kp,1), 1, 9990); #if TEMP_SENSOR_2 != 0
// i is typically a small value so allows values below 1 MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N3, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I " E2", &raw_Ki, 0.01, 9990, copy_and_scalePID_i); #endif
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D " E2", &raw_Kd, 1, 9990, copy_and_scalePID_d); #if EXTRUDERS > 3
#ifdef PID_ADD_EXTRUSION_RATE #if TEMP_SENSOR_3 != 0
MENU_ITEM_EDIT(float3, MSG_PID_C " E2", &PID_PARAM(Kc,1), 1, 9990); MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_NOZZLE MSG_N4, &target_temperature[3], 0, HEATER_3_MAXTEMP - 15);
#endif//PID_ADD_EXTRUSION_RATE #endif
#endif//EXTRUDERS > 1 #endif // EXTRUDERS > 3
#if EXTRUDERS > 2 #endif // EXTRUDERS > 2
// set up temp variables - undo the default scaling #endif // EXTRUDERS > 1
pid_current_extruder = 0; #if TEMP_SENSOR_BED != 0
raw_Ki = unscalePID_i(PID_PARAM(Ki,2)); MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15);
raw_Kd = unscalePID_d(PID_PARAM(Kd,2)); #endif
MENU_ITEM_EDIT(float52, MSG_PID_P " E3", &PID_PARAM(Kp,2), 1, 9990); MENU_MULTIPLIER_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255);
// i is typically a small value so allows values below 1 #if defined(AUTOTEMP) && (TEMP_SENSOR_0 != 0)
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I " E3", &raw_Ki, 0.01, 9990, copy_and_scalePID_i); MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D " E3", &raw_Kd, 1, 9990, copy_and_scalePID_d); MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0);
#endif
#ifdef PIDTEMP
// set up temp variables - undo the default scaling
raw_Ki = unscalePID_i(PID_PARAM(Ki,0));
raw_Kd = unscalePID_d(PID_PARAM(Kd,0));
MENU_ITEM_EDIT(float52, MSG_PID_P, &PID_PARAM(Kp,0), 1, 9990);
// i is typically a small value so allows values below 1
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I, &raw_Ki, 0.01, 9990, copy_and_scalePID_i_E1);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D, &raw_Kd, 1, 9990, copy_and_scalePID_d_E1);
#ifdef PID_ADD_EXTRUSION_RATE
MENU_ITEM_EDIT(float3, MSG_PID_C, &PID_PARAM(Kc,0), 1, 9990);
#endif//PID_ADD_EXTRUSION_RATE
#ifdef PID_PARAMS_PER_EXTRUDER
#if EXTRUDERS > 1
// set up temp variables - undo the default scaling
raw_Ki = unscalePID_i(PID_PARAM(Ki,1));
raw_Kd = unscalePID_d(PID_PARAM(Kd,1));
MENU_ITEM_EDIT(float52, MSG_PID_P MSG_E2, &PID_PARAM(Kp,1), 1, 9990);
// i is typically a small value so allows values below 1
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I MSG_E2, &raw_Ki, 0.01, 9990, copy_and_scalePID_i_E2);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D MSG_E2, &raw_Kd, 1, 9990, copy_and_scalePID_d_E2);
#ifdef PID_ADD_EXTRUSION_RATE #ifdef PID_ADD_EXTRUSION_RATE
MENU_ITEM_EDIT(float3, MSG_PID_C " E3", &PID_PARAM(Kc,2), 1, 9990); MENU_ITEM_EDIT(float3, MSG_PID_C MSG_E2, &PID_PARAM(Kc,1), 1, 9990);
#endif//PID_ADD_EXTRUSION_RATE #endif//PID_ADD_EXTRUSION_RATE
#endif//EXTRUDERS > 2
#endif // PID_PARAMS_PER_EXTRUDER #if EXTRUDERS > 2
#endif//PIDTEMP // set up temp variables - undo the default scaling
MENU_ITEM(submenu, MSG_PREHEAT_PLA_SETTINGS, lcd_control_temperature_preheat_pla_settings_menu); raw_Ki = unscalePID_i(PID_PARAM(Ki,2));
MENU_ITEM(submenu, MSG_PREHEAT_ABS_SETTINGS, lcd_control_temperature_preheat_abs_settings_menu); raw_Kd = unscalePID_d(PID_PARAM(Kd,2));
END_MENU(); MENU_ITEM_EDIT(float52, MSG_PID_P MSG_E3, &PID_PARAM(Kp,2), 1, 9990);
// i is typically a small value so allows values below 1
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I MSG_E3, &raw_Ki, 0.01, 9990, copy_and_scalePID_i_E3);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D MSG_E3, &raw_Kd, 1, 9990, copy_and_scalePID_d_E3);
#ifdef PID_ADD_EXTRUSION_RATE
MENU_ITEM_EDIT(float3, MSG_PID_C MSG_E3, &PID_PARAM(Kc,2), 1, 9990);
#endif//PID_ADD_EXTRUSION_RATE
#if EXTRUDERS > 3
// set up temp variables - undo the default scaling
raw_Ki = unscalePID_i(PID_PARAM(Ki,3));
raw_Kd = unscalePID_d(PID_PARAM(Kd,3));
MENU_ITEM_EDIT(float52, MSG_PID_P MSG_E4, &PID_PARAM(Kp,3), 1, 9990);
// i is typically a small value so allows values below 1
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I MSG_E4, &raw_Ki, 0.01, 9990, copy_and_scalePID_i_E4);
MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D MSG_E4, &raw_Kd, 1, 9990, copy_and_scalePID_d_E4);
#ifdef PID_ADD_EXTRUSION_RATE
MENU_ITEM_EDIT(float3, MSG_PID_C MSG_E4, &PID_PARAM(Kc,3), 1, 9990);
#endif//PID_ADD_EXTRUSION_RATE
#endif//EXTRUDERS > 3
#endif//EXTRUDERS > 2
#endif//EXTRUDERS > 1
#endif //PID_PARAMS_PER_EXTRUDER
#endif//PIDTEMP
MENU_ITEM(submenu, MSG_PREHEAT_PLA_SETTINGS, lcd_control_temperature_preheat_pla_settings_menu);
MENU_ITEM(submenu, MSG_PREHEAT_ABS_SETTINGS, lcd_control_temperature_preheat_abs_settings_menu);
END_MENU();
} }
static void lcd_control_temperature_preheat_pla_settings_menu() static void lcd_control_temperature_preheat_pla_settings_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &plaPreheatFanSpeed, 0, 255);
MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &plaPreheatFanSpeed, 0, 255); #if TEMP_SENSOR_0 != 0
#if TEMP_SENSOR_0 != 0
MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15);
#endif #endif
#if TEMP_SENSOR_BED != 0 #if TEMP_SENSOR_BED != 0
MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, 0, BED_MAXTEMP - 15); MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, 0, BED_MAXTEMP - 15);
#endif #endif
#ifdef EEPROM_SETTINGS #ifdef EEPROM_SETTINGS
MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings);
#endif #endif
END_MENU(); END_MENU();
} }
static void lcd_control_temperature_preheat_abs_settings_menu() static void lcd_control_temperature_preheat_abs_settings_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu);
MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &absPreheatFanSpeed, 0, 255);
MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &absPreheatFanSpeed, 0, 255); #if TEMP_SENSOR_0 != 0
#if TEMP_SENSOR_0 != 0
MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15);
#endif #endif
#if TEMP_SENSOR_BED != 0 #if TEMP_SENSOR_BED != 0
MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, 0, BED_MAXTEMP - 15); MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, 0, BED_MAXTEMP - 15);
#endif #endif
#ifdef EEPROM_SETTINGS #ifdef EEPROM_SETTINGS
MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings);
#endif #endif
END_MENU(); END_MENU();
} }
static void lcd_control_motion_menu() static void lcd_control_motion_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); #ifdef ENABLE_AUTO_BED_LEVELING
#ifdef ENABLE_AUTO_BED_LEVELING
MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, 0.5, 50); MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, 0.5, 50);
#endif #endif
MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 500, 99000); MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 500, 99000);
MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990); MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990);
MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &max_z_jerk, 0.1, 990); MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &max_z_jerk, 0.1, 990);
MENU_ITEM_EDIT(float3, MSG_VE_JERK, &max_e_jerk, 1, 990); MENU_ITEM_EDIT(float3, MSG_VE_JERK, &max_e_jerk, 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &max_feedrate[X_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &max_feedrate[X_AXIS], 1, 999);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &max_feedrate[Y_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &max_feedrate[Y_AXIS], 1, 999);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &max_feedrate[Z_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &max_feedrate[Z_AXIS], 1, 999);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E, &max_feedrate[E_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E, &max_feedrate[E_AXIS], 1, 999);
MENU_ITEM_EDIT(float3, MSG_VMIN, &minimumfeedrate, 0, 999); MENU_ITEM_EDIT(float3, MSG_VMIN, &minimumfeedrate, 0, 999);
MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &mintravelfeedrate, 0, 999); MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &mintravelfeedrate, 0, 999);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, reset_acceleration_rates);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, reset_acceleration_rates);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &max_acceleration_units_per_sq_second[Z_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &max_acceleration_units_per_sq_second[Z_AXIS], 100, 99000, reset_acceleration_rates);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_E, &max_acceleration_units_per_sq_second[E_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_E, &max_acceleration_units_per_sq_second[E_AXIS], 100, 99000, reset_acceleration_rates);
MENU_ITEM_EDIT(float5, MSG_A_RETRACT, &retract_acceleration, 100, 99000); MENU_ITEM_EDIT(float5, MSG_A_RETRACT, &retract_acceleration, 100, 99000);
MENU_ITEM_EDIT(float52, MSG_XSTEPS, &axis_steps_per_unit[X_AXIS], 5, 9999); MENU_ITEM_EDIT(float52, MSG_XSTEPS, &axis_steps_per_unit[X_AXIS], 5, 9999);
MENU_ITEM_EDIT(float52, MSG_YSTEPS, &axis_steps_per_unit[Y_AXIS], 5, 9999); MENU_ITEM_EDIT(float52, MSG_YSTEPS, &axis_steps_per_unit[Y_AXIS], 5, 9999);
MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &axis_steps_per_unit[Z_AXIS], 5, 9999); MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &axis_steps_per_unit[Z_AXIS], 5, 9999);
MENU_ITEM_EDIT(float51, MSG_ESTEPS, &axis_steps_per_unit[E_AXIS], 5, 9999); MENU_ITEM_EDIT(float51, MSG_ESTEPS, &axis_steps_per_unit[E_AXIS], 5, 9999);
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &abort_on_endstop_hit); MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &abort_on_endstop_hit);
#endif #endif
#ifdef SCARA #ifdef SCARA
MENU_ITEM_EDIT(float74, MSG_XSCALE, &axis_scaling[X_AXIS],0.5,2); MENU_ITEM_EDIT(float74, MSG_XSCALE, &axis_scaling[X_AXIS],0.5,2);
MENU_ITEM_EDIT(float74, MSG_YSCALE, &axis_scaling[Y_AXIS],0.5,2); MENU_ITEM_EDIT(float74, MSG_YSCALE, &axis_scaling[Y_AXIS],0.5,2);
#endif #endif
END_MENU(); END_MENU();
} }
static void lcd_control_volumetric_menu() static void lcd_control_volumetric_menu() {
{ START_MENU();
START_MENU(); MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
MENU_ITEM_EDIT_CALLBACK(bool, MSG_VOLUMETRIC_ENABLED, &volumetric_enabled, calculate_volumetric_multipliers); MENU_ITEM_EDIT_CALLBACK(bool, MSG_VOLUMETRIC_ENABLED, &volumetric_enabled, calculate_volumetric_multipliers);
if (volumetric_enabled) { if (volumetric_enabled) {
MENU_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_0, &filament_size[0], DEFAULT_NOMINAL_FILAMENT_DIA - .5, DEFAULT_NOMINAL_FILAMENT_DIA + .5, calculate_volumetric_multipliers); MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_0, &filament_size[0], 1.5, 3.25, calculate_volumetric_multipliers);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
MENU_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_1, &filament_size[1], DEFAULT_NOMINAL_FILAMENT_DIA - .5, DEFAULT_NOMINAL_FILAMENT_DIA + .5, calculate_volumetric_multipliers); MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_1, &filament_size[1], 1.5, 3.25, calculate_volumetric_multipliers);
#if EXTRUDERS > 2 #if EXTRUDERS > 2
MENU_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_2, &filament_size[2], DEFAULT_NOMINAL_FILAMENT_DIA - .5, DEFAULT_NOMINAL_FILAMENT_DIA + .5, calculate_volumetric_multipliers); MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_2, &filament_size[2], 1.5, 3.25, calculate_volumetric_multipliers);
#if EXTRUDERS > 3 #if EXTRUDERS > 3
MENU_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_3, &filament_size[3], DEFAULT_NOMINAL_FILAMENT_DIA - .5, DEFAULT_NOMINAL_FILAMENT_DIA + .5, calculate_volumetric_multipliers); MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(float43, MSG_FILAMENT_SIZE_EXTRUDER_3, &filament_size[3], 1.5, 3.25, calculate_volumetric_multipliers);
#endif //EXTRUDERS > 3 #endif //EXTRUDERS > 3
#endif //EXTRUDERS > 2 #endif //EXTRUDERS > 2
#endif //EXTRUDERS > 1 #endif //EXTRUDERS > 1
} }
END_MENU(); END_MENU();
} }
#ifdef DOGLCD #ifdef DOGLCD
static void lcd_set_contrast()
{ static void lcd_set_contrast() {
if (encoderPosition != 0) if (encoderPosition != 0) {
{ lcd_contrast -= encoderPosition;
lcd_contrast -= encoderPosition; if (lcd_contrast < 0) lcd_contrast = 0;
if (lcd_contrast < 0) lcd_contrast = 0; else if (lcd_contrast > 63) lcd_contrast = 63;
else if (lcd_contrast > 63) lcd_contrast = 63; encoderPosition = 0;
encoderPosition = 0; lcdDrawUpdate = 1;
lcdDrawUpdate = 1; u8g.setContrast(lcd_contrast);
u8g.setContrast(lcd_contrast); }
} if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
if (lcdDrawUpdate) if (LCD_CLICKED) lcd_goto_menu(lcd_control_menu);
{
lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
}
if (LCD_CLICKED) lcd_goto_menu(lcd_control_menu);
} }
#endif
#endif //DOGLCD
#ifdef FWRETRACT #ifdef FWRETRACT
static void lcd_control_retract_menu()
{ static void lcd_control_retract_menu() {
START_MENU(); START_MENU();
MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); MENU_ITEM(back, MSG_CONTROL, lcd_control_menu);
MENU_ITEM_EDIT(bool, MSG_AUTORETRACT, &autoretract_enabled); MENU_ITEM_EDIT(bool, MSG_AUTORETRACT, &autoretract_enabled);
MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT, &retract_length, 0, 100); MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT, &retract_length, 0, 100);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_SWAP, &retract_length_swap, 0, 100); MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_SWAP, &retract_length_swap, 0, 100);
#endif #endif
MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACTF, &retract_feedrate, 1, 999); MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACTF, &retract_feedrate, 1, 999);
MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_ZLIFT, &retract_zlift, 0, 999); MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_ZLIFT, &retract_zlift, 0, 999);
MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER, &retract_recover_length, 0, 100); MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER, &retract_recover_length, 0, 100);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER_SWAP, &retract_recover_length_swap, 0, 100); MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER_SWAP, &retract_recover_length_swap, 0, 100);
#endif #endif
MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999); MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999);
END_MENU(); END_MENU();
} }
#endif //FWRETRACT #endif //FWRETRACT
#if SDCARDDETECT == -1 #if SDCARDDETECT == -1
static void lcd_sd_refresh() static void lcd_sd_refresh() {
{
card.initsd(); card.initsd();
currentMenuViewOffset = 0; currentMenuViewOffset = 0;
} }
#endif #endif
static void lcd_sd_updir()
{ static void lcd_sd_updir() {
card.updir(); card.updir();
currentMenuViewOffset = 0; currentMenuViewOffset = 0;
} }
void lcd_sdcard_menu() void lcd_sdcard_menu() {
{ if (lcdDrawUpdate == 0 && LCD_CLICKED == 0) return; // nothing to do (so don't thrash the SD card)
if (lcdDrawUpdate == 0 && LCD_CLICKED == 0) uint16_t fileCnt = card.getnrfilenames();
return; // nothing to do (so don't thrash the SD card) START_MENU();
uint16_t fileCnt = card.getnrfilenames(); MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
START_MENU(); card.getWorkDirName();
MENU_ITEM(back, MSG_MAIN, lcd_main_menu); if (card.filename[0] == '/') {
card.getWorkDirName(); #if SDCARDDETECT == -1
if(card.filename[0]=='/') MENU_ITEM(function, LCD_STR_REFRESH MSG_REFRESH, lcd_sd_refresh);
{ #endif
#if SDCARDDETECT == -1 }
MENU_ITEM(function, LCD_STR_REFRESH MSG_REFRESH, lcd_sd_refresh); else {
#endif MENU_ITEM(function, LCD_STR_FOLDER "..", lcd_sd_updir);
}else{ }
MENU_ITEM(function, LCD_STR_FOLDER "..", lcd_sd_updir);
}
for(uint16_t i=0;i<fileCnt;i++) for(uint16_t i = 0; i < fileCnt; i++) {
{ if (_menuItemNr == _lineNr) {
if (_menuItemNr == _lineNr) #ifndef SDCARD_RATHERRECENTFIRST
{ card.getfilename(i);
#ifndef SDCARD_RATHERRECENTFIRST #else
card.getfilename(i); card.getfilename(fileCnt-1-i);
#else #endif
card.getfilename(fileCnt-1-i); if (card.filenameIsDir)
#endif MENU_ITEM(sddirectory, MSG_CARD_MENU, card.filename, card.longFilename);
if (card.filenameIsDir) else
{ MENU_ITEM(sdfile, MSG_CARD_MENU, card.filename, card.longFilename);
MENU_ITEM(sddirectory, MSG_CARD_MENU, card.filename, card.longFilename);
}else{
MENU_ITEM(sdfile, MSG_CARD_MENU, card.filename, card.longFilename);
}
}else{
MENU_ITEM_DUMMY();
}
} }
END_MENU(); else {
MENU_ITEM_DUMMY();
}
}
END_MENU();
} }
#define menu_edit_type(_type, _name, _strFunc, scale) \ #define menu_edit_type(_type, _name, _strFunc, scale) \
void menu_edit_ ## _name () \ bool _menu_edit_ ## _name () { \
{ \ bool isClicked = LCD_CLICKED; \
if ((int32_t)encoderPosition < 0) encoderPosition = 0; \ if ((int32_t)encoderPosition < 0) encoderPosition = 0; \
if ((int32_t)encoderPosition > maxEditValue) encoderPosition = maxEditValue; \ if ((int32_t)encoderPosition > maxEditValue) encoderPosition = maxEditValue; \
if (lcdDrawUpdate) \ if (lcdDrawUpdate) \
lcd_implementation_drawedit(editLabel, _strFunc(((_type)((int32_t)encoderPosition + minEditValue)) / scale)); \ lcd_implementation_drawedit(editLabel, _strFunc(((_type)((int32_t)encoderPosition + minEditValue)) / scale)); \
if (LCD_CLICKED) \ if (isClicked) { \
{ \ *((_type*)editValue) = ((_type)((int32_t)encoderPosition + minEditValue)) / scale; \
*((_type*)editValue) = ((_type)((int32_t)encoderPosition + minEditValue)) / scale; \ lcd_goto_menu(prevMenu, prevEncoderPosition); \
lcd_goto_menu(prevMenu, prevEncoderPosition); \
} \
} \ } \
void menu_edit_callback_ ## _name () { \ return isClicked; \
menu_edit_ ## _name (); \ } \
if (LCD_CLICKED) (*callbackFunc)(); \ void menu_edit_ ## _name () { _menu_edit_ ## _name(); } \
} \ void menu_edit_callback_ ## _name () { if (_menu_edit_ ## _name ()) (*callbackFunc)(); } \
static void menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) \ static void _menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) { \
{ \ prevMenu = currentMenu; \
prevMenu = currentMenu; \ prevEncoderPosition = encoderPosition; \
prevEncoderPosition = encoderPosition; \ \
\ lcdDrawUpdate = 2; \
lcdDrawUpdate = 2; \ currentMenu = menu_edit_ ## _name; \
currentMenu = menu_edit_ ## _name; \ \
\ editLabel = pstr; \
editLabel = pstr; \ editValue = ptr; \
editValue = ptr; \ minEditValue = minValue * scale; \
minEditValue = minValue * scale; \ maxEditValue = maxValue * scale - minEditValue; \
maxEditValue = maxValue * scale - minEditValue; \ encoderPosition = (*ptr) * scale - minEditValue; \
encoderPosition = (*ptr) * scale - minEditValue; \ } \
}\ static void menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) { \
static void menu_action_setting_edit_callback_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue, menuFunc_t callback) \ _menu_action_setting_edit_ ## _name(pstr, ptr, minValue, maxValue); \
{ \ currentMenu = menu_edit_ ## _name; \
prevMenu = currentMenu; \ }\
prevEncoderPosition = encoderPosition; \ static void menu_action_setting_edit_callback_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue, menuFunc_t callback) { \
\ _menu_action_setting_edit_ ## _name(pstr, ptr, minValue, maxValue); \
lcdDrawUpdate = 2; \ currentMenu = menu_edit_callback_ ## _name; \
currentMenu = menu_edit_callback_ ## _name; \ callbackFunc = callback; \
\ }
editLabel = pstr; \
editValue = ptr; \
minEditValue = minValue * scale; \
maxEditValue = maxValue * scale - minEditValue; \
encoderPosition = (*ptr) * scale - minEditValue; \
callbackFunc = callback;\
}
menu_edit_type(int, int3, itostr3, 1) menu_edit_type(int, int3, itostr3, 1)
menu_edit_type(float, float3, ftostr3, 1) menu_edit_type(float, float3, ftostr3, 1)
menu_edit_type(float, float32, ftostr32, 100) menu_edit_type(float, float32, ftostr32, 100)
@ -1117,88 +1071,81 @@ menu_edit_type(float, float52, ftostr52, 100)
menu_edit_type(unsigned long, long5, ftostr5, 0.01) menu_edit_type(unsigned long, long5, ftostr5, 0.01)
#ifdef REPRAPWORLD_KEYPAD #ifdef REPRAPWORLD_KEYPAD
static void reprapworld_keypad_move_z_up() { static void reprapworld_keypad_move_z_up() {
encoderPosition = 1; encoderPosition = 1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_z(); lcd_move_z();
} }
static void reprapworld_keypad_move_z_down() { static void reprapworld_keypad_move_z_down() {
encoderPosition = -1; encoderPosition = -1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_z(); lcd_move_z();
} }
static void reprapworld_keypad_move_x_left() { static void reprapworld_keypad_move_x_left() {
encoderPosition = -1; encoderPosition = -1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_x(); lcd_move_x();
} }
static void reprapworld_keypad_move_x_right() { static void reprapworld_keypad_move_x_right() {
encoderPosition = 1; encoderPosition = 1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_x(); lcd_move_x();
} }
static void reprapworld_keypad_move_y_down() { static void reprapworld_keypad_move_y_down() {
encoderPosition = 1; encoderPosition = 1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_y();
}
static void reprapworld_keypad_move_y_up() {
encoderPosition = -1;
move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
lcd_move_y(); lcd_move_y();
} }
static void reprapworld_keypad_move_home() { static void reprapworld_keypad_move_y_up() {
enquecommand_P((PSTR("G28"))); // move all axis home encoderPosition = -1;
} move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP;
#endif lcd_move_y();
}
static void reprapworld_keypad_move_home() {
enquecommands_P((PSTR("G28"))); // move all axis home
}
#endif //REPRAPWORLD_KEYPAD
/** End of menus **/ /** End of menus **/
static void lcd_quick_feedback() static void lcd_quick_feedback() {
{ lcdDrawUpdate = 2;
lcdDrawUpdate = 2; blocking_enc = millis() + 500;
blocking_enc = millis() + 500; lcd_implementation_quick_feedback();
lcd_implementation_quick_feedback();
} }
/** Menu action functions **/ /** Menu action functions **/
static void menu_action_back(menuFunc_t data) { lcd_goto_menu(data); } static void menu_action_back(menuFunc_t data) { lcd_goto_menu(data); }
static void menu_action_submenu(menuFunc_t data) { lcd_goto_menu(data); } static void menu_action_submenu(menuFunc_t data) { lcd_goto_menu(data); }
static void menu_action_gcode(const char* pgcode) { enquecommand_P(pgcode); } static void menu_action_gcode(const char* pgcode) { enquecommands_P(pgcode); }
static void menu_action_function(menuFunc_t data) { (*data)(); } static void menu_action_function(menuFunc_t data) { (*data)(); }
static void menu_action_sdfile(const char* filename, char* longFilename) static void menu_action_sdfile(const char* filename, char* longFilename) {
{ char cmd[30];
char cmd[30]; char* c;
char* c; sprintf_P(cmd, PSTR("M23 %s"), filename);
sprintf_P(cmd, PSTR("M23 %s"), filename); for(c = &cmd[4]; *c; c++) *c = tolower(*c);
for(c = &cmd[4]; *c; c++) enquecommand(cmd);
*c = tolower(*c); enquecommands_P(PSTR("M24"));
enquecommand(cmd); lcd_return_to_status();
enquecommand_P(PSTR("M24"));
lcd_return_to_status();
} }
static void menu_action_sddirectory(const char* filename, char* longFilename) static void menu_action_sddirectory(const char* filename, char* longFilename) {
{ card.chdir(filename);
card.chdir(filename); encoderPosition = 0;
encoderPosition = 0;
} }
static void menu_action_setting_edit_bool(const char* pstr, bool* ptr) static void menu_action_setting_edit_bool(const char* pstr, bool* ptr) { *ptr = !(*ptr); }
{ static void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, menuFunc_t callback) {
*ptr = !(*ptr); menu_action_setting_edit_bool(pstr, ptr);
(*callback)();
} }
static void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, menuFunc_t callback)
{ #endif //ULTIPANEL
menu_action_setting_edit_bool(pstr, ptr);
(*callback)();
}
#endif//ULTIPANEL
/** LCD API **/ /** LCD API **/
void lcd_init() void lcd_init() {
{ lcd_implementation_init();
lcd_implementation_init();
#ifdef NEWPANEL
#ifdef NEWPANEL
SET_INPUT(BTN_EN1); SET_INPUT(BTN_EN1);
SET_INPUT(BTN_EN2); SET_INPUT(BTN_EN2);
WRITE(BTN_EN1,HIGH); WRITE(BTN_EN1,HIGH);
@ -1233,133 +1180,170 @@ void lcd_init()
#endif // SR_LCD_2W_NL #endif // SR_LCD_2W_NL
#endif//!NEWPANEL #endif//!NEWPANEL
#if defined (SDSUPPORT) && defined(SDCARDDETECT) && (SDCARDDETECT > 0) #if defined(SDSUPPORT) && defined(SDCARDDETECT) && (SDCARDDETECT > 0)
pinMode(SDCARDDETECT,INPUT); pinMode(SDCARDDETECT, INPUT);
WRITE(SDCARDDETECT, HIGH); WRITE(SDCARDDETECT, HIGH);
lcd_oldcardstatus = IS_SD_INSERTED; lcd_oldcardstatus = IS_SD_INSERTED;
#endif//(SDCARDDETECT > 0) #endif //(SDCARDDETECT > 0)
#ifdef LCD_HAS_SLOW_BUTTONS
#ifdef LCD_HAS_SLOW_BUTTONS
slow_buttons = 0; slow_buttons = 0;
#endif #endif
lcd_buttons_update();
#ifdef ULTIPANEL lcd_buttons_update();
#ifdef ULTIPANEL
encoderDiff = 0; encoderDiff = 0;
#endif #endif
} }
void lcd_update() int lcd_strlen(char *s) {
{ int i = 0, j = 0;
static unsigned long timeoutToStatus = 0; while (s[i]) {
if ((s[i] & 0xc0) != 0x80) j++;
i++;
}
return j;
}
#ifdef LCD_HAS_SLOW_BUTTONS int lcd_strlen_P(const char *s) {
int j = 0;
while (pgm_read_byte(s)) {
if ((pgm_read_byte(s) & 0xc0) != 0x80) j++;
s++;
}
return j;
}
void lcd_update() {
static unsigned long timeoutToStatus = 0;
#ifdef LCD_HAS_SLOW_BUTTONS
slow_buttons = lcd_implementation_read_slow_buttons(); // buttons which take too long to read in interrupt context slow_buttons = lcd_implementation_read_slow_buttons(); // buttons which take too long to read in interrupt context
#endif
lcd_buttons_update();
#if (SDCARDDETECT > 0)
if (IS_SD_INSERTED != lcd_oldcardstatus && lcd_detected()) {
lcdDrawUpdate = 2;
lcd_oldcardstatus = IS_SD_INSERTED;
lcd_implementation_init( // to maybe revive the LCD if static electricity killed it.
#if defined(LCD_PROGRESS_BAR) && defined(SDSUPPORT) && !defined(DOGLCD)
currentMenu == lcd_status_screen
#endif
);
if (lcd_oldcardstatus) {
card.initsd();
LCD_MESSAGEPGM(MSG_SD_INSERTED);
}
else {
card.release();
LCD_MESSAGEPGM(MSG_SD_REMOVED);
}
}
#endif//CARDINSERTED
long ms = millis();
if (ms > lcd_next_update_millis) {
#ifdef ULTIPANEL
#ifdef REPRAPWORLD_KEYPAD
if (REPRAPWORLD_KEYPAD_MOVE_Z_UP) reprapworld_keypad_move_z_up();
if (REPRAPWORLD_KEYPAD_MOVE_Z_DOWN) reprapworld_keypad_move_z_down();
if (REPRAPWORLD_KEYPAD_MOVE_X_LEFT) reprapworld_keypad_move_x_left();
if (REPRAPWORLD_KEYPAD_MOVE_X_RIGHT) reprapworld_keypad_move_x_right();
if (REPRAPWORLD_KEYPAD_MOVE_Y_DOWN) reprapworld_keypad_move_y_down();
if (REPRAPWORLD_KEYPAD_MOVE_Y_UP) reprapworld_keypad_move_y_up();
if (REPRAPWORLD_KEYPAD_MOVE_HOME) reprapworld_keypad_move_home();
#endif
bool encoderPastThreshold = (abs(encoderDiff) >= ENCODER_PULSES_PER_STEP);
if (encoderPastThreshold || LCD_CLICKED) {
if (encoderPastThreshold) {
int32_t encoderMultiplier = 1;
#ifdef ENCODER_RATE_MULTIPLIER
if (encoderRateMultiplierEnabled) {
int32_t encoderMovementSteps = abs(encoderDiff) / ENCODER_PULSES_PER_STEP;
if (lastEncoderMovementMillis != 0) {
// Note that the rate is always calculated between to passes through the
// loop and that the abs of the encoderDiff value is tracked.
float encoderStepRate = (float)(encoderMovementSteps) / ((float)(ms - lastEncoderMovementMillis)) * 1000.0;
if (encoderStepRate >= ENCODER_100X_STEPS_PER_SEC) encoderMultiplier = 100;
else if (encoderStepRate >= ENCODER_10X_STEPS_PER_SEC) encoderMultiplier = 10;
#ifdef ENCODER_RATE_MULTIPLIER_DEBUG
SERIAL_ECHO_START;
SERIAL_ECHO("Enc Step Rate: ");
SERIAL_ECHO(encoderStepRate);
SERIAL_ECHO(" Multiplier: ");
SERIAL_ECHO(encoderMultiplier);
SERIAL_ECHO(" ENCODER_10X_STEPS_PER_SEC: ");
SERIAL_ECHO(ENCODER_10X_STEPS_PER_SEC);
SERIAL_ECHO(" ENCODER_100X_STEPS_PER_SEC: ");
SERIAL_ECHOLN(ENCODER_100X_STEPS_PER_SEC);
#endif //ENCODER_RATE_MULTIPLIER_DEBUG
}
lastEncoderMovementMillis = ms;
}
#endif //ENCODER_RATE_MULTIPLIER
lcdDrawUpdate = 1;
encoderPosition += (encoderDiff * encoderMultiplier) / ENCODER_PULSES_PER_STEP;
encoderDiff = 0;
}
timeoutToStatus = ms + LCD_TIMEOUT_TO_STATUS;
}
#endif //ULTIPANEL
#ifdef DOGLCD // Changes due to different driver architecture of the DOGM display
blink++; // Variable for fan animation and alive dot
u8g.firstPage();
do {
u8g.setFont(FONT_MENU);
u8g.setPrintPos(125, 0);
if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot
u8g.drawPixel(127, 63); // draw alive dot
u8g.setColorIndex(1); // black on white
(*currentMenu)();
if (!lcdDrawUpdate) break; // Terminate display update, when nothing new to draw. This must be done before the last dogm.next()
} while( u8g.nextPage() );
#else
(*currentMenu)();
#endif #endif
lcd_buttons_update(); #ifdef LCD_HAS_STATUS_INDICATORS
lcd_implementation_update_indicators();
#endif
#if (SDCARDDETECT > 0) #ifdef ULTIPANEL
if((IS_SD_INSERTED != lcd_oldcardstatus && lcd_detected())) if (currentMenu != lcd_status_screen && millis() > timeoutToStatus) {
{ lcd_return_to_status();
lcdDrawUpdate = 2; lcdDrawUpdate = 2;
lcd_oldcardstatus = IS_SD_INSERTED; }
lcd_implementation_init( // to maybe revive the LCD if static electricity killed it. #endif //ULTIPANEL
#if defined(LCD_PROGRESS_BAR) && defined(SDSUPPORT) && !defined(DOGLCD)
currentMenu == lcd_status_screen
#endif
);
if(lcd_oldcardstatus) if (lcdDrawUpdate == 2) lcd_implementation_clear();
{ if (lcdDrawUpdate) lcdDrawUpdate--;
card.initsd(); lcd_next_update_millis = millis() + LCD_UPDATE_INTERVAL;
LCD_MESSAGEPGM(MSG_SD_INSERTED); }
}
else
{
card.release();
LCD_MESSAGEPGM(MSG_SD_REMOVED);
}
}
#endif//CARDINSERTED
if (lcd_next_update_millis < millis())
{
#ifdef ULTIPANEL
#ifdef REPRAPWORLD_KEYPAD
if (REPRAPWORLD_KEYPAD_MOVE_Z_UP) {
reprapworld_keypad_move_z_up();
}
if (REPRAPWORLD_KEYPAD_MOVE_Z_DOWN) {
reprapworld_keypad_move_z_down();
}
if (REPRAPWORLD_KEYPAD_MOVE_X_LEFT) {
reprapworld_keypad_move_x_left();
}
if (REPRAPWORLD_KEYPAD_MOVE_X_RIGHT) {
reprapworld_keypad_move_x_right();
}
if (REPRAPWORLD_KEYPAD_MOVE_Y_DOWN) {
reprapworld_keypad_move_y_down();
}
if (REPRAPWORLD_KEYPAD_MOVE_Y_UP) {
reprapworld_keypad_move_y_up();
}
if (REPRAPWORLD_KEYPAD_MOVE_HOME) {
reprapworld_keypad_move_home();
}
#endif
if (abs(encoderDiff) >= ENCODER_PULSES_PER_STEP)
{
lcdDrawUpdate = 1;
encoderPosition += encoderDiff / ENCODER_PULSES_PER_STEP;
encoderDiff = 0;
timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
}
if (LCD_CLICKED)
timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
#endif//ULTIPANEL
#ifdef DOGLCD // Changes due to different driver architecture of the DOGM display
blink++; // Variable for fan animation and alive dot
u8g.firstPage();
do
{
u8g.setFont(FONT_MENU);
u8g.setPrintPos(125,0);
if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot
u8g.drawPixel(127,63); // draw alive dot
u8g.setColorIndex(1); // black on white
(*currentMenu)();
if (!lcdDrawUpdate) break; // Terminate display update, when nothing new to draw. This must be done before the last dogm.next()
} while( u8g.nextPage() );
#else
(*currentMenu)();
#endif
#ifdef LCD_HAS_STATUS_INDICATORS
lcd_implementation_update_indicators();
#endif
#ifdef ULTIPANEL
if(timeoutToStatus < millis() && currentMenu != lcd_status_screen)
{
lcd_return_to_status();
lcdDrawUpdate = 2;
}
#endif//ULTIPANEL
if (lcdDrawUpdate == 2) lcd_implementation_clear();
if (lcdDrawUpdate) lcdDrawUpdate--;
lcd_next_update_millis = millis() + LCD_UPDATE_INTERVAL;
}
} }
void lcd_ignore_click(bool b) void lcd_ignore_click(bool b) {
{ ignore_click = b;
ignore_click = b; wait_for_unclick = false;
wait_for_unclick = false;
} }
void lcd_finishstatus() { void lcd_finishstatus() {
int len = strlen(lcd_status_message); int len = lcd_strlen(lcd_status_message);
if (len > 0) { if (len > 0) {
while (len < LCD_WIDTH) { while (len < LCD_WIDTH) {
lcd_status_message[len++] = ' '; lcd_status_message[len++] = ' ';
@ -1378,145 +1362,133 @@ void lcd_finishstatus() {
message_millis = millis(); //get status message to show up for a while message_millis = millis(); //get status message to show up for a while
#endif #endif
} }
void lcd_setstatus(const char* message)
{ void lcd_setstatus(const char* message) {
if (lcd_status_message_level > 0) if (lcd_status_message_level > 0) return;
return; strncpy(lcd_status_message, message, LCD_WIDTH);
strncpy(lcd_status_message, message, LCD_WIDTH); lcd_finishstatus();
lcd_finishstatus();
}
void lcd_setstatuspgm(const char* message)
{
if (lcd_status_message_level > 0)
return;
strncpy_P(lcd_status_message, message, LCD_WIDTH);
lcd_finishstatus();
}
void lcd_setalertstatuspgm(const char* message)
{
lcd_setstatuspgm(message);
lcd_status_message_level = 1;
#ifdef ULTIPANEL
lcd_return_to_status();
#endif//ULTIPANEL
}
void lcd_reset_alert_level()
{
lcd_status_message_level = 0;
} }
void lcd_setstatuspgm(const char* message) {
if (lcd_status_message_level > 0) return;
strncpy_P(lcd_status_message, message, LCD_WIDTH);
lcd_finishstatus();
}
void lcd_setalertstatuspgm(const char* message) {
lcd_setstatuspgm(message);
lcd_status_message_level = 1;
#ifdef ULTIPANEL
lcd_return_to_status();
#endif
}
void lcd_reset_alert_level() { lcd_status_message_level = 0; }
#ifdef DOGLCD #ifdef DOGLCD
void lcd_setcontrast(uint8_t value) void lcd_setcontrast(uint8_t value) {
{
lcd_contrast = value & 63; lcd_contrast = value & 63;
u8g.setContrast(lcd_contrast); u8g.setContrast(lcd_contrast);
} }
#endif #endif
#ifdef ULTIPANEL #ifdef ULTIPANEL
////////////////////////
// Setup Rotary Encoder Bit Values (for two pin encoders to indicate movement)
// These values are independent of which pins are used for EN_A and EN_B indications
// The rotary encoder part is also independent to the chipset used for the LCD
#if defined(EN_A) && defined(EN_B)
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
#endif
/* Warning: This function is called from interrupt context */ /* Warning: This function is called from interrupt context */
void lcd_buttons_update() void lcd_buttons_update() {
{ #ifdef NEWPANEL
#ifdef NEWPANEL uint8_t newbutton = 0;
uint8_t newbutton=0; if (READ(BTN_EN1) == 0) newbutton |= EN_A;
if(READ(BTN_EN1)==0) newbutton|=EN_A; if (READ(BTN_EN2) == 0) newbutton |= EN_B;
if(READ(BTN_EN2)==0) newbutton|=EN_B; #if BTN_ENC > 0
#if BTN_ENC > 0 if (millis() > blocking_enc && READ(BTN_ENC) == 0) newbutton |= EN_C;
if((blocking_enc<millis()) && (READ(BTN_ENC)==0)) #endif
newbutton |= EN_C;
#endif
buttons = newbutton; buttons = newbutton;
#ifdef LCD_HAS_SLOW_BUTTONS #ifdef LCD_HAS_SLOW_BUTTONS
buttons |= slow_buttons; buttons |= slow_buttons;
#endif #endif
#ifdef REPRAPWORLD_KEYPAD #ifdef REPRAPWORLD_KEYPAD
// for the reprapworld_keypad // for the reprapworld_keypad
uint8_t newbutton_reprapworld_keypad=0; uint8_t newbutton_reprapworld_keypad=0;
WRITE(SHIFT_LD,LOW); WRITE(SHIFT_LD, LOW);
WRITE(SHIFT_LD,HIGH); WRITE(SHIFT_LD, HIGH);
for(int8_t i=0;i<8;i++) { for(int8_t i = 0; i < 8; i++) {
newbutton_reprapworld_keypad = newbutton_reprapworld_keypad>>1; newbutton_reprapworld_keypad >>= 1;
if(READ(SHIFT_OUT)) if (READ(SHIFT_OUT)) newbutton_reprapworld_keypad |= (1 << 7);
newbutton_reprapworld_keypad|=(1<<7); WRITE(SHIFT_CLK, HIGH);
WRITE(SHIFT_CLK,HIGH); WRITE(SHIFT_CLK, LOW);
WRITE(SHIFT_CLK,LOW);
} }
buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0 buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0
#endif #endif
#else //read it from the shift register #else //read it from the shift register
uint8_t newbutton=0; uint8_t newbutton = 0;
WRITE(SHIFT_LD,LOW); WRITE(SHIFT_LD, LOW);
WRITE(SHIFT_LD,HIGH); WRITE(SHIFT_LD, HIGH);
unsigned char tmp_buttons=0; unsigned char tmp_buttons = 0;
for(int8_t i=0;i<8;i++) for(int8_t i=0; i<8; i++) {
{ newbutton >>= 1;
newbutton = newbutton>>1; if (READ(SHIFT_OUT)) newbutton |= (1 << 7);
if(READ(SHIFT_OUT)) WRITE(SHIFT_CLK, HIGH);
newbutton|=(1<<7); WRITE(SHIFT_CLK, LOW);
WRITE(SHIFT_CLK,HIGH);
WRITE(SHIFT_CLK,LOW);
} }
buttons=~newbutton; //invert it, because a pressed switch produces a logical 0 buttons = ~newbutton; //invert it, because a pressed switch produces a logical 0
#endif//!NEWPANEL #endif //!NEWPANEL
//manage encoder rotation //manage encoder rotation
uint8_t enc=0; uint8_t enc=0;
if (buttons & EN_A) enc |= B01; if (buttons & EN_A) enc |= B01;
if (buttons & EN_B) enc |= B10; if (buttons & EN_B) enc |= B10;
if(enc != lastEncoderBits) if (enc != lastEncoderBits) {
{ switch(enc) {
switch(enc) case encrot0:
{ if (lastEncoderBits==encrot3) encoderDiff++;
case encrot0: else if (lastEncoderBits==encrot1) encoderDiff--;
if(lastEncoderBits==encrot3) break;
encoderDiff++; case encrot1:
else if(lastEncoderBits==encrot1) if (lastEncoderBits==encrot0) encoderDiff++;
encoderDiff--; else if (lastEncoderBits==encrot2) encoderDiff--;
break; break;
case encrot1: case encrot2:
if(lastEncoderBits==encrot0) if (lastEncoderBits==encrot1) encoderDiff++;
encoderDiff++; else if (lastEncoderBits==encrot3) encoderDiff--;
else if(lastEncoderBits==encrot2) break;
encoderDiff--; case encrot3:
break; if (lastEncoderBits==encrot2) encoderDiff++;
case encrot2: else if (lastEncoderBits==encrot0) encoderDiff--;
if(lastEncoderBits==encrot1) break;
encoderDiff++;
else if(lastEncoderBits==encrot3)
encoderDiff--;
break;
case encrot3:
if(lastEncoderBits==encrot2)
encoderDiff++;
else if(lastEncoderBits==encrot0)
encoderDiff--;
break;
}
} }
lastEncoderBits = enc; }
lastEncoderBits = enc;
} }
bool lcd_detected(void) bool lcd_detected(void) {
{ #if (defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008)) && defined(DETECT_DEVICE)
#if (defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008)) && defined(DETECT_DEVICE) return lcd.LcdDetected() == 1;
return lcd.LcdDetected() == 1; #else
#else return true;
return true; #endif
#endif
} }
void lcd_buzz(long duration, uint16_t freq) void lcd_buzz(long duration, uint16_t freq) {
{ #ifdef LCD_USE_I2C_BUZZER
#ifdef LCD_USE_I2C_BUZZER lcd.buzz(duration,freq);
lcd.buzz(duration,freq); #endif
#endif
} }
bool lcd_clicked() bool lcd_clicked() { return LCD_CLICKED; }
{
return LCD_CLICKED; #endif //ULTIPANEL
}
#endif//ULTIPANEL
/********************************/ /********************************/
/** Float conversion utilities **/ /** Float conversion utilities **/
@ -1772,24 +1744,4 @@ char *ftostr52(const float &x)
return conv; return conv;
} }
// Callback for after editing PID i value
// grab the PID i value out of the temp variable; scale it; then update the PID driver
void copy_and_scalePID_i()
{
#ifdef PIDTEMP
PID_PARAM(Ki, pid_current_extruder) = scalePID_i(raw_Ki);
updatePID();
#endif
}
// Callback for after editing PID d value
// grab the PID d value out of the temp variable; scale it; then update the PID driver
void copy_and_scalePID_d()
{
#ifdef PIDTEMP
PID_PARAM(Kd, pid_current_extruder) = scalePID_d(raw_Kd);
updatePID();
#endif
}
#endif //ULTRA_LCD #endif //ULTRA_LCD

View file

@ -4,7 +4,8 @@
#include "Marlin.h" #include "Marlin.h"
#ifdef ULTRA_LCD #ifdef ULTRA_LCD
int lcd_strlen(char *s);
int lcd_strlen_P(const char *s);
void lcd_update(); void lcd_update();
void lcd_init(); void lcd_init();
void lcd_setstatus(const char* message); void lcd_setstatus(const char* message);

View file

@ -123,17 +123,6 @@
#define LCD_CLICKED (buttons&(B_MI|B_ST)) #define LCD_CLICKED (buttons&(B_MI|B_ST))
#endif #endif
////////////////////////
// Setup Rotary Encoder Bit Values (for two pin encoders to indicate movement)
// These values are independent of which pins are used for EN_A and EN_B indications
// The rotary encoder part is also independent to the chipset used for the LCD
#if defined(EN_A) && defined(EN_B)
#define encrot0 0
#define encrot1 2
#define encrot2 3
#define encrot3 1
#endif
#endif //ULTIPANEL #endif //ULTIPANEL
//////////////////////////////////// ////////////////////////////////////
@ -636,7 +625,7 @@ static void lcd_implementation_drawmenu_generic(uint8_t row, const char* pstr, c
{ {
lcd.print(c); lcd.print(c);
pstr++; pstr++;
n--; if ((pgm_read_byte(pstr) & 0xc0) != 0x80) n--;
} }
while(n--) while(n--)
lcd.print(' '); lcd.print(' ');
@ -648,9 +637,9 @@ static void lcd_implementation_drawmenu_setting_edit_generic(uint8_t row, const
char c; char c;
//Use all characters in narrow LCDs //Use all characters in narrow LCDs
#if LCD_WIDTH < 20 #if LCD_WIDTH < 20
uint8_t n = LCD_WIDTH - 1 - 1 - strlen(data); uint8_t n = LCD_WIDTH - 1 - 1 - lcd_strlen(data);
#else #else
uint8_t n = LCD_WIDTH - 1 - 2 - strlen(data); uint8_t n = LCD_WIDTH - 1 - 2 - lcd_strlen(data);
#endif #endif
lcd.setCursor(0, row); lcd.setCursor(0, row);
lcd.print(pre_char); lcd.print(pre_char);
@ -658,7 +647,7 @@ static void lcd_implementation_drawmenu_setting_edit_generic(uint8_t row, const
{ {
lcd.print(c); lcd.print(c);
pstr++; pstr++;
n--; if ((pgm_read_byte(pstr) & 0xc0) != 0x80) n--;
} }
lcd.print(':'); lcd.print(':');
while(n--) while(n--)
@ -670,9 +659,9 @@ static void lcd_implementation_drawmenu_setting_edit_generic_P(uint8_t row, cons
char c; char c;
//Use all characters in narrow LCDs //Use all characters in narrow LCDs
#if LCD_WIDTH < 20 #if LCD_WIDTH < 20
uint8_t n = LCD_WIDTH - 1 - 1 - strlen_P(data); uint8_t n = LCD_WIDTH - 1 - 1 - lcd_strlen_P(data);
#else #else
uint8_t n = LCD_WIDTH - 1 - 2 - strlen_P(data); uint8_t n = LCD_WIDTH - 1 - 2 - lcd_strlen_P(data);
#endif #endif
lcd.setCursor(0, row); lcd.setCursor(0, row);
lcd.print(pre_char); lcd.print(pre_char);
@ -680,7 +669,7 @@ static void lcd_implementation_drawmenu_setting_edit_generic_P(uint8_t row, cons
{ {
lcd.print(c); lcd.print(c);
pstr++; pstr++;
n--; if ((pgm_read_byte(pstr) & 0xc0) != 0x80) n--;
} }
lcd.print(':'); lcd.print(':');
while(n--) while(n--)
@ -733,9 +722,9 @@ void lcd_implementation_drawedit(const char* pstr, char* value)
lcd_printPGM(pstr); lcd_printPGM(pstr);
lcd.print(':'); lcd.print(':');
#if LCD_WIDTH < 20 #if LCD_WIDTH < 20
lcd.setCursor(LCD_WIDTH - strlen(value), 1); lcd.setCursor(LCD_WIDTH - lcd_strlen(value), 1);
#else #else
lcd.setCursor(LCD_WIDTH -1 - strlen(value), 1); lcd.setCursor(LCD_WIDTH -1 - lcd_strlen(value), 1);
#endif #endif
lcd.print(value); lcd.print(value);
} }
@ -832,32 +821,28 @@ static void lcd_implementation_drawmenu_sddirectory(uint8_t row, const char* pst
static void lcd_implementation_quick_feedback() static void lcd_implementation_quick_feedback()
{ {
#ifdef LCD_USE_I2C_BUZZER #ifdef LCD_USE_I2C_BUZZER
#if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS) #if defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS) && defined(LCD_FEEDBACK_FREQUENCY_HZ)
lcd_buzz(1000/6,100); lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
#else
lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS,LCD_FEEDBACK_FREQUENCY_HZ);
#endif
#elif defined(BEEPER) && BEEPER > -1
SET_OUTPUT(BEEPER);
#if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
for(int8_t i=0;i<10;i++)
{
WRITE(BEEPER,HIGH);
delayMicroseconds(100);
WRITE(BEEPER,LOW);
delayMicroseconds(100);
}
#else #else
for(int8_t i=0;i<(LCD_FEEDBACK_FREQUENCY_DURATION_MS / (1000 / LCD_FEEDBACK_FREQUENCY_HZ));i++) lcd_buzz(1000/6, 100);
{
WRITE(BEEPER,HIGH);
delayMicroseconds(1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2);
WRITE(BEEPER,LOW);
delayMicroseconds(1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2);
}
#endif #endif
#endif #elif defined(BEEPER) && BEEPER > -1
SET_OUTPUT(BEEPER);
#if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
const unsigned int delay = 100;
uint8_t i = 10;
#else
const unsigned int delay = 1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2;
int8_t i = LCD_FEEDBACK_FREQUENCY_DURATION_MS * LCD_FEEDBACK_FREQUENCY_HZ / 1000;
#endif
while (i--) {
WRITE(BEEPER,HIGH);
delayMicroseconds(delay);
WRITE(BEEPER,LOW);
delayMicroseconds(delay);
}
#endif
} }
#ifdef LCD_HAS_STATUS_INDICATORS #ifdef LCD_HAS_STATUS_INDICATORS

View file

@ -47,12 +47,9 @@ uint8_t u8g_dev_rrd_st7920_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, vo
{ {
case U8G_DEV_MSG_INIT: case U8G_DEV_MSG_INIT:
{ {
SET_OUTPUT(ST7920_CS_PIN); OUT_WRITE(ST7920_CS_PIN,LOW);
WRITE(ST7920_CS_PIN,0); OUT_WRITE(ST7920_DAT_PIN,LOW);
SET_OUTPUT(ST7920_DAT_PIN); OUT_WRITE(ST7920_CLK_PIN,HIGH);
WRITE(ST7920_DAT_PIN,0);
SET_OUTPUT(ST7920_CLK_PIN);
WRITE(ST7920_CLK_PIN,1);
ST7920_CS(); ST7920_CS();
u8g_Delay(120); //initial delay for boot up u8g_Delay(120); //initial delay for boot up