added support for optical encoders

ROSArduinoBridge/ROSArduinoBridge.ino

@@ -60,7 +60,10 @@
    //#define ROBOGAIA
 
    /* Encoders directly attached to Arduino board */
-   #define ARDUINO_ENC_COUNTER
+   //#define ARDUINO_ENC_COUNTER
+
+   /* Transmissive Optical Sensor with Phototransistor Output with external pull down resistor*/
+   #define TOSPO_ENCODER
 
    /* L298 Motor driver*/
    #define L298_MOTOR_DRIVER
@@ -119,6 +122,12 @@
 #endif
 
 /* Variable initialization */
+#ifdef TOSPO_ENCODER
+  #include "globals.h"  // Fix capitalization to match actual filename
+  // Initialize global variables with a default direction
+  MotorDirection leftMotorDirection = STOPPED; 
+  MotorDirection rightMotorDirection = STOPPED;
+#endif
 
 // A pair of varibles to help parse serial commands (thanks Fergs)
 int arg = 0;
@@ -251,27 +260,47 @@ 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);
+  #if defined(ESP32)
+    // ESP32 specific initialization
+    #if defined(ARDUINO_ENC_COUNTER) || defined(TOSPO_ENCODER)
+      initEncoders();  // Call our new ESP32 encoder initialization function
+    #endif
+  #else
+    // Original AVR-specific code
+    #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);
+    #elif defined(TOSPO_ENCODER)
+      //set as inputs
+      DDRD &= ~(1<<LEFT_ENC_PIN_A);
+      DDRC &= ~(1<<RIGHT_ENC_PIN_A);
+      // no pull up because of external pull down
+      // tell pin change mask to listen to left encoder pin
+      PCMSK2 |= (1 << LEFT_ENC_PIN_A);
+      // tell pin change mask to listen to right encoder pin
+      PCMSK1 |= (1 << RIGHT_ENC_PIN_A);
+      // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
+      PCICR |= (1 << PCIE1) | (1 << PCIE2);
+    #endif
   #endif
+
   initMotorController();
   resetPID();
 #endif

ROSArduinoBridge/encoder_driver.h

@@ -2,19 +2,34 @@
    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
+#if defined(ESP32)
+  // ESP32 pin definitions are handled in encoder_driver.ino
+  // No need for the AVR-specific pin definitions here
+#else
+  #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
+  #elif defined(TOSPO_ENCODER)
+    #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
 #endif
 
 long readEncoder(int i);
 void resetEncoder(int i);
 void resetEncoders();
 
+#if defined(ESP32) && (defined(ARDUINO_ENC_COUNTER) || defined(TOSPO_ENCODER))
+void initEncoders();  // Add declaration for our new function
+#endif
+

ROSArduinoBridge/encoder_driver.ino

@@ -32,6 +32,46 @@
   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
 
+  #if defined(ESP32)
+  // ESP32 specific encoder implementation
+  // Define the pins used for encoders
+  const int LEFT_ENC_A = 2;  // Adjust these pin numbers to match your wiring
+  const int LEFT_ENC_B = 3;
+  const int RIGHT_ENC_A = 4;
+  const int RIGHT_ENC_B = 5;
+
+  void IRAM_ATTR leftEncoderISR() {
+    static uint8_t enc_last = 0;
+    uint8_t enc_current = (digitalRead(LEFT_ENC_A) << 1) | digitalRead(LEFT_ENC_B);
+
+    enc_last <<= 2;
+    enc_last |= enc_current;
+
+    left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+
+  void IRAM_ATTR rightEncoderISR() {
+    static uint8_t enc_last = 0;
+    uint8_t enc_current = (digitalRead(RIGHT_ENC_A) << 1) | digitalRead(RIGHT_ENC_B);
+
+    enc_last <<= 2;
+    enc_last |= enc_current;
+
+    right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+
+  void setupEncoders() {
+    pinMode(LEFT_ENC_A, INPUT_PULLUP);
+    pinMode(LEFT_ENC_B, INPUT_PULLUP);
+    pinMode(RIGHT_ENC_A, INPUT_PULLUP);
+    pinMode(RIGHT_ENC_B, INPUT_PULLUP);
+
+    attachInterrupt(digitalPinToInterrupt(LEFT_ENC_A), leftEncoderISR, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(LEFT_ENC_B), leftEncoderISR, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_A), rightEncoderISR, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_B), rightEncoderISR, CHANGE);
+  }
+  #else
   /* Interrupt routine for LEFT encoder, taking care of actual counting */
   ISR (PCINT2_vect){
   	static uint8_t enc_last=0;
@@ -51,6 +91,70 @@
 
   	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
   }
+  #endif
+
+  /* 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;
+    }
+  }
+#elif defined(TOSPO_ENCODER)
+  #include "globals.h"
+  volatile long left_enc_pos = 0L;
+  volatile long right_enc_pos = 0L;
+
+  #if defined(ESP32)
+  // ESP32 specific encoder implementation for TOSPO
+  const int LEFT_ENC_PIN = 2;  // Adjust these pin numbers to match your wiring
+  const int RIGHT_ENC_PIN = 4;
+
+  // External variables from Globals.h
+  extern int leftMotorDirection;
+  extern int rightMotorDirection;
+  extern const int FORWARD;
+  extern const int BACKWARD;
+
+  void IRAM_ATTR leftEncoderISR() {
+    if (leftMotorDirection == FORWARD) { left_enc_pos++; } 
+    else if (leftMotorDirection == BACKWARD) { left_enc_pos--; }
+  }
+
+  void IRAM_ATTR rightEncoderISR() {
+    if (rightMotorDirection == FORWARD) { right_enc_pos++; } 
+    else if (rightMotorDirection == BACKWARD) { right_enc_pos--; }
+  }
+
+  void setupEncoders() {
+    pinMode(LEFT_ENC_PIN, INPUT_PULLUP);
+    pinMode(RIGHT_ENC_PIN, INPUT_PULLUP);
+
+    attachInterrupt(digitalPinToInterrupt(LEFT_ENC_PIN), leftEncoderISR, CHANGE);
+    attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_PIN), rightEncoderISR, CHANGE);
+  }
+  #else
+  /* Interrupt routine for LEFT encoder, taking care of actual counting */
+  ISR (PCINT2_vect){
+    if (leftMotorDirection == FORWARD) { left_enc_pos++; } 
+    else if (leftMotorDirection == BACKWARD) { left_enc_pos--; }
+  }
+
+  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
+  ISR (PCINT1_vect){
+    if (rightMotorDirection == FORWARD) { right_enc_pos++; } 
+    else if (rightMotorDirection == BACKWARD) { right_enc_pos--; }
+  }
+  #endif
 
   /* Wrap the encoder reading function */
   long readEncoder(int i) {
@@ -78,5 +182,12 @@ void resetEncoders() {
   resetEncoder(RIGHT);
 }
 
+#if defined(ESP32) && (defined(ARDUINO_ENC_COUNTER) || defined(TOSPO_ENCODER))
+// This function needs to be called in the setup() function of the main sketch
+void initEncoders() {
+  setupEncoders();
+}
+#endif
+
 #endif
 

ROSArduinoBridge/globals.h

@@ -0,0 +1,16 @@
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+// Define motor direction states
+enum MotorDirection {
+  STOPPED,
+  FORWARD,
+  BACKWARD
+};
+
+// Initialize direction state variables for each motor
+extern MotorDirection leftMotorDirection;
+extern MotorDirection rightMotorDirection;
+
+
+#endif

ROSArduinoBridge/motor_driver.ino

@@ -72,13 +72,15 @@
     if (spd > 255)
       spd = 255;
 
-    if (i == LEFT) { 
-      if      (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
-      else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
+    if (i == LEFT) {
+      if      (reverse == 0 && spd > 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); leftMotorDirection = FORWARD;}
+      else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); leftMotorDirection = BACKWARD;}
+      else { analogWrite(LEFT_MOTOR_FORWARD, 0); analogWrite(LEFT_MOTOR_BACKWARD, 0); leftMotorDirection = STOPPED; }
     }
     else /*if (i == RIGHT) //no need for condition*/ {
-      if      (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
-      else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
+      if      (reverse == 0 && spd > 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); rightMotorDirection = FORWARD;}
+      else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); rightMotorDirection = BACKWARD;}
+      else { analogWrite(RIGHT_MOTOR_BACKWARD, 0); analogWrite(RIGHT_MOTOR_FORWARD, 0); rightMotorDirection = STOPPED; }
     }
   }