From 4d399627e6f97a42f46185463f96108660d5edcc Mon Sep 17 00:00:00 2001
From: Kristof Robot <krirobo@gmail.com>
Date: Sun, 1 Sep 2013 21:16:08 +0200
Subject: [PATCH] Added direct support for wheel encoders (Arduino Uno)

Added support for wheel encoder counting directly on the main Arduino board.
This allows connecting wheel encoders directly to the Arduino board,
without the need for any additional wheel encoder counter equipment.

For speed, the code is directly addressing the specific Atmega328p ports and interrupts, making this implementation Atmega328p (Arduino Uno) dependent.
It should be easy to adapt for other boards/AVR chips though.
---
 .../ROSArduinoBridge/ROSArduinoBridge.ino     | 24 +++++++++++
 .../ROSArduinoBridge/encoder_driver.h         | 12 ++++++
 .../ROSArduinoBridge/encoder_driver.ino       | 41 +++++++++++++++++++
 3 files changed, 77 insertions(+)

diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 77322d2..ff8bfe1 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -58,6 +58,9 @@
 
    /* The RoboGaia encoder shield */
    #define ROBOGAIA
+   
+   /* Encoders directly attached to Arduino board */
+   //#define ARDUINO_ENC_COUNTER
 #endif
 
 //#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
@@ -236,6 +239,27 @@ void setup() {
 
 // Initialize the motor controller if used */
 #ifdef USE_BASE
+  #ifdef ARDUINO_ENC_COUNTER
+    //set as inputs
+    DDRD &= ~(1<<LEFT_ENC_PIN_A);
+    DDRD &= ~(1<<LEFT_ENC_PIN_B);
+    DDRC &= ~(1<<RIGHT_ENC_PIN_A);
+    DDRC &= ~(1<<RIGHT_ENC_PIN_B);
+    
+    //enable pull up resistors
+    PORTD |= (1<<LEFT_ENC_PIN_A);
+    PORTD |= (1<<LEFT_ENC_PIN_B);
+    PORTC |= (1<<RIGHT_ENC_PIN_A);
+    PORTC |= (1<<RIGHT_ENC_PIN_B);
+    
+    // tell pin change mask to listen to left encoder pins
+    PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
+    // tell pin change mask to listen to right encoder pins
+    PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
+    
+    // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
+    PCICR |= (1 << PCIE1) | (1 << PCIE2);
+  #endif
   initMotorController();
   resetPID();
 #endif
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
index 033d373..8d35ab1 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
@@ -2,6 +2,18 @@
    Encoder driver function definitions - by James Nugen
    ************************************************************ */
    
+   
+#ifdef ARDUINO_ENC_COUNTER
+  //below can be changed, but should be PORTD pins; 
+  //otherwise additional changes in the code are required
+  #define LEFT_ENC_PIN_A PD2  //pin 2
+  #define LEFT_ENC_PIN_B PD3  //pin 3
+  
+  //below can be changed, but should be PORTC pins
+  #define RIGHT_ENC_PIN_A PC4  //pin A4
+  #define RIGHT_ENC_PIN_B PC5   //pin A5
+#endif
+   
 long readEncoder(int i);
 void resetEncoder(int i);
 void resetEncoders();
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
index af9434d..294cdb3 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
@@ -27,6 +27,47 @@
     if (i == LEFT) return encoders.YAxisReset();
     else return encoders.XAxisReset();
   }
+#elif defined ARDUINO_ENC_COUNTER
+  volatile long left_enc_pos = 0L;
+  volatile long right_enc_pos = 0L;
+  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};  //encoder lookup table
+    
+  /* Interrupt routine for LEFT encoder, taking care of actual counting */
+  ISR (PCINT2_vect){
+  	static uint8_t enc_last=0;
+        
+	enc_last <<=2; //shift previous state two places
+	enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
+  
+  	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+  
+  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
+  ISR (PCINT1_vect){
+        static uint8_t enc_last=0;
+          	
+	enc_last <<=2; //shift previous state two places
+	enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
+  
+  	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+  
+  /* Wrap the encoder reading function */
+  long readEncoder(int i) {
+    if (i == LEFT) return left_enc_pos;
+    else return right_enc_pos;
+  }
+
+  /* Wrap the encoder reset function */
+  void resetEncoder(int i) {
+    if (i == LEFT){
+      left_enc_pos=0L;
+      return;
+    } else { 
+      right_enc_pos=0L;
+      return;
+    }
+  }
 #else
   #error A encoder driver must be selected!
 #endif