/*
* Encoder example sketch
* by Andrew Kramer
* 1/1/2016
*
* Records encoder ticks for each wheel
* and prints the number of ticks for
* each encoder every 500ms
*
*/

#define SERIAL_BAUD 57600
#define MOTORA_SPEED 200
#define MOTORB_SPEED 180

// pins for the encoder inputs
#define RH_ENCODER_A 18
#define RH_ENCODER_B 19
#define LH_ENCODER_A 2
#define LH_ENCODER_B 3

// variables to store the number of encoder pulses
// for each motor
volatile double leftCount = 0;
volatile double rightCount = 0;
int motorAenA = 10;
int motorAIN1 = 9;
int motorAIN2 = 8;

int motorBIN3 = 7;
int motorBIN4 = 6;
int motorBenB = 5;
void setup() {

Serial.begin(SERIAL_BAUD);
Serial.print(“Starting…”);
delay(5000);
pinMode(LH_ENCODER_A, INPUT);
pinMode(LH_ENCODER_B, INPUT);

pinMode(RH_ENCODER_A, INPUT);
pinMode(RH_ENCODER_B, INPUT);

// initialize hardware interrupts
attachInterrupt(0, leftEncoderEvent, CHANGE);
attachInterrupt(5, rightEncoderEvent, CHANGE);

pinMode(motorAIN1, OUTPUT);
pinMode(motorAIN2, OUTPUT);

pinMode(motorBIN3, OUTPUT);
pinMode(motorBIN4, OUTPUT);

}

void loop() {

analogWrite(motorAenA, MOTORA_SPEED);
analogWrite(motorBenB, MOTORB_SPEED);

digitalWrite(motorAIN1, HIGH);
digitalWrite(motorAIN2, LOW);

digitalWrite(motorBIN3, HIGH);
digitalWrite(motorBIN4, LOW);

Serial.print(“Right Count: “);
Serial.println(rightCount);
Serial.print(“Left Count: “);
Serial.print(leftCount);
Serial.println();
delay(500);
}

// encoder event for the interrupt call
void leftEncoderEvent() {
//Serial.print(“esquerdo”);
if (digitalRead(LH_ENCODER_A) == HIGH) {
if (digitalRead(LH_ENCODER_B) == LOW) {
leftCount++;
} else {
leftCount–;
}
} else {
if (digitalRead(LH_ENCODER_B) == LOW) {
leftCount–;
} else {
leftCount++;
}
}
}

// encoder event for the interrupt call
void rightEncoderEvent() {
if (digitalRead(RH_ENCODER_A) == HIGH) {
if (digitalRead(RH_ENCODER_B) == LOW) {
rightCount++;
} else {
rightCount–;
}
} else {
if (digitalRead(RH_ENCODER_B) == LOW) {
rightCount–;
} else {
rightCount++;
}
}
}