Merge pull request #14 from KristofRobot/arduino_enc_counter

Added direct support for wheel encoders (Arduino Uno)

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

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();

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