/**

* Marlin 3D Printer Firmware

* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]

*

* Based on Sprinter and grbl.

* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm

*

* This program is free software: you can redistribute it and/or modify

* it under the terms of the GNU General Public License as published by

* the Free Software Foundation, either version 3 of the License, or

* (at your option) any later version.

*

* This program is distributed in the hope that it will be useful,

* but WITHOUT ANY WARRANTY; without even the implied warranty of

* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

* GNU General Public License for more details.

*

* You should have received a copy of the GNU General Public License

* along with this program. If not, see <http://www.gnu.org/licenses/>.

*

*/

/**

*

* About Marlin

*

* This firmware is a mashup between Sprinter and grbl.

* - https://github.com/kliment/Sprinter

* - https://github.com/simen/grbl/tree

*

* It has preliminary support for Matthew Roberts advance algorithm

* - http://reprap.org/pipermail/reprap-dev/2011-May/003323.html

*/

#include "Marlin.h"

#if ENABLED(AUTO_BED_LEVELING_FEATURE)

#include "vector_3.h"

#if ENABLED(AUTO_BED_LEVELING_GRID)

#include "qr_solve.h"

#endif

#endif // AUTO_BED_LEVELING_FEATURE

#if ENABLED(MESH_BED_LEVELING)

#include "mesh_bed_leveling.h"

#endif

#if ENABLED(BEZIER_CURVE_SUPPORT)

#include "planner_bezier.h"

#endif

#include "ultralcd.h"

#include "planner.h"

#include "stepper.h"

#include "endstops.h"

#include "temperature.h"

#include "cardreader.h"

#include "configuration_store.h"

#include "language.h"

#include "pins_arduino.h"

#include "math.h"

#include "nozzle.h"

#include "duration_t.h"

#include "types.h"

#if ENABLED(USE_WATCHDOG)

#include "watchdog.h"

#endif

#if ENABLED(BLINKM)

#include "blinkm.h"

#include "Wire.h"

#endif

#if HAS_SERVOS

#include "servo.h"

#endif

#if HAS_DIGIPOTSS

#include <SPI.h>

#endif

#if ENABLED(DAC_STEPPER_CURRENT)

#include "stepper_dac.h"

#endif

#if ENABLED(EXPERIMENTAL_I2CBUS)

#include "twibus.h"

#endif

#ifdef UARM_SWIFT

extern HardwareSerial* CommSerial;

#endif // UARM_SWIFT

/**

* Look here for descriptions of G-codes:

* - http://linuxcnc.org/handbook/gcode/g-code.html

* - http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes

*

* Help us document these G-codes online:

* - https://github.com/MarlinFirmware/Marlin/wiki/G-Code-in-Marlin

* - http://reprap.org/wiki/G-code

*

* -----------------

* Implemented Codes

* -----------------

*

* "G" Codes

*

* G0 -> G1

* G1 - Coordinated Movement X Y Z E

* G2 - CW ARC

* G3 - CCW ARC

* G4 - Dwell S<seconds> or P<milliseconds>

* G5 - Cubic B-spline with XYZE destination and IJPQ offsets

* G10 - Retract filament according to settings of M207

* G11 - Retract recover filament according to settings of M208

* G12 - Clean tool

* G20 - Set input units to inches

* G21 - Set input units to millimeters

* G28 - Home one or more axes

* G29 - Detailed Z probe, probes the bed at 3 or more points. Will fail if you haven't homed yet.

* G30 - Single Z probe, probes bed at current XY location.

* G31 - Dock sled (Z_PROBE_SLED only)

* G32 - Undock sled (Z_PROBE_SLED only)

* G90 - Use Absolute Coordinates

* G91 - Use Relative Coordinates

* G92 - Set current position to coordinates given

*

* "M" Codes

*

* M0 - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)

* M1 - Same as M0

* M17 - Enable/Power all stepper motors

* M18 - Disable all stepper motors; same as M84

* M20 - List SD card

* M21 - Init SD card

* M22 - Release SD card

* M23 - Select SD file (M23 filename.g)

* M24 - Start/resume SD print

* M25 - Pause SD print

* M26 - Set SD position in bytes (M26 S12345)

* M27 - Report SD print status

* M28 - Start SD write (M28 filename.g)

* M29 - Stop SD write

* M30 - Delete file from SD (M30 filename.g)

* M31 - Output time since last M109 or SD card start to serial

* M32 - Select file and start SD print (Can be used _while_ printing from SD card files):

* syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"

* Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).

* The '#' is necessary when calling from within sd files, as it stops buffer prereading

* M33 - Get the longname version of a path

* M42 - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.

* M48 - Measure Z_Probe repeatability. M48 [P # of points] [X position] [Y position] [V_erboseness #] [E_ngage Probe] [L # of legs of travel]

* M75 - Start the print job timer

* M76 - Pause the print job timer

* M77 - Stop the print job timer

* M78 - Show statistical information about the print jobs

* M80 - Turn on Power Supply

* M81 - Turn off Power Supply

* M82 - Set E codes absolute (default)

* M83 - Set E codes relative while in Absolute Coordinates (G90) mode

* M84 - Disable steppers until next move,

* or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout.

* M85 - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)

* M92 - Set planner.axis_steps_per_mm - same syntax as G92

* M104 - Set extruder target temp

* M105 - Read current temp

* M106 - Fan on

* M107 - Fan off

* M108 - Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.

* M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating

* Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling

* IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F

* M110 - Set the current line number

* M111 - Set debug flags with S<mask>. See flag bits defined in enum.h.

* M112 - Emergency stop

* M113 - Get or set the timeout interval for Host Keepalive "busy" messages

* M114 - Output current position to serial port

* M115 - Capabilities string

* M117 - Display a message on the controller screen

* M119 - Output Endstop status to serial port

* M120 - Enable endstop detection

* M121 - Disable endstop detection

* M126 - Solenoid Air Valve Open (BariCUDA support 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)

* M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)

* M140 - Set bed target temp

* M145 - Set the heatup state H<hotend> B<bed> F<fan speed> for S<material> (0=PLA, 1=ABS)

* M149 - Set temperature units

* M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.

* M163 - Set a single proportion for a mixing extruder. Requires MIXING_EXTRUDER.

* M164 - Save the mix as a virtual extruder. Requires MIXING_EXTRUDER and MIXING_VIRTUAL_TOOLS.

* M165 - Set the proportions for a mixing extruder. Use parameters ABCDHI to set the mixing factors. Requires MIXING_EXTRUDER.

* 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

* M200 - Set filament diameter, D<diameter>, setting E axis units to cubic. (Use S0 to revert to linear units.)

* M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)

* M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!

* M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec

* M204 - Set default acceleration: P for Printing moves, R for Retract only (no X, Y, Z) moves and T for Travel (non printing) moves (ex. M204 P800 T3000 R9000) in units/sec^2

* M205 - Set advanced settings. Current units apply:

S<print> T<travel> minimum speeds

B<minimum segment time>

X<max xy jerk>, Z<max Z jerk>, E<max E jerk>

* M206 - Set additional homing offset

* M207 - Set Retract Length: S<length>, Feedrate: F<units/min>, and Z lift: Z<distance>

* M208 - Set Recover (unretract) Additional (!) Length: S<length> and Feedrate: F<units/min>

* M209 - Turn Automatic Retract Detection on/off: S<bool> (For slicers that don't support G10/11).

Every normal extrude-only move will be classified as retract depending on the direction.

* M218 - Set a tool offset: T<index> X<offset> Y<offset>

* M220 - Set Feedrate Percentage: S<percent> ("FR" on your LCD)

* M221 - Set Flow Percentage: S<percent>

* M226 - Wait until the specified pin reaches the state required: P<pin number> S<pin state>

* M240 - Trigger a camera to take a photograph

* M250 - Set LCD contrast C<contrast value> (value 0..63)

* M280 - Set servo position absolute. P: servo index, S: angle or microseconds

* M300 - Play beep sound S<frequency Hz> P<duration ms>

* M301 - Set PID parameters P I and D

* 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)

* M304 - Set bed PID parameters P I and D

* M380 - Activate solenoid on active extruder

* M381 - Disable all solenoids

* M400 - Finish all moves

* M401 - Lower Z probe if present

* M402 - Raise Z probe if present

* M404 - Display or set the Nominal Filament Width: [ N<diameter> ]

* M405 - Enable Filament Sensor extrusion control. Optional delay between sensor and extruder: D<cm>

* M406 - Disable Filament Sensor extrusion control

* M407 - Display measured filament diameter in millimeters

* M410 - Quickstop. Abort all the planned moves

* M420 - Enable/Disable Mesh Leveling (with current values) S1=enable S0=disable

* M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units>

* M428 - Set the home_offset logically based on the current_position

* M500 - Store parameters in EEPROM

* M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).

* M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.

* M503 - Print the current settings (from memory not from EEPROM). Use S0 to leave off headings.

* M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)

* M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]

* M665 - Set delta configurations: L<diagonal rod> R<delta radius> S<segments/s>

* M666 - Set delta endstop adjustment

* M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]

* M851 - Set Z probe's Z offset in current units. (Negative values apply to probes that extend below the nozzle.)

* M907 - Set digital trimpot motor current using axis codes.

* M908 - Control digital trimpot directly.

* M909 - DAC_STEPPER_CURRENT: Print digipot/DAC current value

* M910 - DAC_STEPPER_CURRENT: Commit digipot/DAC value to external EEPROM via I2C

* M350 - Set microstepping mode.

* M351 - Toggle MS1 MS2 pins directly.

*

* ************ SCARA Specific - This can change to suit future G-code regulations

* M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)

* M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)

* M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)

* M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)

* M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)

* M365 - SCARA calibration: Scaling factor, X, Y, Z axis

* ************* SCARA End ***************

*

* ************ Custom codes - This can change to suit future G-code regulations

* M100 - Watch Free Memory (For Debugging Only)

* M928 - Start SD logging (M928 filename.g) - ended by M29

* M999 - Restart after being stopped by error

*

* "T" Codes

*

* T0-T3 - Select a tool by index (usually an extruder) [ F<units/min> ]

*

*/

#if ENABLED(M100_FREE_MEMORY_WATCHER)

void gcode_M100();

#endif

#if ENABLED(SDSUPPORT)

CardReader card;

#endif

#if ENABLED(EXPERIMENTAL_I2CBUS)

TWIBus i2c;

#endif

bool Running = true;

uint8_t marlin_debug_flags = DEBUG_NONE;

float current_position[NUM_AXIS] = { 0.0 };

#ifdef UARM_SWIFT

float destination[NUM_AXIS] = { 0.0 };

const char polor_axis_codes[NUM_AXIS] = {'S', 'R', 'H', 'E'};

float feedrate_mm_m = 1500.0, saved_feedrate_mm_m;

#else

static float destination[NUM_AXIS] = { 0.0 };

static float feedrate_mm_m = 1500.0, saved_feedrate_mm_m;

#endif

bool axis_known_position[3] = { false };

bool axis_homed[3] = { false };

static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;

static char command_queue[BUFSIZE][MAX_CMD_SIZE];

static char* current_command, *current_command_args;

static uint8_t cmd_queue_index_r = 0,

cmd_queue_index_w = 0,

commands_in_queue = 0;

#if ENABLED(INCH_MODE_SUPPORT)

float linear_unit_factor = 1.0;

float volumetric_unit_factor = 1.0;

#endif

#if ENABLED(TEMPERATURE_UNITS_SUPPORT)

TempUnit input_temp_units = TEMPUNIT_C;

#endif

/**

* Feed rates are often configured with mm/m

* but the planner and stepper like mm/s units.

*/

const float homing_feedrate_mm_m[] = {

#if ENABLED(DELTA)

HOMING_FEEDRATE_Z, HOMING_FEEDRATE_Z,

#else

HOMING_FEEDRATE_XY, HOMING_FEEDRATE_XY,

#endif

HOMING_FEEDRATE_Z, 0

};

int feedrate_percentage = 100, saved_feedrate_percentage;

bool axis_relative_modes[] = AXIS_RELATIVE_MODES;

int extruder_multiplier[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100);

bool volumetric_enabled = false;

float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(DEFAULT_NOMINAL_FILAMENT_DIA);

float volumetric_multiplier[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(1.0);

// The distance that XYZ has been offset by G92. Reset by G28.

float position_shift[3] = { 0 };

// This offset is added to the configured home position.

// Set by M206, M428, or menu item. Saved to EEPROM.

float home_offset[3] = { 0 };

// Software Endstops. Default to configured limits.

float sw_endstop_min[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };

float sw_endstop_max[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };

#if FAN_COUNT > 0

int fanSpeeds[FAN_COUNT] = { 0 };

#endif

// The active extruder (tool). Set with T<extruder> command.

uint8_t active_extruder = 0;

// Relative Mode. Enable with G91, disable with G90.

static bool relative_mode = false;

volatile bool wait_for_heatup = true;

const char errormagic[] PROGMEM = "Error:";

const char echomagic[] PROGMEM = "echo:";

const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};

static int serial_count = 0;

// GCode parameter pointer used by code_seen(), code_value_float(), etc.

static char* seen_pointer;

// Next Immediate GCode Command pointer. NULL if none.

const char* queued_commands_P = NULL;

const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42

// Inactivity shutdown

millis_t previous_cmd_ms = 0;

static millis_t max_inactive_time = 0;

static millis_t stepper_inactive_time = (DEFAULT_STEPPER_DEACTIVE_TIME) * 1000UL;

// Print Job Timer

#if ENABLED(PRINTCOUNTER)

PrintCounter print_job_timer = PrintCounter();

#else

Stopwatch print_job_timer = Stopwatch();

#endif

// Buzzer - I2C on the LCD or a BEEPER_PIN

#if ENABLED(LCD_USE_I2C_BUZZER)

#define BUZZ(d,f) lcd_buzz(d, f)

#elif HAS_BUZZER

Buzzer buzzer;

#define BUZZ(d,f) buzzer.tone(d, f)

#else

#define BUZZ(d,f) NOOP

#endif

static uint8_t target_extruder;

#if HAS_BED_PROBE

float zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER;

#endif

#define PLANNER_XY_FEEDRATE() (min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))

#if ENABLED(AUTO_BED_LEVELING_FEATURE)

int xy_probe_feedrate_mm_m = XY_PROBE_SPEED;

bool bed_leveling_in_progress = false;

#define XY_PROBE_FEEDRATE_MM_M xy_probe_feedrate_mm_m

#elif defined(XY_PROBE_SPEED)

#define XY_PROBE_FEEDRATE_MM_M XY_PROBE_SPEED

#else

#define XY_PROBE_FEEDRATE_MM_M MMS_TO_MMM(PLANNER_XY_FEEDRATE())

#endif

#if ENABLED(Z_DUAL_ENDSTOPS) && DISABLED(DELTA)

float z_endstop_adj = 0;

#endif

// Extruder offsets

#if HOTENDS > 1

float hotend_offset[][HOTENDS] = {

HOTEND_OFFSET_X,

HOTEND_OFFSET_Y

#ifdef HOTEND_OFFSET_Z

, HOTEND_OFFSET_Z

#endif

};

#endif

#if HAS_Z_SERVO_ENDSTOP

const int z_servo_angle[2] = Z_SERVO_ANGLES;

#endif

#if ENABLED(BARICUDA)

int baricuda_valve_pressure = 0;

int baricuda_e_to_p_pressure = 0;

#endif

#if ENABLED(FWRETRACT)

bool autoretract_enabled = false;

bool retracted[EXTRUDERS] = { false };

bool retracted_swap[EXTRUDERS] = { false };

float retract_length = RETRACT_LENGTH;

float retract_length_swap = RETRACT_LENGTH_SWAP;

float retract_feedrate_mm_s = RETRACT_FEEDRATE;

float retract_zlift = RETRACT_ZLIFT;

float retract_recover_length = RETRACT_RECOVER_LENGTH;

float retract_recover_length_swap = RETRACT_RECOVER_LENGTH_SWAP;

float retract_recover_feedrate_mm_s = RETRACT_RECOVER_FEEDRATE;

#endif // FWRETRACT

#if ENABLED(ULTIPANEL) && HAS_POWER_SWITCH

bool powersupply =

#if ENABLED(PS_DEFAULT_OFF)

false

#else

true

#endif

;

#endif

#if ENABLED(DELTA)

#define TOWER_1 X_AXIS

#define TOWER_2 Y_AXIS

#define TOWER_3 Z_AXIS

float delta[3];

float cartesian_position[3] = { 0 };

#define SIN_60 0.8660254037844386

#define COS_60 0.5

float endstop_adj[3] = { 0 };

// these are the default values, can be overriden with M665

float delta_radius = DELTA_RADIUS;

float delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower

float delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1);

float delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); // front right tower

float delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2);

float delta_tower3_x = 0; // back middle tower

float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3);

float delta_diagonal_rod = DELTA_DIAGONAL_ROD;

float delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1;

float delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2;

float delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3;

float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1);

float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2);

float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3);

float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;

float delta_clip_start_height = Z_MAX_POS;

#if ENABLED(AUTO_BED_LEVELING_FEATURE)

int delta_grid_spacing[2] = { 0, 0 };

float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];

#endif

float delta_safe_distance_from_top();

#else

static bool home_all_axis = true;

#endif

#if ENABLED(SCARA)

float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;

float delta[3];

float axis_scaling[3] = { 1, 1, 1 }; // Build size scaling, default to 1

#endif

#if ENABLED(FILAMENT_WIDTH_SENSOR)

//Variables for Filament Sensor input

float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA; //Set nominal filament width, can be changed with M404

bool filament_sensor = false; //M405 turns on filament_sensor control, M406 turns it off

float filament_width_meas = DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter

int8_t measurement_delay[MAX_MEASUREMENT_DELAY + 1]; //ring buffer to delay measurement store extruder factor after subtracting 100

int filwidth_delay_index1 = 0; //index into ring buffer

int filwidth_delay_index2 = -1; //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized

int meas_delay_cm = MEASUREMENT_DELAY_CM; //distance delay setting

#endif

#if ENABLED(FILAMENT_RUNOUT_SENSOR)

static bool filament_ran_out = false;

#endif

#if ENABLED(FILAMENT_CHANGE_FEATURE)

FilamentChangeMenuResponse filament_change_menu_response;

#endif

#if ENABLED(MIXING_EXTRUDER)

float mixing_factor[MIXING_STEPPERS];

#if MIXING_VIRTUAL_TOOLS > 1

float mixing_virtual_tool_mix[MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS];

#endif

#endif

static bool send_ok[BUFSIZE];

#if HAS_SERVOS

Servo servo[NUM_SERVOS];

#define MOVE_SERVO(I, P) servo[I].move(P)

#if HAS_Z_SERVO_ENDSTOP

#define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])

#define STOW_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[1])

#endif

#endif

#ifdef CHDK

millis_t chdkHigh = 0;

boolean chdkActive = false;

#endif

#if ENABLED(PID_EXTRUSION_SCALING)

int lpq_len = 20;

#endif

#if ENABLED(HOST_KEEPALIVE_FEATURE)

static MarlinBusyState busy_state = NOT_BUSY;

static millis_t next_busy_signal_ms = 0;

uint8_t host_keepalive_interval = DEFAULT_KEEPALIVE_INTERVAL;

#define KEEPALIVE_STATE(n) do{ busy_state = n; }while(0)

#else

#define host_keepalive() ;

#define KEEPALIVE_STATE(n) ;

#endif // HOST_KEEPALIVE_FEATURE

/**

* ***************************************************************************

* ******************************** FUNCTIONS ********************************

* ***************************************************************************

*/

void stop();

void get_available_commands();

void process_next_command();

void prepare_move_to_destination();

void set_current_from_steppers_for_axis(AxisEnum axis);

#if ENABLED(ARC_SUPPORT)

void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);

#endif

#if ENABLED(BEZIER_CURVE_SUPPORT)

void plan_cubic_move(const float offset[4]);

#endif

void serial_echopair_P(const char* s_P, char v) { serialprintPGM(s_P); SERIAL_CHAR(v); }

void serial_echopair_P(const char* s_P, int v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

void serial_echopair_P(const char* s_P, long v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

void serial_echopair_P(const char* s_P, float v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

void serial_echopair_P(const char* s_P, double v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

void serial_echopair_P(const char* s_P, unsigned long v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

void tool_change(const uint8_t tmp_extruder, const float fr_mm_m=0.0, bool no_move=false);

static void report_current_position();

#if ENABLED(DEBUG_LEVELING_FEATURE)

void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {

serialprintPGM(prefix);

SERIAL_ECHOPAIR("(", x);

SERIAL_ECHOPAIR(", ", y);

SERIAL_ECHOPAIR(", ", z);

SERIAL_ECHOPGM(")");

if (suffix) serialprintPGM(suffix);

else SERIAL_EOL;

}

void print_xyz(const char* prefix, const char* suffix, const float xyz[]) {

print_xyz(prefix, suffix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);

}

#if ENABLED(AUTO_BED_LEVELING_FEATURE)

void print_xyz(const char* prefix, const char* suffix, const vector_3 &xyz) {

print_xyz(prefix, suffix, xyz.x, xyz.y, xyz.z);

}

#endif

#define DEBUG_POS(SUFFIX,VAR) do { \

print_xyz(PSTR(STRINGIFY(VAR) "="), PSTR(" : " SUFFIX "\n"), VAR); } while(0)

#endif

/**

* sync_plan_position

* Set planner / stepper positions to the cartesian current_position.

* The stepper code translates these coordinates into step units.

* Allows translation between steps and millimeters for cartesian & core robots

*/

inline void sync_plan_position() {

#if ENABLED(DEBUG_LEVELING_FEATURE)

if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);

#endif

planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

}

inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }

#if ENABLED(DELTA) || ENABLED(SCARA)

inline void sync_plan_position_delta() {

#if ENABLED(DEBUG_LEVELING_FEATURE)

if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);

#endif

inverse_kinematics(current_position);

planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);

}

#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()

#else

#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position()

#endif

#if ENABLED(SDSUPPORT)

#include "SdFatUtil.h"

int freeMemory() { return SdFatUtil::FreeRam(); }

#else

extern "C" {

extern unsigned int __bss_end;

extern unsigned int __heap_start;

extern void* __brkval;

int freeMemory() {

int free_memory;

if ((int)__brkval == 0)

free_memory = ((int)&free_memory) - ((int)&__bss_end);

else

free_memory = ((int)&free_memory) - ((int)__brkval);

return free_memory;

}

}

#endif //!SDSUPPORT

#if ENABLED(DIGIPOT_I2C)

extern void digipot_i2c_set_current(int channel, float current);

extern void digipot_i2c_init();

#endif

/**

* Inject the next "immediate" command, when possible.

* Return true if any immediate commands remain to inject.

*/

static bool drain_queued_commands_P() {

if (queued_commands_P != NULL) {

size_t i = 0;

char c, cmd[30];

strncpy_P(cmd, queued_commands_P, sizeof(cmd) - 1);

cmd[sizeof(cmd) - 1] = '\0';

while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command

cmd[i] = '\0';

if (enqueue_and_echo_command(cmd)) { // success?

if (c) // newline char?

queued_commands_P += i + 1; // advance to the next command

else

queued_commands_P = NULL; // nul char? no more commands

}

}

return (queued_commands_P != NULL); // return whether any more remain

}

/**

* Record one or many commands to run from program memory.

* Aborts the current queue, if any.

* Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards

*/

void enqueue_and_echo_commands_P(const char* pgcode) {

queued_commands_P = pgcode;

drain_queued_commands_P(); // first command executed asap (when possible)

}

void clear_command_queue() {

cmd_queue_index_r = cmd_queue_index_w;

commands_in_queue = 0;

}

/**

* Once a new command is in the ring buffer, call this to commit it

*/

inline void _commit_command(bool say_ok) {

send_ok[cmd_queue_index_w] = say_ok;

cmd_queue_index_w = (cmd_queue_index_w + 1) % BUFSIZE;

commands_in_queue++;

}

/**

* Copy a command directly into the main command buffer, from RAM.

* Returns true if successfully adds the command

*/

inline bool _enqueuecommand(const char* cmd, bool say_ok=false) {

if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;

strcpy(command_queue[cmd_queue_index_w], cmd);

_commit_command(say_ok);

return true;

}

void enqueue_and_echo_command_now(const char* cmd) {

while (!enqueue_and_echo_command(cmd)) idle();

}

/**

* Enqueue with Serial Echo

*/

bool enqueue_and_echo_command(const char* cmd, bool say_ok/*=false*/) {

if (_enqueuecommand(cmd, say_ok)) {

SERIAL_ECHO_START;

SERIAL_ECHOPGM(MSG_Enqueueing);

SERIAL_ECHO(cmd);

SERIAL_ECHOLNPGM("\"");

return true;

}

return false;

}

void setup_killpin() {

#if HAS_KILL

SET_INPUT(KILL_PIN);

WRITE(KILL_PIN, HIGH);

#endif

}

#if ENABLED(FILAMENT_RUNOUT_SENSOR)

void setup_filrunoutpin() {

pinMode(FIL_RUNOUT_PIN, INPUT);

#if ENABLED(ENDSTOPPULLUP_FIL_RUNOUT)

WRITE(FIL_RUNOUT_PIN, HIGH);

#endif

}

#endif

// Set home pin

void setup_homepin(void) {

#if HAS_HOME

SET_INPUT(HOME_PIN);

WRITE(HOME_PIN, HIGH);

#endif

}

void setup_photpin() {

#if HAS_PHOTOGRAPH

OUT_WRITE(PHOTOGRAPH_PIN, LOW);

#endif

}

void setup_powerhold() {

#if HAS_SUICIDE

OUT_WRITE(SUICIDE_PIN, HIGH);

#endif

#if HAS_POWER_SWITCH

#if ENABLED(PS_DEFAULT_OFF)

OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);

#else

OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE);

#endif

#endif

}

void suicide() {

#if HAS_SUICIDE

OUT_WRITE(SUICIDE_PIN, LOW);

#endif

}

void servo_init() {

#if NUM_SERVOS >= 1 && HAS_SERVO_0

servo[0].attach(SERVO0_PIN);

servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.

#endif

#if NUM_SERVOS >= 2 && HAS_SERVO_1

servo[1].attach(SERVO1_PIN);

servo[1].detach();

#endif

#if NUM_SERVOS >= 3 && HAS_SERVO_2

servo[2].attach(SERVO2_PIN);

servo[2].detach();

#endif

#if NUM_SERVOS >= 4 && HAS_SERVO_3

servo[3].attach(SERVO3_PIN);

servo[3].detach();

#endif

#if HAS_Z_SERVO_ENDSTOP

/**

* Set position of Z Servo Endstop

*

* The servo might be deployed and positioned too low to stow

* when starting up the machine or rebooting the board.

* There's no way to know where the nozzle is positioned until

* homing has been done - no homing with z-probe without init!

*

*/

STOW_Z_SERVO();

#endif

#if HAS_BED_PROBE

endstops.enable_z_probe(false);

#endif

}

/**

* Stepper Reset (RigidBoard, et.al.)

*/

#if HAS_STEPPER_RESET

void disableStepperDrivers() {

pinMode(STEPPER_RESET_PIN, OUTPUT);

digitalWrite(STEPPER_RESET_PIN, LOW); // drive it down to hold in reset motor driver chips

}

void enableStepperDrivers() { pinMode(STEPPER_RESET_PIN, INPUT); } // set to input, which allows it to be pulled high by pullups

#endif

/**

* Marlin entry-point: Set up before the program loop

* - Set up the kill pin, filament runout, power hold

* - Start the serial port

* - Print startup messages and diagnostics

* - Get EEPROM or default settings

* - Initialize managers for:

* �temperature

* �planner

* �watchdog

* �stepper

* �photo pin

* �servos

* �LCD controller

* �Digipot I2C

* �Z probe sled

* �status LEDs

*/

void setup() {

#ifdef DISABLE_JTAG

// Disable JTAG on AT90USB chips to free up pins for IO

MCUCR = 0x80;

MCUCR = 0x80;

#endif

#if ENABLED(FILAMENT_RUNOUT_SENSOR)

setup_filrunoutpin();

#endif

setup_killpin();

setup_powerhold();

#if HAS_STEPPER_RESET

disableStepperDrivers();

#endif

MYSERIAL.begin(BAUDRATE);

SERIAL_PROTOCOLLNPGM("start");

SERIAL_ECHO_START;

// Check startup - does nothing if bootloader sets MCUSR to 0

byte mcu = MCUSR;

if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);

if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);

if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);

if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);

if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);

MCUSR = 0;

SERIAL_ECHOPGM(MSG_MARLIN);

SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION);

#ifdef STRING_DISTRIBUTION_DATE

#ifdef STRING_CONFIG_H_AUTHOR

SERIAL_ECHO_START;

SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);

SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE);

SERIAL_ECHOPGM(MSG_AUTHOR);

SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);

SERIAL_ECHOPGM("Compiled: ");

SERIAL_ECHOLNPGM(__DATE__);

#endif // STRING_CONFIG_H_AUTHOR

#endif // STRING_DISTRIBUTION_DATE

SERIAL_ECHO_START;

SERIAL_ECHOPGM(MSG_FREE_MEMORY);

SERIAL_ECHO(freeMemory());

SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);

SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);

// Send "ok" after commands by default

for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true;

// Load data from EEPROM if available (or use defaults)

// This also updates variables in the planner, elsewhere

Config_RetrieveSettings();

// Initialize current position based on home_offset

memcpy(current_position, home_offset, sizeof(home_offset));

// Vital to init stepper/planner equivalent for current_position

SYNC_PLAN_POSITION_KINEMATIC();

thermalManager.init(); // Initialize temperature loop

#if ENABLED(USE_WATCHDOG)

watchdog_init();

#endif

stepper.init(); // Initialize stepper, this enables interrupts!

setup_photpin();

servo_init();

#if HAS_CONTROLLERFAN

SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan

#endif

#if HAS_STEPPER_RESET

enableStepperDrivers();

#endif

#if ENABLED(DIGIPOT_I2C)

digipot_i2c_init();

#endif

#if ENABLED(DAC_STEPPER_CURRENT)

dac_init();

#endif

#if ENABLED(Z_PROBE_SLED) && PIN_EXISTS(SLED)

pinMode(SLED_PIN, OUTPUT);

digitalWrite(SLED_PIN, LOW); // turn it off

#endif // Z_PROBE_SLED

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

lcd_init();

#if ENABLED(SHOW_BOOTSCREEN)

#if ENABLED(DOGLCD)

safe_delay(BOOTSCREEN_TIMEOUT);

#elif ENABLED(ULTRA_LCD)

bootscreen();

lcd_init();

#endif

#endif

#if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1

// Initialize mixing to 100% color 1

for (uint8_t i = 0; i < MIXING_STEPPERS; i++)

mixing_factor[i] = (i == 0) ? 1 : 0;

for (uint8_t t = 0; t < MIXING_VIRTUAL_TOOLS; t++)