{

float error;

float output;

error = setPoint - input;

// Kd is implemented in two parts

// The biggest one using only the input (sensor) part not the SetPoint input-input(t-1).

// And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1)

float Kd_setPoint = constrain((setPoint - setPointOld), -8, 8); // We limit the input part...

output = Kp * error + (Kd * Kd_setPoint - Kd * (input - PID_errorOld)) / DT;

//Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");

//PID_errorOld2 = PID_errorOld;

PID_errorOld = input; // error for Kd is only the input component

setPointOld = setPoint;

return (output);

{

int16_t error;

float output;

error = setPoint - input;

PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);

PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);

//Serial.println(PID_errorSum);

output = Kp * error + Ki * PID_errorSum * DT; // DT is in miliseconds...

return (output);

{

float output;

float P;

P = constrain(Kpp * float(setPointPos - actualPos), -115, 115); // Limit command

output = P + Kdp * float(speedM);

return (output);

{

if (dir_M1 == 0) // If we are not moving we dont generate a pulse

return;

// We generate 1us STEP pulse

SET(PORTE, 6); // STEP MOTOR 1

//delay_1us();

if (dir_M1 > 0)

steps1--;

else

steps1++;

CLR(PORTE, 6);

{

if (dir_M2 == 0) // If we are not moving we dont generate a pulse

return;

// We generate 1us STEP pulse

SET(PORTD, 6); // STEP MOTOR 2

//delay_1us();

if (dir_M2 > 0)

steps2--;

else

steps2++;

CLR(PORTD, 6);

{

long timer_period;

int16_t speed;

// Limit max speed?

// WE LIMIT MAX ACCELERATION of the motors

if ((speed_M1 - tspeed) > MAX_ACCEL)

speed_M1 -= MAX_ACCEL;

else if ((speed_M1 - tspeed) < -MAX_ACCEL)

speed_M1 += MAX_ACCEL;

else

speed_M1 = tspeed;

#if MICROSTEPPING==16

speed = speed_M1 * 50; // Adjust factor from control output speed to real motor speed in steps/second

#else

speed = speed_M1 * 25; // 1/8 Microstepping

#endif

if (speed == 0)

{

timer_period = ZERO_SPEED;

dir_M1 = 0;

}

else if (speed > 0)

{

timer_period = 2000000 / speed; // 2Mhz timer

dir_M1 = 1;

SET(PORTB, 4); // DIR Motor 1 (Forward)

}

else

{

timer_period = 2000000 / -speed;

dir_M1 = -1;

CLR(PORTB, 4); // Dir Motor 1

}

if (timer_period > 65535) // Check for minimun speed (maximun period without overflow)

timer_period = ZERO_SPEED;

OCR1A = timer_period;

// Check if we need to reset the timer...

if (TCNT1 > OCR1A)

TCNT1 = 0;

{

long timer_period;

int16_t speed;

// Limit max speed?

// WE LIMIT MAX ACCELERATION of the motors

if ((speed_M2 - tspeed) > MAX_ACCEL)

speed_M2 -= MAX_ACCEL;

else if ((speed_M2 - tspeed) < -MAX_ACCEL)

speed_M2 += MAX_ACCEL;

else

speed_M2 = tspeed;

#if MICROSTEPPING==16

speed = speed_M2 * 50; // Adjust factor from control output speed to real motor speed in steps/second

#else

speed = speed_M2 * 25; // 1/8 Microstepping

#endif

if (speed == 0)

{

timer_period = ZERO_SPEED;

dir_M2 = 0;

}

else if (speed > 0)

{

timer_period = 2000000 / speed; // 2Mhz timer

dir_M2 = 1;

CLR(PORTC, 6); // Dir Motor2 (Forward)

}

else

{

timer_period = 2000000 / -speed;

dir_M2 = -1;

SET(PORTC, 6); // DIR Motor 2

}

if (timer_period > 65535) // Check for minimun speed (maximun period without overflow)

timer_period = ZERO_SPEED;

OCR3A = timer_period;

// Check if we need to reset the timer...

if (TCNT3 > OCR3A)

TCNT3 = 0;