const byte Encoder_C1 = 2;
const byte Encoder_C2 = 8;
byte Encoder_C1Last;
int pulseNumber;
boolean direction_m;
//variaveis for PID
unsigned long lastTime;
double Input, Output, Setpoint;
double errSum, lastErr;
double kp, ki, kd;
int SampleTime = 1000; //1 sec
double outMin, outMax;
//*******************************
//Pinos de ligacao ponte H L298N
#define pino_motor1 10
#define pino_motor2 9

void setup()
{
  Serial.begin(57600);
  SetOutputLimits(-255, 255);
  SetSampleTime(1);
  SetTunings(5, 0.5, 0.01);
  //Definicao pinos ponte H
  pinMode(pino_motor1, OUTPUT);
  pinMode(pino_motor2, OUTPUT);
  //Definicao do encoder
  pinMode(Encoder_C1, INPUT);
  pinMode(Encoder_C2, INPUT);
  attachInterrupt(0, encoder, CHANGE);
}

void loop()
{
Serial.println(pulseNumber);
Serial.println(Output);

if(Output > 0){
    analogWrite(pino_motor1, Output);
    analogWrite(pino_motor2, 0);
}
   Compute();
   pulseNumber = 0;
   delay(100);
}


void encoder()
{
  int Lstate = digitalRead(Encoder_C1);
 if(!Encoder_C1Last && Lstate){
  int val = digitalRead(Encoder_C2);
  if(!val && direction_m){
    direction_m = false;
  }
  else if(val && !direction_m){
    direction_m = true;
  }

 }

Encoder_C1Last = Lstate;
if(direction_m) pulseNumber++;
else pulseNumber--;
}
void Compute()
{
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      Setpoint = 25;
      Input = pulseNumber;
      double error =  Setpoint - Input;
      errSum += error;
      double dErr = (error - lastErr);

      /*Compute PID Output*/
      Output = kp * error + ki * errSum + kd * dErr;

      /*Remember some variables for next time*/
      lastErr = error;
      lastTime = now;
   }
}

void SetTunings(double Kp, double Ki, double Kd)
{
  double SampleTimeInSec = ((double)SampleTime)/1000;
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;
}

void SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}
void SetOutputLimits(double Min, double Max)
{
   if(Min > Max) return;
   outMin = Min;
   outMax = Max;
   if(Output > outMax) Output = outMax;
   else if(Output < outMin) Output = outMin;
   if(errSum> outMax) errSum= outMax;
   else if(errSum < outMin) errSum= outMin;
}