From 0ac44d6dd47b7f396719ceb58e99aabefd4656e1 Mon Sep 17 00:00:00 2001
From: Jonas Fleischer <jonas.fleischer@lila-it.de>
Date: Fri, 29 Mar 2024 18:19:27 +0100
Subject: [PATCH 1/2] added support for optical encoders

---
 ROSArduinoBridge/ROSArduinoBridge.ino | 22 +++++++++++++++++-
 ROSArduinoBridge/encoder_driver.h     |  7 ++++++
 ROSArduinoBridge/encoder_driver.ino   | 33 +++++++++++++++++++++++++++
 ROSArduinoBridge/globals.h            | 16 +++++++++++++
 ROSArduinoBridge/motor_driver.ino     | 12 ++++++----
 5 files changed, 84 insertions(+), 6 deletions(-)
 create mode 100644 ROSArduinoBridge/globals.h

diff --git a/ROSArduinoBridge/ROSArduinoBridge.ino b/ROSArduinoBridge/ROSArduinoBridge.ino
index d851d6f9..7e32e179 100644
--- a/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/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"
+  // 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;
@@ -269,6 +278,17 @@ void setup() {
     // 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
diff --git a/ROSArduinoBridge/encoder_driver.h b/ROSArduinoBridge/encoder_driver.h
index 8d35ab1c..e2ab0040 100644
--- a/ROSArduinoBridge/encoder_driver.h
+++ b/ROSArduinoBridge/encoder_driver.h
@@ -12,6 +12,13 @@
   //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
    
 long readEncoder(int i);
diff --git a/ROSArduinoBridge/encoder_driver.ino b/ROSArduinoBridge/encoder_driver.ino
index ef2b8b11..8872b255 100644
--- a/ROSArduinoBridge/encoder_driver.ino
+++ b/ROSArduinoBridge/encoder_driver.ino
@@ -58,6 +58,39 @@
     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;
+
+  /* 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--; }
+  }  
+  
+  /* 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){
diff --git a/ROSArduinoBridge/globals.h b/ROSArduinoBridge/globals.h
new file mode 100644
index 00000000..3e142e12
--- /dev/null
+++ b/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
diff --git a/ROSArduinoBridge/motor_driver.ino b/ROSArduinoBridge/motor_driver.ino
index 3d8b6fe8..aa44eada 100644
--- a/ROSArduinoBridge/motor_driver.ino
+++ b/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; }
     }
   }
   

From bb0c907c680252a1f8bcc7f5195a7e0c7e0a5108 Mon Sep 17 00:00:00 2001
From: Jonas Flesicher <jnsflschr@gmail.com>
Date: Mon, 3 Mar 2025 18:56:36 +0100
Subject: [PATCH 2/2] add support for nano esp32

---
 ROSArduinoBridge/ROSArduinoBridge.ino | 73 +++++++++++++-----------
 ROSArduinoBridge/encoder_driver.h     | 42 ++++++++------
 ROSArduinoBridge/encoder_driver.ino   | 82 ++++++++++++++++++++++++++-
 3 files changed, 146 insertions(+), 51 deletions(-)

diff --git a/ROSArduinoBridge/ROSArduinoBridge.ino b/ROSArduinoBridge/ROSArduinoBridge.ino
index 7e32e179..c101f676 100644
--- a/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -123,7 +123,7 @@
 
 /* Variable initialization */
 #ifdef TOSPO_ENCODER
-  #include "Globals.h"
+  #include "globals.h"  // Fix capitalization to match actual filename
   // Initialize global variables with a default direction
   MotorDirection leftMotorDirection = STOPPED; 
   MotorDirection rightMotorDirection = STOPPED;
@@ -260,38 +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);
-  #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);
+  #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
diff --git a/ROSArduinoBridge/encoder_driver.h b/ROSArduinoBridge/encoder_driver.h
index e2ab0040..52ab9345 100644
--- a/ROSArduinoBridge/encoder_driver.h
+++ b/ROSArduinoBridge/encoder_driver.h
@@ -2,26 +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
-#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
+#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
+
diff --git a/ROSArduinoBridge/encoder_driver.ino b/ROSArduinoBridge/encoder_driver.ino
index 8872b255..59688cef 100644
--- a/ROSArduinoBridge/encoder_driver.ino
+++ b/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,7 @@
   
   	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
   }
+  #endif
   
   /* Wrap the encoder reading function */
   long readEncoder(int i) {
@@ -69,10 +110,39 @@
     }
   }
 #elif defined(TOSPO_ENCODER)
-  #include "Globals.h"
+  #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++; } 
@@ -83,7 +153,8 @@
   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) {
@@ -111,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