#include <LiquidCrystal_I2C.h>
#include <Wire.h>

// Inicializamos las variables.
LiquidCrystal_I2C lcd(0x27, 16,2); // 0x3F 0x27


int sensor01;
int sensor02;

int stepOld;
int step;

long PulseCount;
float PulsePost;

float dis_recorrida ; 
float dia_rueda =200;
int Encoder_pulsos=20;
#define SENSOR_A 3
#define SENSOR_B 2


void checkState(){
  sensor01 = digitalRead(SENSOR_A);
  sensor02 = digitalRead(SENSOR_B);

  if(sensor01 == 1 && sensor02 == 1){
    step = 0;
    if(stepOld == 1){
      PulseCount--;
    }
    if(stepOld == 3){
      PulseCount++;
    }
    stepOld = 0;
  }

  if(sensor01 == 0 && sensor02 == 1){
    step = 1;
    if(stepOld == 2){
      PulseCount--;
    }
    if(stepOld == 0){
      PulseCount++;
    }
    stepOld = 1;
  }

  if(sensor01 == 0 && sensor02 == 0){
    step = 2;
    if(stepOld == 3){
      PulseCount--;
    }
    if(stepOld == 1){
      PulseCount++;
    }
    stepOld = 2;
  }

  if(sensor01 == 1 && sensor02 == 0){
    step = 3;
    if(stepOld == 0){
      PulseCount--;
    }
    if(stepOld == 2){
      PulseCount++;
    }
    stepOld = 3;
  }

}

void setup() {
  // set up the LCD's number of columns and rows:
  lcd.init();
  lcd.backlight();
  lcd.clear();
 

  attachInterrupt(0, checkState, CHANGE);
  attachInterrupt(1, checkState, CHANGE);



  PulseCount = 0;
  PulsePost = 0;
}

void loop() {

  // set the cursor to column 0, line 1
  // (note: line 1 is the second row, since counting begins with 0):
 PulsePost= ((((dia_rueda*PI)/Encoder_pulsos)*(PulseCount/4))/10);
  lcd.setCursor(0, 0);



  lcd.print("Dist:");


  if(PulsePost > -1){
    lcd.print(" ");
  }

  if(PulsePost< 10 && PulsePost > -10){
    lcd.print(" ");
  }

  if(PulsePost < 100 && PulsePost > -100){
    lcd.print(" ");
  }

  lcd.print(PulsePost);

  lcd.setCursor(14,0 );
  lcd.print("Cm");
  

}