// Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.

#include "fastio.h"

#include "Configuration.h"

#include "pins.h"

#include "Sprinter.h"

#define SERVO 1

#ifdef SDSUPPORT

#include "SdFat.h"

#endif

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

bool move_direction[NUM_AXIS];

unsigned long axis_previous_micros[NUM_AXIS];

unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater;

unsigned long move_steps_to_take[NUM_AXIS];

#ifdef RAMP_ACCELERATION

unsigned long axis_max_interval[NUM_AXIS];

unsigned long axis_steps_per_sqr_second[NUM_AXIS];

unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS];

unsigned long max_interval;

unsigned long steps_per_sqr_second, plateau_steps;

#endif

boolean acceleration_enabled = false, accelerating = false;

unsigned long interval;

float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};

float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};

unsigned long steps_taken[NUM_AXIS];

long axis_interval[NUM_AXIS]; // for speed delay

bool home_all_axis = true;

int feedrate = 1500, next_feedrate, saved_feedrate;

float time_for_move;

long gcode_N, gcode_LastN;

bool relative_mode = false; //Determines Absolute or Relative Coordinates

bool relative_mode_e = false; //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.

long timediff = 0;

float d = 0;

float axis_diff[NUM_AXIS] = {0, 0, 0, 0};

#ifdef STEP_DELAY_RATIO

long long_step_delay_ratio = STEP_DELAY_RATIO * 100;

#endif

#define MAX_CMD_SIZE 96

#define BUFSIZE 8

char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];

bool fromsd[BUFSIZE];

int bufindr = 0;

int bufindw = 0;

int buflen = 0;

int i = 0;

char serial_char;

int serial_count = 0;

boolean comment_mode = false;

char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc

int target_raw = 0;

int target_temp = 0;

int current_raw = 0;

int target_bed_raw = 0;

int current_bed_raw = 0;

int tt = 0, bt = 0;

#ifdef PIDTEMP

int temp_iState = 0;

int prev_temp = 0;

int pTerm;

int iTerm;

int dTerm;

//int output;

int error;

int heater_duty = 0;

const int temp_iState_min = 256L * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;

const int temp_iState_max = 256L * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN;

#endif

#ifndef HEATER_CURRENT

#define HEATER_CURRENT 255

#endif

#ifdef SMOOTHING

uint32_t nma = 0;

#endif

#ifdef WATCHPERIOD

int watch_raw = -1000;

unsigned long watchmillis = 0;

#endif

#ifdef MINTEMP

int minttemp = temp2analogh(MINTEMP);

#endif

#ifdef MAXTEMP

int maxttemp = temp2analogh(MAXTEMP);

#endif

unsigned long previous_millis_cmd = 0;

unsigned long max_inactive_time = 0;

unsigned long stepper_inactive_time = 0;

#ifdef SDSUPPORT

Sd2Card card;

SdVolume volume;

SdFile root;

SdFile file;

uint32_t filesize = 0;

uint32_t sdpos = 0;

bool sdmode = false;

bool sdactive = false;

bool savetosd = false;

int16_t n;

void initsd(){

sdactive = false;

#if SDSS >- 1

if(root.isOpen())

root.close();

if (!card.init(SPI_FULL_SPEED,SDSS)){

//if (!card.init(SPI_HALF_SPEED,SDSS))

Serial.println("SD init fail");

}

else if (!volume.init(&card))

Serial.println("volume.init failed");

else if (!root.openRoot(&volume))

Serial.println("openRoot failed");

else

sdactive = true;

#endif

}

inline void write_command(char *buf){

char* begin = buf;

char* npos = 0;

char* end = buf + strlen(buf) - 1;

file.writeError = false;

if((npos = strchr(buf, 'N')) != NULL){

begin = strchr(npos, ' ') + 1;

end = strchr(npos, '*') - 1;

}

end[1] = '\r';

end[2] = '\n';

end[3] = '\0';

//Serial.println(begin);

file.write(begin);

if (file.writeError){

Serial.println("error writing to file");

}

}

#endif

#ifdef SERVO

#define SERVOCOUNT 13

#include <Servo.h>

static Servo servos[SERVOCOUNT];

#endif

void setup()

{

Serial.begin(BAUDRATE);

Serial.println("start");

for(int i = 0; i < BUFSIZE; i++){

fromsd[i] = false;

}

//Initialize Dir Pins

#if X_DIR_PIN > -1

SET_OUTPUT(X_DIR_PIN);

#endif

#if Y_DIR_PIN > -1

SET_OUTPUT(Y_DIR_PIN);

#endif

#if Z_DIR_PIN > -1

SET_OUTPUT(Z_DIR_PIN);

#endif

#if E_DIR_PIN > -1

SET_OUTPUT(E_DIR_PIN);

#endif

//Initialize Enable Pins - steppers default to disabled.

#if (X_ENABLE_PIN > -1)

SET_OUTPUT(X_ENABLE_PIN);

if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);

#endif

#if (Y_ENABLE_PIN > -1)

SET_OUTPUT(Y_ENABLE_PIN);

if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);

#endif

#if (Z_ENABLE_PIN > -1)

SET_OUTPUT(Z_ENABLE_PIN);

if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);

#endif

#if (E_ENABLE_PIN > -1)

SET_OUTPUT(E_ENABLE_PIN);

if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH);

#endif

#ifdef CONTROLLERFAN_PIN

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

#endif

//endstops and pullups

#ifdef ENDSTOPPULLUPS

#if X_MIN_PIN > -1

SET_INPUT(X_MIN_PIN);

WRITE(X_MIN_PIN,HIGH);

#endif

#if X_MAX_PIN > -1

SET_INPUT(X_MAX_PIN);

WRITE(X_MAX_PIN,HIGH);

#endif

#if Y_MIN_PIN > -1

SET_INPUT(Y_MIN_PIN);

WRITE(Y_MIN_PIN,HIGH);

#endif

#if Y_MAX_PIN > -1

SET_INPUT(Y_MAX_PIN);

WRITE(Y_MAX_PIN,HIGH);

#endif

#if Z_MIN_PIN > -1

SET_INPUT(Z_MIN_PIN);

WRITE(Z_MIN_PIN,HIGH);

#endif

#if Z_MAX_PIN > -1

SET_INPUT(Z_MAX_PIN);

WRITE(Z_MAX_PIN,HIGH);

#endif

#else

#if X_MIN_PIN > -1

SET_INPUT(X_MIN_PIN);

#endif

#if X_MAX_PIN > -1

SET_INPUT(X_MAX_PIN);

#endif

#if Y_MIN_PIN > -1

SET_INPUT(Y_MIN_PIN);

#endif

#if Y_MAX_PIN > -1

SET_INPUT(Y_MAX_PIN);

#endif

#if Z_MIN_PIN > -1

SET_INPUT(Z_MIN_PIN);

#endif

#if Z_MAX_PIN > -1

SET_INPUT(Z_MAX_PIN);

#endif

#endif

#if (HEATER_0_PIN > -1)

SET_OUTPUT(HEATER_0_PIN);

WRITE(HEATER_0_PIN,LOW);

#endif

#if (HEATER_1_PIN > -1)

SET_OUTPUT(HEATER_1_PIN);

WRITE(HEATER_1_PIN,LOW);

#endif

//Initialize Fan Pin

#if (FAN_PIN > -1)

SET_OUTPUT(FAN_PIN);

#endif

//Initialize Alarm Pin

#if (ALARM_PIN > -1)

SET_OUTPUT(ALARM_PIN);

WRITE(ALARM_PIN,LOW);

#endif

//Initialize LED Pin

#if (LED_PIN > -1)

SET_OUTPUT(LED_PIN);

WRITE(LED_PIN,LOW);

#endif

#if (X_STEP_PIN > -1)

SET_OUTPUT(X_STEP_PIN);

#endif

#if (Y_STEP_PIN > -1)

SET_OUTPUT(Y_STEP_PIN);

#endif

#if (Z_STEP_PIN > -1)

SET_OUTPUT(Z_STEP_PIN);

#endif

#if (E_STEP_PIN > -1)

SET_OUTPUT(E_STEP_PIN);

#endif

#ifdef RAMP_ACCELERATION

setup_acceleration();

#endif

#ifdef HEATER_USES_MAX6675

SET_OUTPUT(SCK_PIN);

WRITE(SCK_PIN,0);

SET_OUTPUT(MOSI_PIN);

WRITE(MOSI_PIN,1);

SET_INPUT(MISO_PIN);

WRITE(MISO_PIN,1);

SET_OUTPUT(MAX6675_SS);

WRITE(MAX6675_SS,1);

#endif

#ifdef SDSUPPORT

//power to SD reader

#if SDPOWER > -1

SET_OUTPUT(SDPOWER);

WRITE(SDPOWER,HIGH);

#endif

initsd();

#endif

#ifdef SERVO

servos[0].attach(23);

servos[1].attach(25);

servos[2].attach(27);

servos[3].attach(29);

servos[4].attach(31);

servos[5].attach(33);

servos[6].attach(35);

servos[7].attach(37);

servos[8].attach(39);

servos[9].attach(41);

servos[10].attach(43);

servos[11].attach(45);

servos[12].attach(47);

int i;

for(i=0;i<12;++i)

servos[i].write(180);

servos[12].write(90);

#endif

}

void loop()

{

if(buflen<3)

get_command();

if(buflen){

#ifdef SDSUPPORT

if(savetosd){

if(strstr(cmdbuffer[bufindr],"M29") == NULL){

write_command(cmdbuffer[bufindr]);

Serial.println("ok");

}else{

file.sync();

file.close();

savetosd = false;

Serial.println("Done saving file.");

}

}else{

process_commands();

}

#else

process_commands();

#endif

buflen = (buflen-1);

bufindr = (bufindr + 1)%BUFSIZE;

}

//check heater every n milliseconds

manage_heater();

manage_inactivity(1);

}

inline void get_command()

{

while( Serial.available() > 0 && buflen < BUFSIZE) {

serial_char = Serial.read();

if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) )

{

if(!serial_count) return; //if empty line

cmdbuffer[bufindw][serial_count] = 0; //terminate string

if(!comment_mode){

fromsd[bufindw] = false;

if(strstr(cmdbuffer[bufindw], "N") != NULL)

{

strchr_pointer = strchr(cmdbuffer[bufindw], 'N');

gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));

if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) {

Serial.print("Serial Error: Line Number is not Last Line Number+1, Last Line:");

Serial.println(gcode_LastN);

//Serial.println(gcode_N);

FlushSerialRequestResend();

serial_count = 0;

return;

}

if(strstr(cmdbuffer[bufindw], "*") != NULL)

{

byte checksum = 0;

byte count = 0;

while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];

strchr_pointer = strchr(cmdbuffer[bufindw], '*');

if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {

Serial.print("Error: checksum mismatch, Last Line:");

Serial.println(gcode_LastN);

FlushSerialRequestResend();

serial_count = 0;

return;

}

//if no errors, continue parsing

}

else

{

Serial.print("Error: No Checksum with line number, Last Line:");

Serial.println(gcode_LastN);

FlushSerialRequestResend();

serial_count = 0;

return;

}

gcode_LastN = gcode_N;

//if no errors, continue parsing

}

else // if we don't receive 'N' but still see '*'

{

if((strstr(cmdbuffer[bufindw], "*") != NULL))

{

Serial.print("Error: No Line Number with checksum, Last Line:");

Serial.println(gcode_LastN);

serial_count = 0;

return;

}

}

if((strstr(cmdbuffer[bufindw], "G") != NULL)){

strchr_pointer = strchr(cmdbuffer[bufindw], 'G');

switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){

case 0:

case 1:

#ifdef SDSUPPORT

if(savetosd)

break;

#endif

Serial.println("ok");

break;

default:

break;

}

}

bufindw = (bufindw + 1)%BUFSIZE;

buflen += 1;

}

comment_mode = false; //for new command

serial_count = 0; //clear buffer

}

else

{

if(serial_char == ';') comment_mode = true;

if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;

}

}

#ifdef SDSUPPORT

if(!sdmode || serial_count!=0){

return;

}

while( filesize > sdpos && buflen < BUFSIZE) {

n = file.read();

serial_char = (char)n;

if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) || n == -1)

{

sdpos = file.curPosition();

if(sdpos >= filesize){

sdmode = false;

Serial.println("Done printing file");

}

if(!serial_count) return; //if empty line

cmdbuffer[bufindw][serial_count] = 0; //terminate string

if(!comment_mode){

fromsd[bufindw] = true;

buflen += 1;

bufindw = (bufindw + 1)%BUFSIZE;

}

comment_mode = false; //for new command

serial_count = 0; //clear buffer

}

else

{

if(serial_char == ';') comment_mode = true;

if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;

}

}

#endif

}

inline float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); }

inline long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); }

inline bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); } //Return True if the string was found

inline bool code_seen(char code)

{

strchr_pointer = strchr(cmdbuffer[bufindr], code);

return (strchr_pointer != NULL); //Return True if a character was found

}

inline void process_commands()

{

unsigned long codenum; //throw away variable

char *starpos = NULL;

if(code_seen('G'))

{

switch((int)code_value())

{

case 0: // G0 -> G1

case 1: // G1

#if (defined DISABLE_CHECK_DURING_ACC) || (defined DISABLE_CHECK_DURING_MOVE) || (defined DISABLE_CHECK_DURING_TRAVEL)

manage_heater();

#endif

get_coordinates(); // For X Y Z E F

prepare_move();

previous_millis_cmd = millis();

//ClearToSend();

return;

//break;

case 4: // G4 dwell

codenum = 0;

if(code_seen('P')) codenum = code_value(); // milliseconds to wait

if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait

codenum += millis(); // keep track of when we started waiting

while(millis() < codenum ){

manage_heater();

}

break;

case 28: //G28 Home all Axis one at a time

saved_feedrate = feedrate;

for(int i=0; i < NUM_AXIS; i++) {

destination[i] = current_position[i];

}

feedrate = 0;

home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));

if((home_all_axis) || (code_seen(axis_codes[0]))) {

if ((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)){

current_position[0] = 0;

destination[0] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;

feedrate = homing_feedrate[0];

prepare_move();

current_position[0] = 0;

destination[0] = -5 * X_HOME_DIR;

prepare_move();

destination[0] = 10 * X_HOME_DIR;

prepare_move();

current_position[0] = (X_HOME_DIR == -1) ? 0 : X_MAX_LENGTH;

destination[0] = current_position[0];

feedrate = 0;

}

}

if((home_all_axis) || (code_seen(axis_codes[1]))) {

if ((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)){

current_position[1] = 0;

destination[1] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;

feedrate = homing_feedrate[1];

prepare_move();

current_position[1] = 0;

destination[1] = -5 * Y_HOME_DIR;

prepare_move();

destination[1] = 10 * Y_HOME_DIR;

prepare_move();

current_position[1] = (Y_HOME_DIR == -1) ? 0 : Y_MAX_LENGTH;

destination[1] = current_position[1];

feedrate = 0;

}

}

if((home_all_axis) || (code_seen(axis_codes[2]))) {

if ((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)){

current_position[2] = 0;

destination[2] = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR;

feedrate = homing_feedrate[2];

prepare_move();

current_position[2] = 0;

destination[2] = -2 * Z_HOME_DIR;

prepare_move();

destination[2] = 10 * Z_HOME_DIR;

prepare_move();

current_position[2] = (Z_HOME_DIR == -1) ? 0 : Z_MAX_LENGTH;

destination[2] = current_position[2];

feedrate = 0;

}

}

feedrate = saved_feedrate;

previous_millis_cmd = millis();

break;

case 90: // G90

relative_mode = false;

break;

case 91: // G91

relative_mode = true;

break;

case 92: // G92

for(int i=0; i < NUM_AXIS; i++) {

if(code_seen(axis_codes[i])) current_position[i] = code_value();

}

break;

}

}

else if(code_seen('M'))

{

switch( (int)code_value() )

{

#ifdef SDSUPPORT

case 20: // M20 - list SD card

Serial.println("Begin file list");

root.ls();

Serial.println("End file list");

break;

case 21: // M21 - init SD card

sdmode = false;

initsd();

break;

case 22: //M22 - release SD card

sdmode = false;

sdactive = false;

break;

case 23: //M23 - Select file

if(sdactive){

sdmode = false;

file.close();

starpos = (strchr(strchr_pointer + 4,'*'));

if(starpos!=NULL)

*(starpos-1)='\0';

if (file.open(&root, strchr_pointer + 4, O_READ)) {

Serial.print("File opened:");

Serial.print(strchr_pointer + 4);

Serial.print(" Size:");

Serial.println(file.fileSize());

sdpos = 0;

filesize = file.fileSize();

Serial.println("File selected");

}

else{

Serial.println("file.open failed");

}

}

break;

case 24: //M24 - Start SD print

if(sdactive){

sdmode = true;

}

break;

case 25: //M25 - Pause SD print

if(sdmode){

sdmode = false;

}

break;

case 26: //M26 - Set SD index

if(sdactive && code_seen('S')){

sdpos = code_value_long();

file.seekSet(sdpos);

}

break;

case 27: //M27 - Get SD status

if(sdactive){

Serial.print("SD printing byte ");

Serial.print(sdpos);

Serial.print("/");

Serial.println(filesize);

}else{

Serial.println("Not SD printing");

}

break;

case 28: //M28 - Start SD write

if(sdactive){

char* npos = 0;

file.close();

sdmode = false;

starpos = (strchr(strchr_pointer + 4,'*'));

if(starpos != NULL){

npos = strchr(cmdbuffer[bufindr], 'N');

strchr_pointer = strchr(npos,' ') + 1;

*(starpos-1) = '\0';

}

if (!file.open(&root, strchr_pointer+4, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))

{

Serial.print("open failed, File: ");

Serial.print(strchr_pointer + 4);

Serial.print(".");

}else{

savetosd = true;

Serial.print("Writing to file: ");

Serial.println(strchr_pointer + 4);

}

}

break;

case 29: //M29 - Stop SD write

//processed in write to file routine above

//savetosd = false;

break;

#endif

case 42: //M42 -Change pin status via gcode

if (code_seen('S'))

{

int pin_status = code_value();

if (code_seen('P') && pin_status >= 0 && pin_status <= 255)

{

int pin_number = code_value();

for(int i = 0; i < sizeof(sensitive_pins); i++)

{

if (sensitive_pins[i] == pin_number)

{

pin_number = -1;

break;

}

}

if (pin_number > -1)

{

pinMode(pin_number, OUTPUT);

digitalWrite(pin_number, pin_status);

analogWrite(pin_number, pin_status);

}

}

}

break;

#ifdef SERVO

case 43:

if (code_seen('S'))

{

int pin_status = code_value();

if (code_seen('P') && pin_status >= 0 && pin_status <= 180)

{

int pin_number = code_value();

if (pin_number > -1 && pin_number<SERVOCOUNT)

{

servos[pin_number].write(pin_status);

}

}

}

break;

#endif

case 104: // M104

if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value());

#ifdef WATCHPERIOD

if(target_raw > current_raw){

watchmillis = max(1,millis());

watch_raw = current_raw;

}else{

watchmillis = 0;

}

#endif

break;

case 140: // M140 set bed temp

#if TEMP_1_PIN > -1 || defined BED_USES_AD595

if (code_seen('S')) target_bed_raw = temp2analogBed(code_value());

#endif

break;

case 105: // M105

#if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675)|| defined HEATER_USES_AD595

tt = analog2temp(current_raw);

#endif

#if TEMP_1_PIN > -1 || defined BED_USES_AD595

bt = analog2tempBed(current_bed_raw);

#endif

#if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675) || defined HEATER_USES_AD595

Serial.print("ok T:");

Serial.print(tt);

#ifdef PIDTEMP

Serial.print(" @:");

Serial.print(heater_duty);

Serial.print(",");

Serial.print(iTerm);

#endif

#if TEMP_1_PIN > -1 || defined BED_USES_AD595

Serial.print(" B:");

Serial.println(bt);

#else

Serial.println();

#endif

#else

#error No temperature source available

#endif

return;

//break;

case 109: { // M109 - Wait for extruder heater to reach target.

if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value());

#ifdef WATCHPERIOD

if(target_raw>current_raw){

watchmillis = max(1,millis());

watch_raw = current_raw;

}else{

watchmillis = 0;

}

#endif

codenum = millis();

/* See if we are heating up or cooling down */

bool target_direction = (current_raw < target_raw); // true if heating, false if cooling

#ifdef TEMP_RESIDENCY_TIME

long residencyStart;

residencyStart = -1;

/* continue to loop until we have reached the target temp

while( (target_direction ? (current_raw < target_raw) : (current_raw > target_raw))

|| (residencyStart > -1 && (millis() - residencyStart) < TEMP_RESIDENCY_TIME*1000) ) {

#else

while ( target_direction ? (current_raw < target_raw) : (current_raw > target_raw) ) {

#endif

if( (millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up/cooling down

{

Serial.print("T:");

Serial.println( analog2temp(current_raw) );

codenum = millis();

}

manage_heater();

#ifdef TEMP_RESIDENCY_TIME

/* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time

if ( (residencyStart == -1 && target_direction && current_raw >= target_raw)

|| (residencyStart == -1 && !target_direction && current_raw <= target_raw)

|| (residencyStart > -1 && labs(analog2temp(current_raw) - analog2temp(target_raw)) > TEMP_HYSTERESIS) ) {

residencyStart = millis();

}

#endif

}

}

break;

case 190: // M190 - Wait bed for heater to reach target.

#if TEMP_1_PIN > -1

if (code_seen('S')) target_bed_raw = temp2analogh(code_value());

codenum = millis();

while(current_bed_raw < target_bed_raw) {

if( (millis()-codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.

{

tt=analog2temp(current_raw);

Serial.print("T:");

Serial.print( tt );

Serial.print(" B:");

Serial.println( analog2temp(current_bed_raw) );

codenum = millis();

}

manage_heater();

}

#endif

break;

#if FAN_PIN > -1

case 106: //M106 Fan On

if (code_seen('S')){

WRITE(FAN_PIN, HIGH);

analogWrite(FAN_PIN, constrain(code_value(),0,255) );

}

else {

WRITE(FAN_PIN, HIGH);

analogWrite(FAN_PIN, 255 );

}

break;

case 107: //M107 Fan Off

analogWrite(FAN_PIN, 0);

WRITE(FAN_PIN, LOW);

break;

#endif

#if (PS_ON_PIN > -1)

case 80: // M81 - ATX Power On

SET_OUTPUT(PS_ON_PIN); //GND

break;

case 81: // M81 - ATX Power Off

SET_INPUT(PS_ON_PIN); //Floating

break;

#endif

case 82:

axis_relative_modes[3] = false;

break;

case 83:

axis_relative_modes[3] = true;

break;

case 84:

if(code_seen('S')){ stepper_inactive_time = code_value() * 1000; }

else{ disable_x(); disable_y(); disable_z(); disable_e(); }

break;

case 85: // M85

code_seen('S');

max_inactive_time = code_value() * 1000;

break;

case 92: // M92

for(int i=0; i < NUM_AXIS; i++) {

if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value();

}

#ifdef RAMP_ACCELERATION

setup_acceleration();

#endif

break;

case 115: // M115

Serial.print("FIRMWARE_NAME:Sprinter FIRMWARE_URL:http%%3A/github.com/kliment/Sprinter/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1 UUID:");

Serial.println(uuid);

break;

case 114: // M114

Serial.print("X:");

Serial.print(current_position[0]);

Serial.print("Y:");

Serial.print(current_position[1]);

Serial.print("Z:");

Serial.print(current_position[2]);

Serial.print("E:");

Serial.println(current_position[3]);

break;

case 119: // M119

#if (X_MIN_PIN > -1)

Serial.print("x_min:");

Serial.print((READ(X_MIN_PIN)^X_ENDSTOP_INVERT)?"H ":"L ");

#endif

#if (X_MAX_PIN > -1)

Serial.print("x_max:");

Serial.print((READ(X_MAX_PIN)^X_ENDSTOP_INVERT)?"H ":"L ");

#endif

#if (Y_MIN_PIN > -1)

Serial.print("y_min:");

Serial.print((READ(Y_MIN_PIN)^Y_ENDSTOP_INVERT)?"H ":"L ");

#endif

#if (Y_MAX_PIN > -1)

Serial.print("y_max:");

Serial.print((READ(Y_MAX_PIN)^Y_ENDSTOP_INVERT)?"H ":"L ");

#endif