pid.cpp

#include "PID.h"
#include "everytime.h"
#include <FrameStream.h>
#include <Frameiterator.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "Odometry.h"
#include "MotorControl.h"
#define OUTPUT__BAUD_RATE 57600
#include "Distance.h"

void PID_Cyclic (int current, struct PID_Einstellung& PID)
{
  	PID.Tn = (millis()-PID.T);
  	PID.I = (1/PID.Tn);
	int AS = 0.15; 		//akzeptierer Fehler


PID.e = PID.goal - current; // aktuelle Regelabweichung bestimmen
if ((PID.e >= AS)||(PID.e) >= (AS * (-1))) // Schwelle
{
if ((PID.u < 1023)&&(PID.u > 0)) // (Anti-Windup)
PID.esum = PID.esum + PID.e;


PID.u = PID.Kp* (PID.e + PID.I*PID.esum)+ PID.Tv*(PID.e-PID.ealt);
PID.ealt = PID.e;
}
if (PID.u>= 128)
  PID.u = 128;

if(PID.u<=-127)
  PID.u = -127;

PID.T = millis();
}