const int pinA1 = 2;
const int pinB1 = 3;
const int pinA2 = 4;
const int pinB2 = 5;
const int pulsesPerRevolution = 80; // Cambia este valor según las especificaciones de tu encoder

volatile int encoderPos1 = 0;
volatile int encoderPos2 = 0;
int lastEncoded1 = 0;
int lastEncoded2 = 0;

void setup() {
  pinMode(pinA1, INPUT_PULLUP);
  pinMode(pinB1, INPUT_PULLUP);
  pinMode(pinA2, INPUT_PULLUP);
  pinMode(pinB2, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(pinA1), updateEncoder1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(pinB1), updateEncoder1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(pinA2), updateEncoder2, CHANGE);
  attachInterrupt(digitalPinToInterrupt(pinB2), updateEncoder2, CHANGE);

  Serial.begin(9600);
}

void loop() {
  float normalizedAngle1 = map(encoderPos1, 0, pulsesPerRevolution, 0, 360);
  float normalizedAngle2 = map(encoderPos2, 0, pulsesPerRevolution, 0, 360);

  Serial.print("Angulo Encoder 1: ");
  Serial.println(normalizedAngle1);
  Serial.print("Angulo Encoder 2: ");
  Serial.println(normalizedAngle2);

  delay(200);
}

void updateEncoder1() {
  int MSB = digitalRead(pinA1);
  int LSB = digitalRead(pinB1);

  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded1 << 2) | encoded;

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) {
    encoderPos1++;
  }
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
    encoderPos1--;
  }

  if (encoderPos1 >= pulsesPerRevolution) {
    encoderPos1 -= pulsesPerRevolution;
  } else if (encoderPos1 < 0) {
    encoderPos1 += pulsesPerRevolution;
  }

  lastEncoded1 = encoded;
}

void updateEncoder2() {
  int MSB = digitalRead(pinA2);
  int LSB = digitalRead(pinB2);

  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded2 << 2) | encoded;

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) {
    encoderPos2++;
  }
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
    encoderPos2--;
  }

  if (encoderPos2 >= pulsesPerRevolution) {
    encoderPos2 -= pulsesPerRevolution;
  } else if (encoderPos2 < 0) {
    encoderPos2 += pulsesPerRevolution;
  }

  lastEncoded2 = encoded;
}