Completed Tele-op Pipeline

python/hlc/homing.py

@@ -0,0 +1,26 @@
+#!/usr/bin/python3
+import os
+from typing import List
+
+KINEMATICS_INFILE_NAME = "kinematics_input.txt"
+
+def read_last_line(filename):
+    num_newlines = 0
+    with open(filename, "rb") as f:
+        try:
+            f.seek(-2, os.SEEK_END)
+            while num_newlines < 1:
+                f.seek(-2, os.SEEK_CUR)
+                if f.read(1) == b"\n":
+                    num_newlines += 1
+        except OSError:
+            f.seek(0)
+        last_line = f.readline().decode().strip()
+    return last_line
+
+def read_aruco_data() -> List[str]:
+    aruco_data = read_last_line(KINEMATICS_INFILE_NAME)
+    aruco_data_list = aruco_data.split(",")
+    if not aruco_data_list:
+        return None
+    return aruco_data_list # x, y, z, distance

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
@@ -25,7 +21,7 @@ def translate_dir_to_motor_angles(x_dir: float, y_dir: float, mag: float) -> tup
     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/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/tele-op/parse_process.py

@@ -0,0 +1,19 @@
+import re
+
+# Expected ASCII data in the format <x,y>
+format = re.compile(r"<(-?\d+\.\d+),\s*(-?\d+\.\d+)>")
+
+def parse_coord(data):
+    match = format.match(data)
+    if match:
+        x = float(match.group(1))
+        y = float(match.group(2))
+        return x, y
+    else:
+        # print("Data format error")
+        return None
+
+def ascii_encode(motor_angles):
+    a, b, c = motor_angles
+    return f"<a, {a:.2f}, {b:.2f}, {c:.2f}>".encode('ascii')
+

python/tele-op/run_tele_op.py

@@ -0,0 +1,82 @@
+"""Barebones function for running tele-op tomorrow"""
+# Python imports
+from subprocess import Popen
+import time
+import numpy as np
+
+# Module imports
+from hlc.homing import read_aruco_data
+from kinematics.wrappers import translate_N_to_motor_angles
+from serial_utils import *
+from pid.position_feedback import Controller
+
+
+def get_motor_angles_from_aruco() -> tuple[float, float, float] | tuple[None, None, None]:
+    """Helper function to encapsulate the aruco/kinematics pipeline
+    https://github.com/thomasonzhou/MTE380_Software/blob/03967e148c3493c3d2852fd17dae668f0a841446/python/run_homing.py
+
+    Returns:
+        tuple of 3 floats OR tuple of 3 Nones: Motor angles (MotorA, MotorB, MotorC) or failure
+    """
+    detect_aruco = Popen("./aruco_detection") # THIS IS THOMASON'S BINARY
+    time.sleep(2)
+    calibrated = False
+    motor_angles = (0, 0, 0)
+    try:
+        while not calibrated:
+            data = read_aruco_data()
+            print(data)
+            if data is None:
+                continue
+            x, y, z, distance = data
+            motor_angles = translate_N_to_motor_angles(np.array([float(x), float(y), float(z)]), float(distance))
+            return motor_angles
+
+    except Exception as e:
+        print(e)
+        detect_aruco.kill()
+    return (None, None, None)
+
+
+if __name__ == "__main__":
+    # Setup
+    desired_coord = (0,0)
+    pid = Controller()
+    motor_serial, joystick_serial = init_serial()
+
+    # Give time to the user to setup
+    input("Press enter when ready to start hte homing sequence: ")
+
+    # Homing
+    print("Starting homing sequence")
+    motor_angles = get_motor_angles_from_aruco()
+    if None in motor_angles:
+        raise ValueError("CV/Aruco -> Kinematics/Motor Angles pipeline failed")
+
+    print("Taring motors")
+    tare_motors(motor_angles)
+
+    # Waiting for start from user
+    print("Waiting for the joystick start")
+    wait_for_user(joystick_serial)
+
+    # Tele-op until keyboard cancellation
+    print("Starting tele-op")
+    try:
+        while True:
+            actual_coord = joystick_decode(joystick_serial)
+            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)
+            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")
+
+    close_serial(motor_serial, joystick_serial)
+
+

python/tele-op/serial_forward.py

@@ -0,0 +1,103 @@
+import serial
+import time
+import platform
+
+from parse_process import parse_coord, ascii_encode
+from pid.position_feedback import Controller
+from kinematics.wrappers import translate_dir_to_motor_angles
+
+pid = Controller()
+desired_coord = (0,0)
+dir_x, dir_y, theta_mag = 0,0,0
+
+computeOS = platform.system()
+if computeOS == 'Linux':
+    input_com_port = '/dev/ttyACM0'
+    output_com_port = '/dev/ttyUSB0'
+elif computeOS == 'Windows':
+    input_com_port = 'COM8'
+    output_com_port = 'COM5'
+
+baudrate = 115200
+
+try:
+    ser_in = serial.Serial(input_com_port, baudrate, timeout=1)
+    ser_out = serial.Serial(output_com_port, baudrate, timeout=1)
+except serial.SerialException as e:
+    print(f"Error: {e}")
+    exit(1)
+
+while True:
+    if ser_in.in_waiting > 0:
+        input_data = ser_in.read(ser_in.in_waiting)
+        try:
+            # Decode the data as ASCII and strip extraneous characters
+            decoded_data = input_data.decode('ascii').strip()
+
+            # Check if the message is "<t, 0, 0, 0>"
+            if decoded_data == "<t, 0, 0, 0>":
+                print("Received 'TARE' Command")
+                ser_out.write((decoded_data + '\r\n').encode('ascii'))
+                break
+
+        except UnicodeDecodeError:
+            print("non-ASCII data")
+
+    time.sleep(0.01)
+
+received_message = ""
+while True:
+    if ser_out.in_waiting > 0:
+        output_data = ser_out.read(ser_out.in_waiting)
+        try:
+            # Decode the data as ASCII and strip extraneous characters
+            received_message += output_data.decode('ascii').strip()
+
+            # Check if "TARE" is received
+            if "TARE" in received_message:
+                print("Received 'TARE' Confirmation")
+                break
+        except UnicodeDecodeError:
+            print("non-ASCII data")
+
+    time.sleep(0.01)
+
+try:
+    while True:
+        if ser_in.in_waiting > 0:
+            data = ser_in.read(ser_in.in_waiting)
+            try:
+                decoded_data = data.decode('ascii').strip()
+                actual_coord = parse_coord(decoded_data)
+                if actual_coord:
+                    actual_x, actual_y = actual_coord
+                    print(f"RX COORD: {decoded_data}")
+
+                    # Calulate control signal
+                    dir_x, dir_y, theta_mag = pid.calculate(desired_coord, actual_coord)
+                # else:
+                #     dir_x, dir_y, theta_mag = 0,0,0
+
+                    # Convert unit direction vector and control signal magnitude to motor angles
+                    motor_angles = translate_dir_to_motor_angles(dir_x, dir_y, theta_mag)
+
+                    # Re-encode and send the ASCII-decoded data over the output port
+                    motor_output = ascii_encode(motor_angles)
+                    ser_out.write(motor_output)
+
+                    # Debug: Confirm data was sent
+                    print(f"PID VECTOR: {(dir_x, dir_y, theta_mag)}")
+                    print(f"TX ANGLE: {motor_output.decode('ascii')}")
+
+            except UnicodeDecodeError:
+                print("non-ASCII data")
+
+        time.sleep(0.01)
+
+except KeyboardInterrupt:
+    print("Keyboard Interrupt")
+
+finally:
+    ser_in.close()
+    ser_out.close()
+    print("Ports Closed")