Feature/encoders implementation and testing

archive/prox_test.py

@@ -0,0 +1,27 @@
+import Jetson.GPIO as GPIO
+import time
+
+GPIO.setwarnings(False)
+
+# set correct channel
+prox_channel = 17
+
+GPIO.setmode(GPIO.BCM)
+GPIO.setup(prox_channel, GPIO.IN)
+
+def update_proximity(channel):
+    prox_detected = GPIO.input(channel)
+
+    print(f"Proximity: {prox_detected}")
+
+GPIO.add_event_detect(prox_channel, GPIO.BOTH, callback=update_proximity)
+
+try:
+    while True:
+        time.sleep(1)
+
+except KeyboardInterrupt:
+    print("Program terminated.")
+
+finally:
+    GPIO.cleanup()

archive/working_test_encoder.py

@@ -0,0 +1,111 @@
+import Jetson.GPIO as GPIO
+import time
+
+# Disable warnings
+GPIO.setwarnings(False)
+
+# Define GPIO pins for A+ / A- and B+ / B-
+A_plus = 17
+A_minus = 18
+B_plus = 27
+B_minus = 22
+
+# Set up GPIO mode
+GPIO.setmode(GPIO.BCM)
+
+# Set up GPIO pins as inputs without pull-up/down resistors
+GPIO.setup(A_plus, GPIO.IN)
+GPIO.setup(A_minus, GPIO.IN)
+GPIO.setup(B_plus, GPIO.IN)
+GPIO.setup(B_minus, GPIO.IN)
+
+position = 0
+last_A_plus = GPIO.input(A_plus)
+last_B_plus = GPIO.input(B_plus)
+
+def update_encoder(channel):
+    global position, last_A_plus, last_B_plus
+
+    # Read the current states of A+ / A-, and B+ / B-
+    A_plus_state = GPIO.input(A_plus)
+    A_minus_state = GPIO.input(A_minus)
+    B_plus_state = GPIO.input(B_plus)
+    B_minus_state = GPIO.input(B_minus)
+
+    # Correct differential signaling processing (checking if A+ is inverted from A-)
+    if A_plus_state != A_minus_state and B_plus_state != B_minus_state:
+        # Determine direction based on phase shift between A+ and B+
+        if last_A_plus == 0 and A_plus_state == 1:  # Rising edge of A+
+            if B_plus_state == 0:
+                position += 1  # Clockwise (CW)
+            else:
+                position -= 1  # Counter-Clockwise (CCW)
+        elif last_B_plus == 0 and B_plus_state == 1:  # Rising edge of B+
+            if A_plus_state == 1:
+                position += 1  # Clockwise (CW)
+            else:
+                position -= 1  # Counter-Clockwise (CCW)
+
+    last_A_plus = A_plus_state
+    last_B_plus = B_plus_state
+
+    print(f"Position: {position}")
+
+# Set up interrupts for A+ and B+ signals
+GPIO.add_event_detect(A_plus, GPIO.BOTH, callback=update_encoder)
+GPIO.add_event_detect(B_plus, GPIO.BOTH, callback=update_encoder)
+
+try:
+    while True:
+        time.sleep(1)  # Keeping the script alive for event detection
+
+except KeyboardInterrupt:
+    print("Program terminated.")
+
+finally:
+    GPIO.cleanup()
+
+
+"""
+import serial
+import Jetson.GPIO as GPIO
+import time
+
+# Set up GPIO mode
+GPIO.setmode(GPIO.BCM)
+
+# Define GPIO pins
+A_plus = 17
+A_minus = 18
+B_plus = 27
+B_minus = 22
+
+# Set up GPIO pins as inputs
+GPIO.setup(A_plus, GPIO.IN)
+GPIO.setup(A_minus, GPIO.IN)
+GPIO.setup(B_plus, GPIO.IN)
+GPIO.setup(B_minus, GPIO.IN)
+
+try:
+    while True:
+        # Read the encoder signals
+        A_plus_state = GPIO.input(A_plus)
+        A_minus_state = GPIO.input(A_minus)
+        B_plus_state = GPIO.input(B_plus)
+        B_minus_state = GPIO.input(B_minus)
+
+        # Process the encoder signals
+        # (Implement your logic here)
+        #        print("A+: {A_plus_state}, A-: {A_minus_state}, B+: {B_plus_state}, B-: {B_minus_state}")
+        print("[A+: ", A_plus_state, ", A-: ", A_minus_state, "B+: ", B_plus_state, "B-: ", B_minus_state,"]")
+
+        time.sleep(0.01)  # Adjust the sleep time as needed
+
+except KeyboardInterrupt:
+    print("Program terminated.")
+
+finally:
+    GPIO.cleanup()
+"""
+
+

src/odometry.py

@@ -2,8 +2,8 @@
 
 import rospy
 from geometry_msgs.msg import Twist
-
-# TODO: Write logic for reading from modules and publishing
+from utils.encoder import Encoder
+import time
 
 class Odometry():
     """
@@ -13,16 +13,37 @@ class Odometry():
     """
     def __init__(self):
         try:
+            # TODO: update encoder pins
+            self.encoder_back = Encoder(17, 18, 27, 22)
+            self.encoder_side_left = Encoder(17, 18, 27, 22)
+            self.encoder_side_right = Encoder(17, 18, 27, 22)
+            self.position = 0
+
             rospy.init_node("odometry", anonymous=False)
             self.rate = rospy.Rate(10)
 
         except Exception as e:
             raise
 
     def odometry_publish(self):
-        shifter_publish = rospy.Publisher("odometry_publish", Twist, queue_size=10)
+        odometry_publish = rospy.Publisher("odometry_publish", Twist, queue_size=10)
+
+        # TODO: compute position
+        position = Twist()
 
         while not rospy.is_shutdown():
+            try:
+
+                # Read encoder data
+                self.encoder_back.update_encoder()
+                self.encoder_side_left.update_encoder()
+                self.encoder_side_right.update_encoder()
+
+                # Compute position and publish
+                odometry_publish.publish(position)
+            except Exception as e:
+                raise
+
             self.rate.sleep()
 
 # Execute odometry

src/proximity.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import rospy
+import Jetson.GPIO as GPIO
+from std_msgs.msg import Bool
+
+class ProximitySensor:
+    """
+    Proximity Sensor class reads data from an NPN proximity sensor
+    and publishes the detection state to a ROS topic.
+    """
+    def __init__(self):
+        try:
+            self.prox_channel = 17  # TODO: Update with actual GPIO pin
+            GPIO.setmode(GPIO.BCM)
+            GPIO.setup(self.prox_channel, GPIO.IN)
+
+            # Initialize the ROS node
+            rospy.init_node("proximity_sensor", anonymous=False)
+            self.pub = rospy.Publisher("proximity_detected", Bool, queue_size=10)
+
+            # Set up event detection for changes
+            GPIO.add_event_detect(self.prox_channel, GPIO.BOTH, callback=self.sensor_callback) # TODO: Update with correct RISING/FALL detection
+
+            self.rate = rospy.Rate(10)  # Control loop rate (10Hz)
+
+        except Exception as e:
+            rospy.logerr(f"Error initializing ProximitySensor: {e}")
+
+    def sensor_callback(self, channel):
+        """
+        Callback function triggered on rising/falling edges of the proximity sensor.
+        Reads sensor state and publishes it.
+        """
+        detected = GPIO.input(channel)
+        rospy.loginfo(f"Proximity detected: {bool(detected)}")
+        self.pub.publish(bool(detected))
+
+    def run(self):
+        """
+        Keeps the node alive while monitoring the sensor.
+        """
+        try:
+            while not rospy.is_shutdown():
+                # Keep the loop alive and allow the callback to handle the code
+                self.rate.sleep()
+        except KeyboardInterrupt:
+            rospy.loginfo("Shutting down proximity sensor node")
+        finally:
+            GPIO.cleanup()
+
+if __name__ == "__main__":
+    try:
+        prox_sensor = ProximitySensor()
+        prox_sensor.run()
+    except rospy.ROSInterruptException:
+        pass
+

src/utils/encoder.py

@@ -0,0 +1,43 @@
+import Jetson.GPIO as GPIO
+
+GPIO.setwarnings(False)
+GPIO.setmode(GPIO.BCM)
+
+class Encoder:
+    def __init__(self, a_plus: int, a_minus: int, b_plus: int, b_minus: int):
+        self.a_plus = a_plus
+        self.a_minus = a_minus
+        self.b_plus = b_plus
+        self.b_minus = b_minus
+        self.position = 0
+
+        GPIO.setup(self.a_plus, GPIO.IN)
+        GPIO.setup(self.a_minus, GPIO.IN)
+        GPIO.setup(self.b_plus, GPIO.IN)
+        GPIO.setup(self.b_minus, GPIO.IN)
+
+    def cleanup(self):
+        GPIO.cleanup([self.a_plus, self.a_minus, self.b_plus, self.b_minus]) 
+
+    def update_encoder(self):
+        global position, last_A_plus, last_B_plus
+
+        A_plus_state = GPIO.input(self.a_plus)
+        A_minus_state = GPIO.input(self.a_minus)
+        B_plus_state = GPIO.input(self.b_plus)
+        B_minus_state = GPIO.input(self.b_minus)
+
+        if A_plus_state != A_minus_state and B_plus_state != B_minus_state:
+            if last_A_plus == 0 and A_plus_state == 1:
+                if B_plus_state == 0:
+                    position += 1
+                else:
+                    position -= 1
+            elif last_B_plus == 0 and B_plus_state == 1:
+                if A_plus_state == 1:
+                    position += 1
+                else:
+                    position -= 1
+
+        last_A_plus = A_plus_state
+        last_B_plus = B_plus_state