PID

#include <avr/io.h>
#include <util/delay.h>


//Map Sensor Number to ADC Channel
#define SENSOR1 0
#define SENSOR2 1
#define SENSOR3 2
#define SENSOR4 3
#define SENSOR5 4


float pGain = 100;   //Proportional Gain
float iGain =  0.1;  //Integral Gain
float dGain =  60;  //Differential Gain


int  eInteg = 0;  //Integral accumulator
int ePrev  =0;      //Previous Error



float ReadSensors();
float PID(float current_value);


float control;
float s;

int main(void)
{
   MotorInit();

   InitADC();


    while(1)
    {

      float sprev;


      s=ReadSensors();

      if(s==0xFF)
      {
        s=sprev;
      }



      control = PID(s);


      if(control > 255)
        {control = 255;}
      if(control < -255)
        {control = -255;}

      if(control > 0.0)// rovera e ot dqsnata strana na liniqta
      {

		Move((255-control) ,255); // zaviva na lqvo


      }
      if(control <= 0.0)// rovera e ot lqvata strana na liniqta
      {

        MotorB(255, (255-control));

      }


      delay_ms(10)

      sprev=s;
    }
}




float PID(float current_value)
{
  float pid;
  float error;

  error =  current_value;
  pid = (pGain * error)  + (iGain * eInteg) + (dGain * (error - ePrev));

  eInteg += error;
  ePrev = error;
  return pid;
}

float ReadSensors()
{
   int eleft. left, middle, right, eright; //tuk slagame stoinostite ot ADC-to
   int left2,left1, centre, right1,right2; // 1 ako e nad liniqta i obratnoto

   float AverageSens = 0;

   eright=ReadADC(SENSOR5);
   if(eright>SENSOR_THRES)
   {
      sensor5 = 1;
      LEDOn(5);
   }
   else
   {
      sensor5 = 0;
      LEDOff(5);
   }

   // Read analog inputs
   right=ReadADC(SENSOR4);
   if(right>SENSOR_THRES)
   {
      sensor4 = 1;

   }
   else
   {
      sensor4 = 0;

   }

   middle=ReadADC(SENSOR3);
   if(middle>SENSOR_THRES)
   {
      sensor3 = 1;

   }
   else
   {
      sensor3 = 0;

   }

   left=ReadADC(SENSOR2);
   if(left>SENSOR_THRES)
   {
      sensor2 = 1;

   }
   else
   {
      sensor2 = 0;

   }

   eleft=ReadADC(SENSOR1);
   if(eleft>SENSOR_THRES)
   {
      sensor1 = 1;

   }
   else
   {
      sensor1 = 0;

   }


   if(left2==0 && left1==0 && centre==0 && right1==0 && right2==0)
   {
      return 0xFF;
   }

   // Calculate weighted mean
   AverageSens = (float) left2*(-2) + left1*(-1) + centre*0 + right1*1 + right2*2 ;
   AverageSens = (float) AverageSens / (left2 + left1 + centre + right1 + right2);

   return AverageSens;
}