Interfacing Pipeline

python/hlc/ball_position.py

@@ -0,0 +1,10 @@
+"""Some module that gets the ball position from CV"""
+import numpy as np
+import numpy.typing as npt
+
+def get_ball_position() -> npt.NDArray:
+    """Helper functino to interface with the CV binary to get the current ball position
+
+    Returns:
+        2 float vector of [x, y] position"""
+    return np.array([0, 0])

python/hlc/main.py

@@ -0,0 +1,49 @@
+# Python imports
+import numpy as np
+import time
+
+# Module imports
+from ball_position import get_ball_position
+from kinematics.constants import REST_MOTOR_ANGLE
+from kinematics.wrappers import translate_dir_to_motor_angles
+from pid.position_feedback import Controller
+from serial.motor_serial import MotorSerial
+
+
+def main():
+    # Setup
+    desired_coord = np.array([0,0])
+    pid = Controller()
+    motor_serial = MotorSerial()
+
+    # Homing
+    print("Starting homing sequence")
+    print("Taring motors")
+    motor_serial.tare_motors((REST_MOTOR_ANGLE, REST_MOTOR_ANGLE, REST_MOTOR_ANGLE))
+
+    # Main pipeline
+    print("Starting the main pipeline")
+    """CURRENTLY JUST BALANCING THE BALL TO THE MIDDLE"""
+    try:
+        while True:
+            desired_coord = np.array([0, 0])
+            actual_coord = get_ball_position()
+            dir_x, dir_y, theta_mag = pid.calculate(desired_coord, actual_coord)
+            motor_angles = translate_dir_to_motor_angles(dir_x, dir_y, theta_mag)
+            motor_serial.send_encoded_motor_commands(motor_angles)
+
+            print(f"PID VECTOR: {(dir_x, dir_y, theta_mag)}")
+            print(f"TX ANGLE: {motor_angles}")    
+
+            time.sleep(0.01)
+
+    except KeyboardInterrupt:
+        print("User Interrupt, exiting out of main loop")
+
+    motor_serial.close()
+
+
+
+
+if __name__ == "__main__":
+    main()

python/kinematics/constants.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+"""Mechanical Constants"""
+MOTOR_LEG_LENGTH = 5 # cm
+PLATE_LEG_LENGTH = 8 # cm
+MOTOR_DIST_FROM_ORIGIN = 11 # cm
+PLATE_DIST_FROM_ORIGIN = 15 # cm
+
+DEFAULT_PLATE_HEIGHT = 8 # cm, rel to shaft
+
+# See `motor_orientations.png`
+MOTOR_ORIENTATIONS = [
+    np.pi/2, # Motor A: along the y-axis
+    7*np.pi/6, # Motor B: 120 deg, CCW from A
+    11*np.pi/6, # Motor C: 120, CW from A
+]
+
+# HELPER CONSTANTS
+UNIT_K = np.array([0, 0, 1])
+
+
+"""Homing Constants"""
+REST_MOTOR_ANGLE = -60 # deg

python/kinematics/motor.py

@@ -1,9 +1,5 @@
 import numpy as np
-
-MOTOR_LEG_LENGTH = 5 # cm
-PLATE_LEG_LENGTH = 8 # cm
-MOTOR_DIST_FROM_ORIGIN = 11 # cm
-PLATE_DIST_FROM_ORIGIN = 15 # cm
+from constants import MOTOR_LEG_LENGTH, PLATE_LEG_LENGTH, MOTOR_DIST_FROM_ORIGIN, PLATE_DIST_FROM_ORIGIN
 
 class Motor:
     """Class defining a motor's orientation and leg details"""

python/kinematics/plate_kinematics.py

@@ -2,19 +2,15 @@
 
 import numpy as np
 import numpy.typing as npt
-from kinematics.motor import Motor, MOTOR_LEG_LENGTH, PLATE_LEG_LENGTH
+from kinematics.motor import Motor
+from kinematics.constants import (
+    MOTOR_LEG_LENGTH,
+    PLATE_LEG_LENGTH,
+    DEFAULT_PLATE_HEIGHT,
+    MOTOR_ORIENTATIONS,
+    UNIT_K
+)
 
-DEFAULT_PLATE_HEIGHT = 8 # cm, rel to shaft
-
-# See `motor_orientations.png`
-MOTOR_ORIENTATIONS = [
-    np.pi/2, # Motor A: along the y-axis
-    7*np.pi/6, # Motor B: 120 deg, CCW from A
-    11*np.pi/6, # Motor C: 120, CW from A
-]
-
-# HELPER CONSTANTS
-UNIT_K = np.array([0, 0, 1])
 
 def calculate_normal_from_dir_vec(dir_vec: npt.NDArray, mag: float) -> npt.NDArray:
     """Helper function to calculate the normal vector of the place given a direction and magnitude

python/kinematics/wrappers.py

@@ -2,10 +2,6 @@
 
 from kinematics.plate_kinematics import *
 
-def get_plate_height():
-    # File I/O based on Thomason's CV binary
-    return DEFAULT_PLATE_HEIGHT
-
 def translate_dir_to_motor_angles(x_dir: float, y_dir: float, mag: float) -> tuple[float, float, float]:
     """Wrapper function for movement kinematics: Translate x-y coordiantes to a vector, then return motor angles
     that tilt the plane in that direction, about a specified magnitude
@@ -21,11 +17,11 @@ def translate_dir_to_motor_angles(x_dir: float, y_dir: float, mag: float) -> tup
 
     dir = np.array([x_dir, y_dir])
     N = calculate_normal_from_dir_vec(dir, mag)
-    motor_angles = translate_N_to_motor_angles(N)
+    motor_angles = translate_N_to_motor_angles(N, DEFAULT_PLATE_HEIGHT)
     return motor_angles
 
 
-def translate_N_to_motor_angles(N_norm: npt.NDArray) -> tuple[float, float, float]:
+def translate_N_to_motor_angles(N_norm: npt.NDArray, distance: float) -> tuple[float, float, float]:
     """Wrapper function for homing: Translate a normal vector and distance, and return motor angles that give the plane 
     it's current tilt
 
@@ -35,10 +31,9 @@ def translate_N_to_motor_angles(N_norm: npt.NDArray) -> tuple[float, float, floa
     Returns: 
     Tuple[float, float, float]: Absolute angles (float) of motor A, B, C
     """
-
     motors = [Motor(orientation) for orientation in MOTOR_ORIENTATIONS]
     phi_x, theta_y = calculate_theta_phi_from_N(N_norm)
-    T = np.array([0, 0, get_plate_height()])
+    T = np.array([0, 0, distance])
     for motor in motors:
         li = calculate_li(T, theta_y, phi_x, pi_plat=motor.PLATE_ORIENTATION_VECTOR, bi = motor.MOTOR_ORIENTATION_VECTOR)
         abs_angle = calculate_abs_motor_angle_from_li(li)

python/pid/position_feedback.py

@@ -83,4 +83,4 @@ def calculate(self, desired_pos: tuple[float, float], actual_pos: tuple[float, f
         # Saturate plate tilt and convert to radians
         sat_theta_mag = math.radians(saturate(theta_mag, self.sat_min, self.sat_max))
 
-        return dir_x, dir_y, sat_theta_mag
+        return dir_x, dir_y, sat_theta_mag

python/pyproject.toml

@@ -8,12 +8,13 @@ description = "MTE380 Python Software"
 version = "0.0.1"
 dependencies = [
   "numpy",
-  "pytest"
+  "pytest",
+  "pyserial"
 ]
 requires-python = ">= 3.10"
 authors = [
   {name = "voat", email = "[email protected]"},
 ]
 
 [tool.setuptools]
-packages = ["kinematics"]
+packages = ["kinematics", "tele-op", "pid"]

python/serial/motor_serial.py

@@ -0,0 +1,79 @@
+import serial
+import platform
+import time
+
+tare_timeout = 1
+
+class MotorSerial:
+
+    def __init__(self, baudrate: int):
+        """Initialize the required serial ports
+
+        Args:
+            baudrate (int): Serial baudrate
+        """
+        self.baudrate = baudrate
+        computeOS = platform.system()
+        if computeOS == 'Linux':
+            output_com_port = '/dev/ttyUSB0'
+        elif computeOS == 'Windows':
+            output_com_port = 'COM5'
+
+        try:
+            motor_serial = serial.Serial(output_com_port, baudrate, timeout=1)
+        except serial.SerialException as e:
+            print(f"Error: {e}")
+            raise Exception("Failed to initalize serial!")
+        else:
+            print(f"Serial Initialized")
+            self.motor_serial = motor_serial
+
+    def send_encoded_motor_commands(self, motor_command: str, motor_angles: tuple[float, float, float]):
+        """ASCII encodes a given motor command and set of motor angles in the expected format
+
+        Args:
+            motor_command (character): Defines the mode of operation
+            motor_angles (tuple of 3 floats, (motorA, motorB, motorC)): Angles of the motors to be operated on (rad)
+        """
+        motorA, motorB, motorC = motor_angles
+        motor_output = f"<{motor_command}, {motorA:.2f}, {motorB:.2f}, {motorC:.2f}>".encode('ascii')
+
+        self.motor_serial.write(motor_output)
+
+    def tare_motors(self, zero_angles: tuple[float, float, float]) -> bool:
+        """Send the tare command and waits for confirmation
+
+        Args: 
+            zero_angles (tuple of 3 floats, (motorA, motorB, motorC)): Angles of the motors' current position (rad)
+        Returns:
+            State of motor tare operation: True if Tare successful and false if unsuccessful
+        """
+        # Tare motors given a tuple of angles
+        self.send_encoded_motor_commands(self.motor_serial, 't', (zero_angles))
+        print("Sent 'TARE' Command")
+
+        # Wait for Tare confirmation to be received
+        timeout_start = time.time()
+        received_message = ""
+        while time.time() < timeout_start + tare_timeout:
+            if self.motor_serial.in_waiting > 0:
+                serial_data = self.motor_serial.read(self.motor_serial.in_waiting)
+                try:
+                    # Decode the data as ASCII and strip extraneous characters
+                    received_message += serial_data.decode('ascii').strip()
+                    # Check if "TARE" is received
+                    if "TARE" in received_message:
+                        print("'TARE' Confirmed")
+                        return True
+                except UnicodeDecodeError:
+                    print("Received Non-ASCII Data")
+            time.sleep(0.01)
+
+        print("Timeout Error: 'TARE' Not Confirmed")
+        return False
+
+    def close(self, motor_serial):
+        """Close the serial communications"""
+        motor_serial.close()
+        print("Ports Closed")
+