/*
 ****************************************************************************
 *  Copyright (c) 2015 Dark Guan <tickel.guan@gmail.com>                    *
 *	This file is part of Ananas.                                            *
 *                                                                          *
 *  Ananas 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.                                     *
 *                                                                          *
 *  Ananas 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 Ananas.  If not, see <http://www.gnu.org/licenses/>.        *
 ****************************************************************************
 */

/*
 * Ananas Used and modified Some Marlin Code
 * "https://github.com/ErikZalm/Marlin/"
 */
// Do not remove the include below
#include "Ananas.h"
#include "con_configuration.h"
#include "controller.h"
#ifdef USE_TLC5615 //no use TLC5615
#include "Da.h"
#endif
#include "Encoder.h"
#include "stepper.h"
#include "language.h"
#include "PWMDA.h"

//The setup function is called once at startup of the sketch
bool closeLoop = true; //false; //true;
//char control_mode;
bool tuning = false;

//static unsigned long previous_time = 0;
//static unsigned long PIDtimePast = 0;

static unsigned long previous_time_us = 0;
//static unsigned long PIDtimePast_us = 0;

static unsigned long previous_time_speed = 0;
//static unsigned long timePast_speed = 0;

static unsigned long previous_time_Voltage = 0;
uint8_t MaxVoltage;
uint8_t MinVoltage;
static uint8_t Voltage;

static unsigned long previous_time_Temp = 0;
int Temp;

//for emergency stops
bool Stopped = false;
//Serial parameter
static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
static bool fromsd[BUFSIZE];
static int bufindr = 0;
static int bufindw = 0;
static int buflen = 0;

static char serial_char;
static int serial_count = 0;
static boolean comment_mode = false; //使用和marlin一样的校验方式
static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc

static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;

static unsigned long previous_millis_cmd = 0;
//static unsigned long max_inactive_time = 6000;

//TODO build a pattern to manager motor's inactive,let user to set it

//static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
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 serial_echopair_P(const char *s_P, uint16_t v) {
	serialprintPGM(s_P);
	SERIAL_ECHO(v);
}
void serial_echopair_P(const char *s_P, bool v) {
	serialprintPGM(s_P);
	SERIAL_ECHO(v);
}

//void serialManager(); //abandon

void printInformation();
void printforOsc();
void get_command();
void process_commands();
void FlushSerialRequestResend();
void ClearToSend();
void kill();
int getTemp();

void setup() {
// Add your initialization code here
//	int8_t resetstate = MCUSR;
	MYSERIAL.begin(BANDRATE);
//	Serial.println(MCUSR);
//	SERIAL_PROTOCOLLNPGM("Reset!");
	if ((MCUSR >> PORF) & 0x01) {
		SERIAL_PROTOCOLLNPGM("Power On Reset!");
	}
	if ((MCUSR >> EXTRF) & 0x01) {
		SERIAL_PROTOCOLLNPGM("Outside Reset!");
	}
	if ((MCUSR >> BORF) & 0x01) {
		SERIAL_PROTOCOLLNPGM("Power Failure Detection Reset!");
	}
	if ((MCUSR >> WDRF) & 0x01) {
		SERIAL_PROTOCOLLNPGM("Watch Dog reset!");
	}
	//refresh MCUSR!
	MCUSR = 0x00;
#ifdef STRING_VERSION_CONFIG_H
#ifdef STRING_CONFIG_H_AUTHOR
	SERIAL_ECHO_START;
	SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
	SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
	SERIAL_ECHOPGM(MSG_AUTHOR);
	SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
	SERIAL_ECHOPGM("Compiled: ");
	SERIAL_ECHOLNPGM(__DATE__);
#endif
#endif

	//装载设置
	Config_RetrieveSettings();
//初始化顺序很重要，可以防止电机在初始化的时候抖动，以及编码器不准的情况
#if	DEVICE == DIY_TEST
	initial_DA();
#elif DEVICE == ANANAS092
	initialPWMDA();
	setVoltate(120);
#endif

	initialStepper();
	initialEncoder();
	PID_initial(); //interrupt
//print information
//	printInformation();

	//set mode
//	control_mode = PIDcontroller; //SIMPLE;

//	close pulse connected directly
//	is_dir_connectted = false;
//	control_mode = true;

	Temp = getTemp();

	MaxVoltage = MAX_DA;
	MinVoltage = DA_initial;

	SET_OUTPUT(LED);
	WRITE(LED, HIGH);

}

// The loop function is called in an endless loop
void loop() {
//Add your repeated code here
	if (buflen < (BUFSIZE - 1))
		get_command();
	if (buflen) {
		process_commands();
		buflen = (buflen - 1);
		bufindr = (bufindr + 1) % BUFSIZE;
		ClearToSend();
	}

	if (closeLoop) {
//		PIDtimePast = millis() - previous_time;
//		if (PIDtimePast >= PIDTIME) {
//			PID_compute();
//			previous_time = millis();
//		}

		if (!tuning) {
//		PIDtimePast_us = micros() - previous_time_us;
//		if (PIDtimePast_us >= PIDTIMEUS) {
			if ((micros() - previous_time_us) >= PIDTIMEUS) {
//			Serial.println("Run !");
#ifdef USEAMS5600
				AMSpeedcalculate();
#endif
				us_PID_Compute_InCMT();
//			us_PID_Compute();
//			PID_compute();
				previous_time_us = micros();
			}
		} else {
#ifdef USEAMS5600
			AMSpeedcalculate();
#endif
			byte val = (runTuning());

			if (val != 0) {
				tuning = false;
			}
			if (!tuning) { //we're done,  print the  parameters
				printTuningPID();
			}
		}

	} else {
		//stop
//		setTarget_feedrate(0);

//		SERIAL_PROTOCOLPGM("STOP!");
	}

//	timePast_speed = millis() - previous_time_speed;

//	if (timePast_speed >= SPEEDMEASURETIME) {
	if ((millis() - previous_time_speed) >= SPEEDMEASURETIME) {

		speed_calculate();
		previous_time_speed = millis();
	}

	if ((millis() - previous_time_Voltage) >= DA_ADJUSTTINE) {
#ifdef TEMPDETECT
		if (Temp <= MaxTemp) {
#endif
			Voltage = float(absencodercounterfeedrate) / max_feedrate * 255;
//		SERIAL_PROTOCOLPGM("4s one time Voltage is : ");
//		SERIAL_PROTOCOLLN(Voltage);
			if (Voltage > MaxVoltage)
				Voltage = MaxVoltage;

			if (Voltage < MinVoltage)
				Voltage = MinVoltage;
#ifdef TEMPDETECT
		} else {
			Voltage = MinVoltage;
		}
#endif
		setVoltate(Voltage);
		previous_time_Voltage = millis();
	}
#ifdef TEMPDETECT
	if ((millis() - previous_time_Temp) >= TEMPDETECTTIME) {
		Temp = getTemp();
		previous_time_Temp = millis();
	}
#endif

}

void get_command() {
//TODO 通讯协议需要适配
	while ( MYSERIAL.available() > 0 && buflen < BUFSIZE) {
		serial_char = MYSERIAL.read();
		if (serial_char == '\n' || serial_char == '\r'
				|| (serial_char == ':' && comment_mode == false)
				|| serial_count >= (MAX_CMD_SIZE - 1)) {
			if (!serial_count) { //if empty line
				comment_mode = false; //for new command
				return;
			}
			cmdbuffer[bufindw][serial_count] = 0; //terminate string
			if (!comment_mode) {
				comment_mode = false; //for new command
				fromsd[bufindw] = false;
				if (strchr(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_P(cmdbuffer[bufindw], PSTR("M110"))
									== NULL)) {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
						SERIAL_ERRORLN(gcode_LastN);
						//Serial.println(gcode_N);
						FlushSerialRequestResend();
						serial_count = 0;
						return;
					}

					if (strchr(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_ERROR_START;
							SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
							SERIAL_ERRORLN(gcode_LastN);
							FlushSerialRequestResend();
							serial_count = 0;
							return;
						}
						//if no errors, continue parsing
					} else {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
						SERIAL_ERRORLN(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 ((strchr(cmdbuffer[bufindw], '*') != NULL)) {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
						SERIAL_ERRORLN(gcode_LastN);
						serial_count = 0;
						return;
					}
				}

				if ((strchr(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:
					case 2:
					case 3:
						//屏蔽运动代码
						if (Stopped == true) {
							SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
//              LCD_MESSAGEPGM(MSG_STOPPED); // No LCD right now
						}
						break;
					default:
						break;
					}

				}

				//If command was e-stop process now
				if (strcmp(cmdbuffer[bufindw], "M112") == 0)
					kill();

				bufindw = (bufindw + 1) % BUFSIZE;
				buflen += 1;
			}
			serial_count = 0; //clear buffer
		} else {
			if (serial_char == ';')
				comment_mode = true;
			if (!comment_mode)
				cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}

}

//获取命令的值
float code_value() {
	return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1],
	NULL));
}

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

bool code_seen(char code) {
	strchr_pointer = strchr(cmdbuffer[bufindr], code);
	return (strchr_pointer != NULL);  //Return True if a character was found
}

//void testminsetupspeed() {
////测试最小启动速度
////范围最小速度10step/s 最大1000step/s
////间隔10
//	long minspeed;
//	long maxspeed;
//
//	unsigned long loopnum = (20000) / 100;
//	unsigned long delaytime = 0;
//	unsigned long speed = 0;
//	bool dir = false;
//	closeLoop = false;
//	unsigned absencodercountfeedrate;
////	long prvtime = 0;
////	long lasttime = prvtime;
//	for (int i = 100; i > 0; i--) {
//		speed = loopnum * i;
//		delaytime = 1000000 / speed;
//		if (delaytime > 10) {
//			delaytime -= 10;
//		}
//		digitalWrite(STEP_DIR, dir);
//		for (int j = 0; j < (MOTOR_STEPS_PER_CIRCLE * DRIVE_SUBDIVE); j++) {
//			//这里注意方向
////			prvtime = micros();
////			lasttime = prvtime;
//			stepastep();
//			absencodercountfeedrate = abs(encodercountfeedrate);
//			if (absencodercountfeedrate > maxspeed) {
//				maxspeed = absencodercountfeedrate;
//			}
//			if (absencodercountfeedrate < minspeed) {
//				minspeed = absencodercountfeedrate;
//			}
////			prvtime = micros();
//			delayMicroseconds(delaytime);
//		}
//		dir = !dir;
//
//		SERIAL_ECHOLNPGM("");
//		SERIAL_ECHO_START;
////		SERIAL_ECHOPGM(" time : ");
////		SERIAL_ECHO(prvtime - lasttime);
////		SERIAL_ECHOLNPGM("");
//		SERIAL_ECHOPGM(" i : ");
//		SERIAL_ECHO(i);
//		SERIAL_ECHOLNPGM("");
//		SERIAL_ECHOPGM(" speed : ");
//		SERIAL_ECHO(speed);
//		SERIAL_ECHOLNPGM("");
//		SERIAL_ECHOPGM("MAX speed : ");
//		SERIAL_ECHO(maxspeed);
//		SERIAL_ECHOLNPGM("");
//		SERIAL_ECHOPGM("MIN speed : ");
//		SERIAL_ECHO(minspeed);
//		SERIAL_ECHOLNPGM("");
//
//		SERIAL_ECHOPGM("loopnum : ");
//		SERIAL_ECHO(loopnum);
//		SERIAL_ECHOLNPGM("");
//
//		long error = maxspeed - speed;
//
//		SERIAL_ECHOPGM("error : ");
//		SERIAL_ECHO(error);
//		SERIAL_ECHOLNPGM("");
//
//		if (abs(error) < loopnum) {
//			SERIAL_ECHO_START;
//			SERIAL_ECHOPGM("minimus setup speed : ");
//			SERIAL_ECHO(maxspeed);
//			SERIAL_ECHOLNPGM("");
//
//			setEncodercount(0);
//			closeLoop = true;
//			return;
//		}
//
//		minspeed = 1000;
//		maxspeed = 0;
//		delay(1000);  //wait for stable
//
//	}
////default test on initial time
//	setEncodercount(0);
//	closeLoop = true;
//}

void process_commands() {
	unsigned long codenum; //throw away variable temp variable
//	float codevalue;
//	char *starpos = NULL;
//
//	double tempKp=0;
//	double tempKi=0;
//	double tempKd=0;
	if (code_seen('G')) {
		codenum = 0;
		switch ((int) code_value()) {
		case 0:
		case 1:
			if (code_seen('X'))
				setSteps(code_value());
			//printInformation();
			break;
		default:
			SERIAL_PROTOCOLLNPGM(
					"Command Not Be supported! Please Input Again! ");
			break;
		}

	} else if (code_seen('M')) {
		codenum = 0;
		switch ((int) code_value()) {
		case 119:
			SERIAL_PROTOCOLPGM("Max end is:");
			SERIAL_PROTOCOL(digitalRead(MAXENDSTOP));
//			SERIAL_PROTOCOL(Max_Stop_state);
			SERIAL_PROTOCOLPGM("Min end is:");
			SERIAL_PROTOCOLLN(digitalRead(MINENDSTOP));
//			SERIAL_PROTOCOLLN(Min_Stop_state);
			break;
		case 50:
			printInformation();
			break;

		case 51:
			if (code_seen('S'))
				speedfactor = code_value();
			refresh_endertostep();
			SERIAL_PROTOCOLPGM("Reset velosity calculation: ");
			SERIAL_PROTOCOLLN(endertostep);
			break;

		case 52:
			SERIAL_PROTOCOLPGM("Change close loop state ");
			SERIAL_ECHOLN("");
			closeLoop = !closeLoop;
			if (closeLoop) {
				ENABLE_STEPPER_DRIVER_INTERRUPT();
			} else {
				DISABLE_STEPPER_DRIVER_INTERRUPT();
			}
			SERIAL_PROTOCOLPGM("Close Loop state");
			SERIAL_ECHOLN("");
			SERIAL_PROTOCOLLN(closeLoop);

			break;
		case 53: //disable motor
			SERIAL_PROTOCOLPGM("Disable Motor ");
			SERIAL_ECHOLN("");
			WRITE(STEP_ENABLE, HIGH);
			break;
		case 54:
			printforOsc();
			break;

//		case 55:
//			SERIAL_PROTOCOLPGM("change is_dir_connectted ");
//			SERIAL_ECHOLN("");
//			is_dir_connectted = !is_dir_connectted;
//			SERIAL_PROTOCOLPGM("is_dir_connectted ");
//			SERIAL_ECHOLN("");
//			SERIAL_PROTOCOLLN(is_dir_connectted);
//			break;
		case 56:
			if (code_seen('S')) { //min one
				subdivide = code_value();
			}
			break;

		case 57:
			SERIAL_PROTOCOLPGM("Set Voltage !");
			if (code_seen('S')) { //min one
				int vol = code_value();
				if (vol > 255 && vol < 0) {
					return;
				}
				setVoltate(vol);
				SERIAL_PROTOCOLPGM("Voltage is : ");
				SERIAL_PROTOCOLLN(getVoltage());
			}
			break;
		case 58:
			SERIAL_PROTOCOLPGM("TEMP is ");
			SERIAL_PROTOCOLLN(Temp);
			break;

		case 600:
			SERIAL_PROTOCOLLNPGM("Start Tuning");
			tuning = true;
			SetTuning(0);//PD
			break;
		case 601:
			SERIAL_PROTOCOLLNPGM("Start Tuning");
			tuning = true;
			SetTuning(1);//PID
			break;
		case 602:
			SERIAL_PROTOCOLLNPGM("Stop Tuning");
			tuning = false;
			StopTuning();
			break;
		case 500: //store eeprom setting
			Config_StoreSettings();
			break;
		case 501: //Retrieve eeprom setting
			Config_RetrieveSettings();
			break;
		case 502: //reset the setting ,return to the factory setting
			Config_ResetDefault();
			break;
		case 503: //print the setting in the RAM ,maybe not the same as which  in EEPROM
			Config_PrintSettings();
			break;

		case 60:
			//set PID p i d
			SERIAL_PROTOCOLLNPGM("Set PID! ");
			if (code_seen('P')) { //min one
//				tempKp = code_value();
				setP(code_value());
			}
			if (code_seen('I')) { //min one
//				tempKi = code_value();
				setI(code_value());
			}
			if (code_seen('D')) { //min one
//				tempKd = code_value();
				setD(code_value());
			}
//			setPID(tempKp, tempKi, tempKd);

			break;

			//for test
		case 61:
			SERIAL_PROTOCOLLNPGM("print PID! ");
			printPID();
			printNewPID();
			break;

		case 62:
			SERIAL_PROTOCOLLNPGM("set target speed ");
			if (code_seen('P')) { //min one
				setTarget_feedrate(code_value());
			}
			break;

		case 63:
			SERIAL_PROTOCOLLNPGM("Change TargetDir  ");
			change_targertdir();
			break;

		case 64:
			SERIAL_PROTOCOLLNPGM("Manual PID Compute   ");
//			PID_compute();
			break;

			//test end

		case 65:
			SERIAL_PROTOCOLLNPGM("Set XXX Feedrate! ");
			if (code_seen('S')) {
				setstartfeedrate(code_value());
			}
			if (code_seen('J')) {
				setjerkfeedrate(code_value());
			}
			if (code_seen('X')) {
				setmaxfeedrate(code_value());
			}
			if (code_seen('A')) {
				setaccelerationrate(code_value());
			}
			break;

		case 66:
			if (code_seen('S')) {
				SERIAL_PROTOCOLLNPGM("Set Setp Stepper Factor");
				factor = (code_value());
			}

			if (code_seen('P')) {
				SERIAL_PROTOCOLLNPGM("Set Setp Enceder Factor");
				encoderfactor = (code_value());
			}
			break;

		case 67:
			SERIAL_PROTOCOLLNPGM("Change Count Dir!");
			changeDir();
			break;

		case 68:
			SERIAL_PROTOCOLLNPGM("Change Step Dir!");
			changecontroldir();
			break;

		case 69:
			SERIAL_PROTOCOLLNPGM("Change Step Count Dir!");
			changeStepCounterdir();
			break;

		default:
			SERIAL_PROTOCOLLNPGM(
					"Command Not Be supported! Please Input Again! ");
			break;
		}

	}
}

void FlushSerialRequestResend() {
//char cmdbuffer[bufindr][100]="Resend:";
	MYSERIAL.flush();
	SERIAL_PROTOCOLPGM(MSG_RESEND);
	SERIAL_PROTOCOLLN(gcode_LastN + 1);
	ClearToSend();
}

void ClearToSend() {
	previous_millis_cmd = millis();

	SERIAL_PROTOCOLLNPGM(MSG_OK);
}

void kill() {
	cli();
//TODO need to modify
// Stop interrupts
//Marlin 用来关闭

//#if defined(PS_ON_PIN) && PS_ON_PIN > -1
//  pinMode(PS_ON_PIN,INPUT);
//#endif
	SERIAL_ERROR_START;
	SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
//  LCD_ALERTMESSAGEPGM(MSG_KILLED);
//  suicide(); //自杀 关闭电源？
	while (1) { /* Intentionally left empty */
	} // Wait for reset
}

int getTemp() {
	float T = 0.0;
	float Vout = 0.0;
	float NTC = 0.0;

	Vout = analogRead(ADCPIN) * 0.0048876;	//0.0048876 = 5V/1023
//	SERIAL_PROTOCOLPGM("Vout:");
//	SERIAL_PROTOCOLLN(Vout);
	NTC = (R * 5.0) / (5.0 - Vout);				//the current NTC resistance
//	SERIAL_PROTOCOLPGM("NTC R:");
//	SERIAL_PROTOCOLLN(NTC);
	NTC = log(NTC);
	T = A + B * NTC + C * NTC * NTC * NTC;    	//Steinhart-Hart equation
	T = 1.0 / T;

	return T - 273.15;
}

void printforOsc() {
	Serial.print('H'); /* Unique header to identify start of message */
	Serial.print(",");
	Serial.print(pulseCount, DEC);
	Serial.print(",");
	Serial.print(encodercount, DEC);
	Serial.print(","); /* Note that a comma is sent after the last field */
	Serial.println(); /* send a cr/lf */
}

void printInformation() {
	SERIAL_ECHOLN("");
	SERIAL_ECHO_START;
	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("Pulse Count : ");
	SERIAL_PROTOCOLLN(getSteps());

	SERIAL_PROTOCOLPGM("Encoder Count : ");
	SERIAL_PROTOCOLLN(getEncodercount());
//
	SERIAL_PROTOCOLPGM("Voltage : ");
#if	DEVICE == DIY_TEST
	SERIAL_PROTOCOLLN(getvol());
#elif DEVICE==ANANAS092
	SERIAL_PROTOCOLLN(getVoltage());
#endif

	SERIAL_PROTOCOLPGM("absencodercounterfeedrate : ");
	SERIAL_PROTOCOLLN(absencodercounterfeedrate);

	SERIAL_PROTOCOLPGM("error : ");
	SERIAL_PROTOCOLLN(error);

	SERIAL_PROTOCOLPGM("close Loop on ? : ");
	SERIAL_PROTOCOLLN(closeLoop);
	SERIAL_ECHOLN("");
}