volatile int encoder0Pos = 0;
void setup()
{
encoder0Pos = 0;
pinMode(encoder0PinA, INPUT);
digitalWrite(encoder0PinA, HIGH);
pinMode(encoder0PinB, INPUT);
digitalWrite(encoder0PinB, HIGH);
attachInterrupt(0, doEncoderA, CHANGE);
attachInterrupt(1, doEncoderB, CHANGE);
}
void doEncoderA()
{
if (digitalRead(encoder0PinA) == HIGH) {
if (digitalRead(encoder0PinB) == LOW) {
encoder0Pos = encoder0Pos + 1; // CW
}
else {
encoder0Pos = encoder0Pos - 1; // CCW
}
}
else
{
if (digitalRead(encoder0PinB) == HIGH) {
encoder0Pos = encoder0Pos + 1; // CW
}
else {
encoder0Pos = encoder0Pos - 1; // CCW
}
}
}
void doEncoderB()
{
if (digitalRead(encoder0PinB) == HIGH) {
if (digitalRead(encoder0PinA) == HIGH) {
encoder0Pos = encoder0Pos + 1; // CW
}
else {
encoder0Pos = encoder0Pos - 1; // CCW
}
}
else {
if (digitalRead(encoder0PinA) == LOW) {
encoder0Pos = encoder0Pos + 1; // CW
}
else {
encoder0Pos = encoder0Pos - 1; // CCW
}
}
}
void loop()
{
//Rotate
if (encoder0Pos<0)
{
encoder0Pos++;
rotate_left(1);
}
if (encoder0Pos>0)
{
encoder0Pos--;
rotate_right(1);
}
}