#include <Servo.h> 
 
 Servo servo;              
 int pos = 0;
 long sensors[]={0, 0, 0};
 int a[]={0, 0, 0};//senzori
 int n,m;
 int proportional=0;
 int integral=0;
 int derivative;
 int read_value, power_difference;
 float last_proportional=0;
 int led = 13;

void setup()
{
  Serial.begin(9600);
  servo.attach(11); 
  pinMode(led, OUTPUT);
}


void loop()
{
  //LED
  digitalWrite(led, HIGH);   
  delay(60);               
  digitalWrite(led, LOW);    
  delay(100);
  digitalWrite (led, HIGH);
  delay(10);
  digitalWrite(led, LOW); 
  delay (30);
  
  //Sensors Read
  for(int i=0; i<3;i++)
  {
    sensors[i]=analogRead(i);
     if (sensors[1]>=480)
      {
        a[1]=1;
        n=0;
        m=1000;
      }
    else
      a[1]=0;
      
    if (sensors[0]>=200)
      {
        a[0]=1;
        n=1;
        m=0;
      }
    else
      a[0]=0;
   
    if (sensors[2]>=480)
      {
        a[2]=1;
        n=2;
        m=2000;
      }
    else
      a[2]=0;
    
    if((a[0]==0) && (a[1]==0) && (a[2]==0))
      n=0;
    Serial.print(a[i]);
    Serial.print( " " );
    

  }
  Serial.print(n);
  Serial.println( "" );
  //unghiurile posibile pt n==i
  if(n==1)
  {
    servo.write(60);   
  }
  else if(n==2)
  {
    servo.write(120);   
  }
  else if(n==0)
  {
    servo.write(90);   
  }
  
  proportional = m - 1000;
  int derivative = proportional - last_proportional;
  integral += proportional;
 
  // Ultima pozitie a senzorului
  last_proportional = proportional;
  power_difference = proportional/4.67 + integral/5000 + derivative*3/4;
  
 
  read_value=power_difference;
  
  Serial.print( m );
   Serial.print( " " );
   Serial.print( proportional );
   Serial.print( " " );
   Serial.print( power_difference );
   Serial.print( " " );
   Serial.print( read_value );
   Serial.print( " " );
   
   //motor();
  delay(50);
  if(read_value > 240)
         read_value = 240;
      if(read_value < -240)
         read_value = -240;

      if(read_value > 0.0)//Viraj dreapta
      {
         if(read_value>240){
               Serial.print( "3" );
          Serial.println( " ");            
            analogWrite(6, read_value-242);}
         else{
               Serial.print( "4" );
          Serial.println( " ");   
            analogWrite(6, 242-read_value);}

        
         analogWrite(3, 240);
      }
      if(read_value <= 0.0)//Viraj stanga
      {
         if(read_value<-240){
               Serial.print( "1" );
          Serial.println( " ");   
           
            analogWrite(3, -(240+read_value));}
         else
         {
               Serial.print( "2" );
          Serial.println( " ");   
            analogWrite(3, (240+read_value));}

         
         analogWrite(6, 242);

  
      }
}