/*
 ****************************************************************************
 *  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/>.        *
 ****************************************************************************
 */

/*
 * controller.cpp
 *
 *  Created on: 2016126
 *      Author: Dark
 */

#include "controller.h"
#include "configuration.h"
#include "con_configuration.h"

#include "speed_lookuptable.h"

#include "Ananas.h"
#include "stepper.h"
#include "Encoder.h"
//#include "PID_v1.h"
#include "PID_AutoTune_v0.h"
//#include "digitalWriteFast.h"

//#include "MsTimer2.h"

// intRes = longIn1 * longIn2 >> 24
// uses:
// r26 to store 0
// r27 to store the byte 1 of the 48bit result
#define MultiU24X24toH16(intRes, longIn1, longIn2) \
asm volatile ( \
"clr r26 \n\t" \
"mul %A1, %B2 \n\t" \
"mov r27, r1 \n\t" \
"mul %B1, %C2 \n\t" \
"movw %A0, r0 \n\t" \
"mul %C1, %C2 \n\t" \
"add %B0, r0 \n\t" \
"mul %C1, %B2 \n\t" \
"add %A0, r0 \n\t" \
"adc %B0, r1 \n\t" \
"mul %A1, %C2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"mul %B1, %B2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"mul %C1, %A2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"mul %B1, %A2 \n\t" \
"add r27, r1 \n\t" \
"adc %A0, r26 \n\t" \
"adc %B0, r26 \n\t" \
"lsr r27 \n\t" \
"adc %A0, r26 \n\t" \
"adc %B0, r26 \n\t" \
"clr r1 \n\t" \
: \
"=&r" (intRes) \
: \
"d" (longIn1), \
"d" (longIn2) \
: \
"r26" , "r27" \
)

// intRes = intIn1 * intIn2 >> 16
// uses:
// r26 to store 0
// r27 to store the byte 1 of the 24 bit result
#define MultiU16X8toH16(intRes, charIn1, intIn2) \
asm volatile ( \
"clr r26 \n\t" \
"mul %A1, %B2 \n\t" \
"movw %A0, r0 \n\t" \
"mul %A1, %A2 \n\t" \
"add %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"lsr r0 \n\t" \
"adc %A0, r26 \n\t" \
"adc %B0, r26 \n\t" \
"clr r1 \n\t" \
: \
"=&r" (intRes) \
: \
"d" (charIn1), \
"d" (intIn2) \
: \
"r26" \
)

#define MAX_STEP_FREQUENCY DEFAULT_MAX_FEEDRATE//40000 // Max step frequency for Ultimaker (5000 pps / half step)

static char step_loops;

volatile bool Max_Stop_state;
volatile bool Min_Stop_state;

//global
uint32_t max_feedrate;

volatile bool controldir; //target motor dir
volatile float factor;
volatile float encoderfactor;

volatile unsigned short targertFeedrate; //target speed steps/s
volatile static unsigned short targertFeedrate_OCR1A;
volatile static char step_loops_targertFeedrate;
volatile bool targertdir; //target motor dir

volatile unsigned short currentFeedrate; //current speed steps/s
volatile bool currentdir; //current motor dir

unsigned long acceleration_rate; //accelarate steps/s^2

unsigned short jerk_rate; //the rate to change dir steps/s
static unsigned short jerk_rate_OCR1A;
static char step_loops_jerk_rate;

unsigned short start_rate; //start rate
static unsigned short start_rate_OCR1A;
static char step_loops_start_rate;

//unsigned short lock_rate; //start rate
//static unsigned short lock_rate_OCR1A;
//static char step_loops_lock_rate;

unsigned short timer;
static long acceleration_time, deceleration_time;

static unsigned short acc_step_rate; // needed for deccelaration start point

//static unsigned short OCR1A_nominal;
//static char step_loops_nominal;
//for PID CONTROLLER

//PID for display
double kp;
double ki;
double kd;
//int

unsigned short PIDtime; //PID time ms
unsigned short PIDtimeus; //PID time us

long PIDoutFeedrate; //speed for PID output
short short_PIDoutFeedrate; //speed for PID output
long pulseforPID; //setpoint for PID Compute
long encouforPID; //input for PID Compute
//parameter for PID Compute
//There did not care
//output is short
long error = 0; //setpoint - input error short less than 65536 e(k)
long lasterror = 0; //e(k-1)
long lastlasterror = 0; //e(k-2)
long ITerm; //should less than maxOutput more than minOutput
long outMax; //max outPut
long outMin; //min outPut
long dinput; //input - lastinput
long lastinput; //lastinput
//Kp Ki Kd for calculated

short Kp;
short Ki;
short Kd;

double DKp;
double DKi;
double DKd;

bool PIDauto;

//bool blink;

long aTuneStep = 50000, aTuneNoise = 4, aTuneStartValue = 25000;
unsigned int aTuneLookBack = 20;

//PID AnanasPID(&encouforPID, &PIDoutFeedrate, &pulseforPID, kp, ki, kd, 1);
//PID AnanasPID(&encouforPID, &PIDoutFeedrate, &pulseforPID, kp, ki, kd, 0);
PID_ATune aTune(&encouforPID, &PIDoutFeedrate);

void setmaxfeedrate(unsigned long max);
void setaccelerationrate(unsigned long acc);

void setjerkfeedrate(unsigned short jerk);
void setstartfeedrate(unsigned short start);
//void setlockfeedrate(unsigned short lock);
void setTarget_feedrate(unsigned short target_Rate);
void setTarget_feedrate_long(long &target_Rate);
void refreshPID();

void PID_initial() {
	//no eeprom
//	kp = KP;
//	ki = KI;
//	kd = KD;
	PIDtime = PIDTIME;
	PIDtimeus = PIDTIMEUS;
//	controldir = false;
//	factor = STRONG_PURSE_FACTOR;
//	setlockfeedrate(SPEEDUNIT * STRONG_PURSE_FACTOR);

	//refresh
//	AnanasPID.SetTunings(kp, ki, kd);
//	max_feedrate = USR_MAX_FEEDRATE;
//	acceleration_rate = DEFAULT_ACC_RATE;

	targertdir = false; //2016.3.21 this must match PID dir
	//else there be a bug for start with a dir
	currentFeedrate = 0;
	targertFeedrate = 0;
	currentdir = targertdir;

	//initial PID
//	AnanasPID.SetMode(AUTOMATIC);
//	AnanasPID.SetSampleTime(PIDTIME);	//1ms
//	AnanasPID.SetOutputLimits(-max_feedrate, max_feedrate);

	PIDauto = true;	//auto calculate PID

	//not run with the  timer2 in encoder at the same time
//	MsTimer2::set(SPEEDMEASURETIME, PID_compute);

//	MsTimer2::start(); //start timer

	//PID initial
	short_PIDoutFeedrate = 0;

	PIDoutFeedrate = 0;
	dinput = 0; //input - lastinput
	lastinput = 0; //lastinput

	//initial timer1
	// waveform generation = 0100 = CTC
	TCCR1B &= ~(1 << WGM13);
	TCCR1B |= (1 << WGM12);
	TCCR1A &= ~(1 << WGM11);
	TCCR1A &= ~(1 << WGM10);

	// output mode = 00 (disconnected)
	TCCR1A &= ~(3 << COM1A0);
	TCCR1A &= ~(3 << COM1B0);
	// Set the timer pre-scaler
	// Generally we use a divider of 8, resulting in a 2MHz timer
	// frequency on a 16MHz MCU. If you are going to change this, be
	// sure to regenerate speed_lookuptable.h with
	// create_speed_lookuptable.py
	TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);

	OCR1A = 0x4000;
	TCNT1 = 0;
	ENABLE_STEPPER_DRIVER_INTERRUPT();

}

/* TODO
 void StopStepper() {
 }

 void reStartStepper() {
 }
 */

int runTuning() {
//	pulseforPID = pulseCount;		//delete factor
	encouforPID = encodercount;		//it should be adjust
	setTarget_feedrate_long(PIDoutFeedrate);
	return aTune.Runtime();
}

void SetTuning(bool type) {

	aTune.SetControlType(type);
	aTune.SetNoiseBand(aTuneNoise);
	aTune.SetOutputStep(aTuneStep);
	aTune.SetLookbackSec((int) aTuneLookBack);

	PIDoutFeedrate = aTuneStartValue;
	setTarget_feedrate_long(PIDoutFeedrate);
}

void StopTuning() {
	aTune.Cancel();
}

void printTuningPID() {
	SERIAL_PROTOCOLPGM("T_P: ");
	SERIAL_PROTOCOL(aTune.GetKp());
	SERIAL_PROTOCOLPGM(";T_I: ");
	SERIAL_PROTOCOL(aTune.GetKi());
	SERIAL_PROTOCOLPGM(";T_D: ");
	SERIAL_PROTOCOLLN(aTune.GetKd());
}

//bool PID_compute() {
//void PID_compute() {
//	//refresh
////	unsigned long now = millis();
////	unsigned long timeChange = (now - lastTime);
////	if (timeChange >= SampleTime) {
////	interrupts();
//	pulseforPID = pulseCount / factor;
//	encouforPID = encodercount;
//
////	printPID();
//	if (AnanasPID.Compute()) {
////		Serial.println("PID RUN !");
////		Serial.print("PIDoutFeedrate ");
////		Serial.println(PIDoutFeedrate);
////		Serial.print("OCR1A ");
////		Serial.println(OCR1A);
////		blink = !blink;
////		WRITE(13, blink);
//		//refresh the speed
//
////		speedcalculate();
//
//		//		//responds actively
////		if (absencodercounterfeedrate > 3 * endertostep
//		if (PIDoutFeedrate == 0) {
//			setTarget_feedrate(0);
//		} else if ((PIDoutFeedrate > 0) ^ controldir) {
//			setTarget_feedrate(PIDoutFeedrate);
//			targertdir = true;
//		} else {
//			setTarget_feedrate(-PIDoutFeedrate);
//			targertdir = false;
//		}
//
////		if (absencodercounterfeedrate < currentFeedrate / 3) {
//////			Serial.println("PID RUN !");
////				setTarget_feedrate(targertFeedrate / 3);
//////			setTarget_feedrate(absencodercounterfeedrate);
//////			setTarget_feedrate((targertFeedrate + start_rate) / 5);
//////			setTarget_feedrate((absencodercounterfeedrate + start_rate) / 2);
//////			return;
////		}
//
////		return true;
////	}	//computePID
////	else
////		return false;
//
//	}
//	//	lastTime = now; //interrupting to run
////	}
//}

//FORCE_INLINE
FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
	unsigned short timer;
	if (step_rate > MAX_STEP_FREQUENCY)
		step_rate = MAX_STEP_FREQUENCY;

	if (step_rate > 40000) { // If steprate > 20kHz >> step 4 times
		step_rate = (step_rate >> 3) & 0x1fff;
		step_loops = 8;
	} else if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
		step_rate = (step_rate >> 2) & 0x3fff;
		step_loops = 4;
	} else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
		step_rate = (step_rate >> 1) & 0x7fff;
		step_loops = 2;
	} else {
		step_loops = 1;
	}
//
	if (step_rate < (F_CPU / 500000))
		step_rate = (F_CPU / 500000);
	step_rate -= (F_CPU / 500000); // Correct for minimal speed
	if (step_rate >= (8 * 256)) { // higher step rate
		unsigned short table_address =
				(unsigned short) &speed_lookuptable_fast[(unsigned char) (step_rate
						>> 8)][0];
		unsigned char tmp_step_rate = (step_rate & 0x00ff);
		unsigned short gain = (unsigned short) pgm_read_word_near(
				table_address + 2);
		MultiU16X8toH16(timer, tmp_step_rate, gain);
		timer = (unsigned short) pgm_read_word_near(table_address) - timer;
	} else { // lower step rates
		unsigned short table_address =
				(unsigned short) &speed_lookuptable_slow[0][0];
		table_address += ((step_rate) >> 1) & 0xfffc;
		timer = (unsigned short) pgm_read_word_near(table_address);
		timer -= (((unsigned short) pgm_read_word_near(table_address + 2)
				* (unsigned char) (step_rate & 0x0007)) >> 3);
	}

	if (timer < 100) {
		timer = 100;
//		MYSERIAL.print(MSG_STEPPER_TOO_HIGH);
		MYSERIAL.println(step_rate);
	} //(20kHz this should never happen)
	return timer;
}

//interrupt for step
ISR(TIMER1_COMPA_vect) {
	//restart interrupts
	interrupts();

	if (targertFeedrate > 0) {
//		Serial.println("step!");

		if (targertdir ^ currentdir) {
//			currentdir = targertdir;
			//			SERIAL_PROTOCOLLNPGM("change dir ");
			//			Serial.println("change dir ");
			//first dec
			acceleration_time = 0;
			MultiU24X24toH16(acc_step_rate, deceleration_time,
					acceleration_rate);
			if (acc_step_rate > currentFeedrate) {
				currentFeedrate = jerk_rate;
			} else {
				currentFeedrate -= acc_step_rate;
			}

			//speed limit
			if (currentFeedrate <= jerk_rate) {
				currentFeedrate = jerk_rate;
				currentdir = targertdir;
			}

			timer = calc_timer(currentFeedrate);
			OCR1A = timer;
			deceleration_time += timer;

			WRITE(STEP_DIR, currentdir);

		} else {
			if (targertFeedrate > currentFeedrate) { //need to acc

				deceleration_time = 0;
//				Serial.println("acc ");
				MultiU24X24toH16(acc_step_rate, acceleration_time,
						acceleration_rate);

				currentFeedrate += acc_step_rate;

				if (currentFeedrate > max_feedrate)
					currentFeedrate = max_feedrate;

				if (currentFeedrate > targertFeedrate) {
					currentFeedrate = targertFeedrate;
				}

//				Serial.print("currentFeedrate ");
//				Serial.println(currentFeedrate);

				timer = calc_timer(currentFeedrate);

//
//				Serial.print("time ");
//				Serial.println(timer);

				OCR1A = timer;
				acceleration_time += timer;

//
			} else if (targertFeedrate < currentFeedrate) {
				//				SERIAL_PROTOCOLLNPGM("dec ");
				//				Serial.println("dec ");

				acceleration_time = 0;
				MultiU24X24toH16(acc_step_rate, deceleration_time,
						acceleration_rate);
				if (acc_step_rate > currentFeedrate) {
					currentFeedrate = start_rate;

				} else {
					currentFeedrate -= acc_step_rate;
				}

//				speed limit
				if (currentFeedrate < start_rate) {
					currentFeedrate = start_rate;
				}

				if (currentFeedrate < targertFeedrate) {
					currentFeedrate = targertFeedrate;
				}

				timer = calc_timer(currentFeedrate);

				OCR1A = timer;
				deceleration_time += timer;
				//
//
			} else {
				deceleration_time = 0;
				acceleration_time = 0;
				currentFeedrate = targertFeedrate;

//				Serial.print("currentFeedrate_OCR1A ");
//				Serial.println(targertFeedrate_OCR1A);

				OCR1A = targertFeedrate_OCR1A;
				step_loops = step_loops_targertFeedrate;
//
			}
		}

		for (int8_t i = step_loops; i > 0; i--) {

#ifdef USE8825
			digitalWriteNoTimer(STEP_STEP, LOW);
#ifdef USEENDSTOP
			Max_Stop_state = READ(MAXENDSTOP);
			Min_Stop_state = READ(MINENDSTOP);
#endif
			digitalWriteNoTimer(STEP_STEP, HIGH);

#else
			WRITE(STEP_STEP, LOW);
#ifdef USEENDSTOP
			Max_Stop_state = READ(MAXENDSTOP);
			Min_Stop_state = READ(MINENDSTOP);
#endif
			WRITE(STEP_STEP, HIGH);
#endif
		}
	} else {
		//wait

		currentFeedrate = start_rate;
		OCR1A = 2000;				// 1kHz.
	}

	//for test
//	if (targertFeedrate > 0) {
//		if (targertdir ^ currentdir) {
//			currentdir = targertdir;
//			WRITE(STEP_DIR, currentdir);
//		} else {
//			currentFeedrate = targertFeedrate;
//			OCR1A = targertFeedrate_OCR1A;
//			step_loops = step_loops_targertFeedrate;
//			for (int8_t i = 0; i < step_loops; i++)
//				stepastep();
//		}
//
//	} else {
//		//wait
//		currentFeedrate = 0;
//		OCR1A = 2000; // 1kHz.
//	}
}

void setTarget_feedrate(unsigned short target_Rate) {

//limit targetFeedrate
	if (target_Rate)				//only when target_Rate>0 enable the stepper
		WRITE(STEP_ENABLE, LOW);

	if (absencodercounterfeedrate < (currentFeedrate >> 2)) {
		//			Serial.println("PID RUN !");
		target_Rate >>= 2;
		//			setTarget_feedrate(absencodercounterfeedrate);
		//			setTarget_feedrate((targertFeedrate + start_rate) / 5);
		//			setTarget_feedrate((absencodercounterfeedrate + start_rate) / 2);
		//			return;
	}

	if (target_Rate <= max_feedrate)
		targertFeedrate = target_Rate;
	else
		targertFeedrate = max_feedrate;

	if (target_Rate < start_rate) {
		targertFeedrate = 0;
		//return to the initial state
//		targertdir = false;
//		currentdir = targertdir;
	}

	targertFeedrate_OCR1A = calc_timer(targertFeedrate);
	step_loops_targertFeedrate = step_loops;

//message
//	SERIAL_PROTOCOLLNPGM("set targertFeedrate ok  : ");
//	SERIAL_PROTOCOLPGM("targertFeedrate_OCR1A : ");
//	SERIAL_PROTOCOLLN(targertFeedrate_OCR1A);
////	SERIAL_ECHOLN("");
//	SERIAL_PROTOCOLPGM("step_loops_targertFeedrate : ");
//	SERIAL_PROTOCOLLN((long )step_loops_targertFeedrate);
////	SERIAL_ECHOLN("");
////	unsigned short timer = OCR1A;
////	SERIAL_PROTOCOLPGM("OCR1A : ");
////	SERIAL_PROTOCOLLN(timer);
////	SERIAL_ECHOLN("");
////
//	SERIAL_PROTOCOLPGM("targertFeedrate : ");
//	SERIAL_PROTOCOLLN((long )targertFeedrate);
//	SERIAL_ECHOLN("");
}
//refresh maxfeedrate
void setTarget_feedrate_long(long &target_Rate) {
	if (target_Rate > outMax)
		target_Rate = outMax;
	else if (target_Rate < outMin)
		target_Rate = outMin;

	if ((target_Rate > 0)) {
		targertdir = true; //first change the dir
		setTarget_feedrate(target_Rate);
	} else {
		targertdir = false;
		setTarget_feedrate(-target_Rate);
	}
}
void changecontroldir() {
	controldir = !controldir;
	refreshPID();
}

//void setfactor(float f) {
//	factor = f;
//}
void refreshmaxfeedrate() {

	outMax = max_feedrate;
	outMin = -max_feedrate;
//	AnanasPID.SetOutputLimits(-max_feedrate, max_feedrate);
}

void setmaxfeedrate(unsigned long max) {
	max_feedrate = max;
	refreshmaxfeedrate();
}

void setaccelerationrate(unsigned long acc) {
	acceleration_rate = acc;
}

void setjerkfeedrate(unsigned short jerk) {
	jerk_rate = jerk;
	jerk_rate_OCR1A = calc_timer(jerk_rate);
	step_loops_jerk_rate = step_loops;
}

void setstartfeedrate(unsigned short start) {
	start_rate = start;
	start_rate_OCR1A = calc_timer(start_rate);
	step_loops_start_rate = step_loops;
}

//void setlockfeedrate(unsigned short lock) {
//	lock_rate = lock;
//	lock_rate_OCR1A = calc_timer(lock_rate);
//	step_loops_lock_rate = step_loops;
//}

void change_targertdir() {
	targertdir = !targertdir;
}

void us_PID_Compute() {
//	pulseforPID = pulseCount / factor;
	pulseforPID = pulseCount;		//delete factor
	//it should be adjust
	encouforPID = encodercount;

	error = pulseforPID - encouforPID; //divison or time not used

//	ITerm += (Ki * error);
	ITerm += (DKi * error);
//	ITerm += (ki * error);min
	if (ITerm > outMax)
		ITerm = outMax;
	else if (ITerm < outMin)
		ITerm = outMin;
	//PID
//	dinput = (encouforPID - lastinput);
	dinput = (error - lasterror); //cal derror
//PID
//	lastinput = encouforPID;
	lasterror = error;
//PID
	PIDoutFeedrate = Kp * error + ITerm - DKd * dinput;
//	PIDoutFeedrate = Kp * error + ITerm + Kd * dinput;	//PI
//	PIDoutFeedrate = Kp * error + ITerm;

//	PIDoutFeedrate = kp * error + ITerm - kd * dinput;
//	PIDoutFeedrate = kp * error + ITerm + kd * dinput;
//	PIDoutFeedrate = DKp * error + ITerm - DKd * dinput;
//

	setTarget_feedrate_long(PIDoutFeedrate);
//	if (PIDoutFeedrate > outMax)
//		PIDoutFeedrate = outMax;
//	else if (PIDoutFeedrate < outMin)
//		PIDoutFeedrate = outMin;
//
//	if ((PIDoutFeedrate > 0)) {
//		targertdir = true; //first change the dir
//		setTarget_feedrate(PIDoutFeedrate);
//	} else {
//		targertdir = false;
//		setTarget_feedrate(-PIDoutFeedrate);
//	}
}

void us_PID_Compute_InCMT() {

	pulseforPID = pulseCount;		//delete factor
	encouforPID = encodercount;		//it should be adjust

	error = pulseforPID - encouforPID; //divison or time not used
	PIDoutFeedrate = PIDoutFeedrate + Kp * (error - lasterror) + Ki * error
			+ Kd * (error - 2 * lasterror + lastlasterror);
	lasterror = error;
	lastlasterror = lasterror;

//	SERIAL_PROTOCOLPGM("PIDoutFeedrate : ");
//	SERIAL_PROTOCOLLN(PIDoutFeedrate);
//	SERIAL_PROTOCOLPGM("outMax : ");
//	SERIAL_PROTOCOLLN(outMax);
//	SERIAL_PROTOCOLPGM("outMin : ");
//	SERIAL_PROTOCOLLN(outMin);
//	SERIAL_PROTOCOLPGM("error : ");
//	SERIAL_PROTOCOLLN(error);

	if (PIDoutFeedrate > outMax)
		PIDoutFeedrate = outMax;
	else if (PIDoutFeedrate < outMin)
		PIDoutFeedrate = outMin;

	if ((PIDoutFeedrate > 0)) {
		targertdir = true; //first change the dir
		setTarget_feedrate(PIDoutFeedrate);
	} else {
		targertdir = false;
		setTarget_feedrate(-PIDoutFeedrate);
	}
}

void setPID(double kpin, double kiin, double kdin) {
	if (kpin < 0 || kiin < 0 || kdin < 0)
		return;

	kp = kpin;
	ki = kiin;
	kd = kdin;
	refreshPID();

//	AnanasPID.SetTunings(kpin, kiin, kdin);
//	AnanasPID.SetControllerDirection(controldir);
}

void refreshPID() {
	if (controldir) {
		Kp = kp;
		Ki = ki;
		Kd = kd;
//		Ki = (short) (ki * (double) PIDTIMEUS);
//		Kd = (short) (kd * (double) PIDTIMEUS);

		DKp = kp;
		DKi = (ki / (double) (1000000 / PIDTIMEUS));
		DKd = (kd * (double) (1000000 / PIDTIMEUS));
	} else {
		Kp = 0 - kp;
		Ki = 0 - ki;
		Kd = 0 - kd;
//		Ki = 0 - (short) (ki * (double) PIDTIMEUS);
//		Kd = 0 - (short) (kd * (double) PIDTIMEUS);

		DKp = 0 - kp;
		DKi = 0 - (ki / (double) (1000000 / PIDTIMEUS));
		DKd = 0 - (kd * (double) (1000000 / PIDTIMEUS));
	}
}

void setP(double kpin) {
	kp = kpin;
	setPID(kpin, ki, kd);
}

void setI(double kiin) {
	ki = kiin;
	setPID(kp, kiin, kd);
}

void setD(double kdin) {
	kd = kdin;
	setPID(kp, ki, kdin);
}

//void setPIDmode(int mode) { //0 for mannul 1 for auto
//	AnanasPID.SetMode(mode);
//}

void printNewPID() {
	SERIAL_PROTOCOLPGM("Kp: ");
	SERIAL_PROTOCOLLN(Kp);

	SERIAL_PROTOCOLPGM("Ki: ");
	SERIAL_PROTOCOLLN(Ki);

	SERIAL_PROTOCOLPGM("Kd: ");
	SERIAL_PROTOCOLLN(Kd);

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

	SERIAL_PROTOCOLPGM("dinput: ");
	SERIAL_PROTOCOLLN(dinput);

	SERIAL_ECHOLN("");
}

void printPID() {
	SERIAL_PROTOCOLPGM("kp: ");
	SERIAL_PROTOCOLLN(kp);
//	SERIAL_PROTOCOLLN(AnanasPID.GetKp());
	SERIAL_PROTOCOLPGM("DKp: ");
	SERIAL_PROTOCOLLN(DKp);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("ki: ");
	SERIAL_PROTOCOLLN(ki);
//	SERIAL_PROTOCOLLN(AnanasPID.GetKi());
	SERIAL_PROTOCOLPGM("DKi: ");
	SERIAL_PROTOCOLLN(DKi);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("kd : ");
	SERIAL_PROTOCOLLN(kd);
//	SERIAL_PROTOCOLLN(AnanasPID.GetKd());
	SERIAL_PROTOCOLPGM("DKd");
	SERIAL_PROTOCOLLN(DKd);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("pulseforPID : ");
	SERIAL_PROTOCOLLN(pulseforPID);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("encouforPID : ");
	SERIAL_PROTOCOLLN(encouforPID);
//	SERIAL_ECHOLN("");

//	SERIAL_PROTOCOLPGM("start_rate : ");
//	SERIAL_PROTOCOLLN(start_rate);
////	SERIAL_ECHOLN("");
//
//	SERIAL_PROTOCOLPGM("start_rate_OCR1A : ");
//	SERIAL_PROTOCOLLN((long )start_rate_OCR1A);
////	SERIAL_ECHOLN("");
//
//	SERIAL_PROTOCOLPGM("step_loops_start_rate : ");
//	SERIAL_PROTOCOLLN((long )step_loops_start_rate);
////	SERIAL_ECHOLN("");

//	unsigned short timer = OCR1A;
//	SERIAL_PROTOCOLPGM("OCR1A : ");
//	SERIAL_PROTOCOLLN(timer);
//	SERIAL_PROTOCOLPGM("targertFeedrate : ");
//	SERIAL_PROTOCOLLN(targertFeedrate);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("PIDoutFeedrate : ");
	SERIAL_PROTOCOLLN(PIDoutFeedrate);

	SERIAL_PROTOCOLPGM("targertdir : ");
	SERIAL_PROTOCOLLN(targertdir);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("currentFeedrate: ");
	SERIAL_PROTOCOLLN(currentFeedrate);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("currentdir : ");
	SERIAL_PROTOCOLLN(currentdir);
//	SERIAL_ECHOLN("");

	SERIAL_PROTOCOLPGM("targertFeedrate : ");
	SERIAL_PROTOCOLLN(targertFeedrate);

//	SERIAL_PROTOCOLPGM("endertostep : ");
//	SERIAL_PROTOCOLLN(endertostep);
	SERIAL_ECHOLN("");

}