Esp32 motor controller

mrobosub_hal/mrobosub_hal/esp32_thruster.py

@@ -0,0 +1,145 @@
+# in-progress
+import rclpy
+from rcl_interfaces.msg import ParameterDescriptor
+
+from mrobosub_lib import Node
+from serial import Serial
+from serial.serialutil import SerialException
+from mrobosub_msgs.msg import ThrusterCommands
+from std_srvs.srv import SetBool
+from typing import Optional
+
+
+NUM_PINS = 12
+FREQUENCY = 50
+
+
+class ESP32_Thruster(Node):
+    """
+    Provides requires /esp32/thruster_commands topic
+    """
+
+    def __init__(self):
+        super().__init__("esp32_thruster")
+        self.get_logger().info("Launched esp32_thruster node")
+        self.port = "/dev/thruster_esp" #IMPORTANT: DETERMINE ACTUAL PORT
+        self.thruster_outputs = [0] * NUM_PINS
+        self.serial: Serial | None = None
+        self.connect()
+
+        self.esp32_thruster_sub = self.create_subscription(
+            ThrusterCommands, "/esp32/thruster_commands", self.eps32_thruster_commands_callback, 1
+        )
+        self.emergency_stop_service = self.create_service(
+            SetBool, "emergency_stop_motors", self.handle_emergency_stop
+        )
+        self.timer = self.create_timer(1.0/FREQUENCY, self.loop)
+
+
+    def connect(self) -> bool:
+        try:
+            self.serial = Serial(self.port, timeout=0.5, write_timeout=0.5)
+            enable_all()
+        except SerialException as e:
+            self.get_logger().info(f"Could not connect to esp32_thruster: {e}")
+            return False
+        return True
+
+    def write(self, data: str) -> bool:
+        if self.serial is None:
+            success = self.connect()
+            if not success or self.serial is None:
+                return False
+        try:
+            self.serial.write(data)
+            return True
+        except SerialException as e:
+            self.get_logger().info(f"write error: {e}")
+            self.serial.close()
+            self.connect()
+        return False
+
+
+    # percent_raw should be in [-1, 1]
+    # percent_val should be in [-100, 100]
+    def convert_percent(self, percent_raw: float) -> Optional[int]:
+        if percent_raw < -1 or percent_raw > 1:
+            self.get_logger().info(
+                f"ESP32 [ERROR]: Raw input value {percent_raw} out of range (should be in [-1, 1])"
+            )
+            return None
+        return int(percent_raw * 100)
+
+    # in case of invalid percent or pin number parameters, does not send any updated signal to the pin controller
+    def send_power(self, pin: int, percent_raw: float) -> int:
+        percent_val = self.convert_percent(percent_raw)
+        if percent_val is None:
+            return False
+
+        if pin < 0 or pin >= NUM_PINS:
+            self.get_logger().info(
+                f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])"
+            )
+            return False
+
+        msg = "POWER:" + str(pin) + "," + str(percent_val)
+
+        self.write(msg)
+
+        return True
+
+
+    def send_enable_disable(self, pin: int, enable: bool) -> int:
+        if(enable == true):
+            msg = "ENABLE:TRUE"
+        else:
+            msg = "ENABLE:FALSE"
+
+        if pin < 0 or pin >= NUM_PINS:
+            self.get_logger().info(
+                f"ERROR: pin number {pin} out of range (should be in [0, {NUM_PINS-1}])"
+            )
+            return False
+
+        self.write(msg)
+        return True
+
+    def enable_all(self):
+        for i in range(NUM_PINS):
+            send_enable_disable(i, true)
+
+
+    def send_estop(self):
+        msg = "ESTOP:"
+        self.write(msg)
+
+
+    def handle_emergency_stop(
+        self, req: SetBool.Request, res: SetBool.Response
+    ) -> SetBool.Response:
+        if req.data:
+            send_estop
+        res.success = True
+        return res
+
+    def eps32_thruster_commands_callback(self, msg: ThrusterCommands):
+        for i in range(NUM_PINS):
+            if msg.valid[i]:
+                self.thruster_outputs[i] = msg.pins[i]
+
+    def loop(self):
+        for i in range(NUM_PINS):
+            self.send_power(i, self.thruster_outputs[i])
+
+
+def main():
+    rclpy.init()
+    node = ESP32_Thruster()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    main()

mrobosub_hal/mrobosub_hal/thruster_controller.py

@@ -5,7 +5,7 @@
 from serial import Serial
 from serial.serialutil import SerialException
 from mrobosub_msgs.msg import MotorState
-from mrobosub_msgs.msg import PololuCommands
+from mrobosub_msgs.msg import ThrusterCommands
 from std_srvs.srv import SetBool
 from typing import Optional
 
@@ -33,8 +33,8 @@ def __init__(self):
         self.motor_sub = self.create_subscription(
             MotorState, "/motor_output", self.motor_callback, 1
         )
-        self.pololu_pub = self.create_publisher(
-            PololuCommands, "/pololu_commands", qos_profile=1
+        self.esp32_thruster_pub = self.create_publisher(
+            ThrusterCommands, "/esp32/thruster_commands", qos_profile=1
         )
 
         params = [Param('thruster_reverse', rclpy.Parameter.Type.BOOL_ARRAY, "List of booleans indicating whether each thruster is reversed"), 
@@ -57,8 +57,8 @@ def handle_emergency_stop(
 
     # in case of invalid PWM or motor number parameters, does not send any updated signal to the motor controller
     def publish_motor_outputs(self, msg: MotorState) -> int:
-        message_valid = [False]*12
-        message_output = [0.0]*12
+        message_valid = [False]*NUM_MOTORS
+        message_output = [0.0]*NUM_MOTORS
 
         if self.emergency_stop:
             for motor in range(NUM_MOTORS):
@@ -79,12 +79,12 @@ def publish_motor_outputs(self, msg: MotorState) -> int:
                     message_output[motor_pin] = msg.motors[motor]
 
 
-        msg = PololuCommands()
+        msg = ThrusterCommands()
         message_output = np.array(message_output, dtype=np.float32)
         msg.pins = message_output
         msg.valid = message_valid
 
-        self.pololu_pub.publish(msg)
+        self.esp32_thruster_pub.publish(msg)
 
         return 0
 

mrobosub_hal/setup.py

@@ -28,7 +28,7 @@
             "dvl_publisher = mrobosub_hal.dvl_publisher:main",
             "botcam = mrobosub_hal.botcam:main",
             "zed = mrobosub_hal.zed:main",
-            "pololu = mrobosub_hal.pololu:main",
+            "esp32_thruster = mrobosub_hal.esp32_controller:main",
             "arduino = mrobosub_hal.arduino:main"
         ],
     },

mrobosub_msgs/CMakeLists.txt

@@ -19,7 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/ImuINS.msg"
     "msg/ImuPIMU.msg"
     "msg/MotorState.msg"
-    "msg/PololuCommands.msg"
+    "msg/ThrusterCommands.msg"
     "msg/Pose.msg"
     "msg/Twist.msg"
     "srv/BinCamPos.srv"

mrobosub_msgs/msg/ThrusterCommands.msg

@@ -0,0 +1,2 @@
+float32[8] pins
+bool[8] valid