Cyber_301/python/sliders_cartesian.py

import serial
import time
import tkinter as tk
import json
import threading
import numpy as np

test_mode = False

# Read the joint configuration from the config.json file
with open('config.json', 'r') as f:
    config = json.load(f)

# Configure the serial port
try:
    ser = serial.Serial(config["COM_PORT"], 9600)  # Replace 'COM9' with your Arduino's serial port
    time.sleep(2)  # Wait for the serial connection to initialize
except serial.SerialException as e:
    print(f"Error opening serial port: {e}")
    print("Starting Test Mode")
    test_mode = True

def send_potentiometer_values(values):
    if len(values) != 6:
        raise ValueError("Exactly 6 values are required")
    
    # Convert the values to a comma-separated string
    values_str = ','.join(map(str, values))
    if not test_mode:
        try:
            # Send the values to the Arduino
            ser.write((values_str + '\n').encode())
            log_serial(f"-> {values_str}")
        except serial.SerialException as e:
            print(f"Error writing to serial port: {e}")
    else:
        log_serial(f"-> {values_str} (test)")

def log_serial(message):
    serial_text.config(state=tk.NORMAL)
    serial_text.insert(tk.END, message + '\n')
    serial_text.config(state=tk.DISABLED)
    serial_text.see(tk.END)

def read_serial():
    while True:
        if ser.in_waiting > 0:
            try:
                line = ser.readline().decode('utf-8').strip()
                log_serial(f"<- {line}")
            except serial.SerialException as e:
                print(f"Error reading from serial port: {e}")
        time.sleep(0.1)

def cartesian_to_angles(x, y, z):
    # Placeholder function for converting Cartesian coordinates to joint angles using Jacobian matrix
    # Replace this with the actual implementation
    # Example: Inverse kinematics calculations using Jacobian matrix
    theta1 = np.arctan2(y, x)
    r = np.sqrt(x**2 + y**2)
    theta2 = np.arctan2(z, r)
    theta3 = np.arctan2(z, r)  # Simplified example
    return [theta1, theta2, theta3]

def angles_to_steps(angles):
    # Convert angles to step values based on the configuration
    steps = []
    for i, angle in enumerate(angles):
        step_range = config['joints'][i]['step_range']
        steps_per_revolution = config['step_per_revolution']
        steps.append(int(angle * steps_per_revolution / (2 * np.pi) * (step_range[1] - step_range[0])))
    return steps

# Create the main window
root = tk.Tk()
root.title("Cartesian Potentiometer Sliders" + (f" ({config['COM_PORT']} Not Found)" if test_mode else f"({config['COM_PORT']})"))

# Create a frame for the sliders
slider_frame = tk.Frame(root)
slider_frame.pack(side=tk.LEFT, padx=10, pady=10)

# Create a list to store the slider values
slider_values = [tk.IntVar() for _ in range(6)]

# Create and pack the sliders for x, y, z coordinates
sliders = []
coordinates = ['x', 'y', 'z']
for i, coord in enumerate(coordinates):
    slider = tk.Scale(
        slider_frame, 
        from_=config['coordinates'][coord]['min'], 
        to=config['coordinates'][coord]['max'], 
        orient=tk.HORIZONTAL, 
        variable=slider_values[i], 
        label=f"{coord.upper()} Coordinate", 
        length=400
    )
    slider.pack()
    sliders.append(slider)

# Create and pack the sliders for the gripper joints
for i in range(3, 6):
    slider = tk.Scale(
        slider_frame, 
        from_=config['joints'][i]['step_range'][0], 
        to=config['joints'][i]['step_range'][1], 
        orient=tk.HORIZONTAL, 
        variable=slider_values[i], 
        label=f"Joint {config['joints'][i]['id']}: {config['joints'][i]['name']}", 
        length=400
    )
    slider.pack()
    sliders.append(slider)

def update_values():
    # Get the Cartesian coordinates from the first three sliders
    x = slider_values[0].get()
    y = slider_values[1].get()
    z = slider_values[2].get()
    
    # Convert Cartesian coordinates to joint angles using Jacobian matrix
    angles = cartesian_to_angles(x, y, z)
    
    # Convert joint angles to step values
    steps = angles_to_steps(angles)
    
    # Get the values for the last three joints from the sliders
    gripper_values = [slider_values[i].get() for i in range(3, 6)]
    
    # Combine the steps and gripper values
    values = steps + gripper_values
    
    send_potentiometer_values(values)

# Create a frame for the serial communication
serial_frame = tk.Frame(root)
serial_frame.pack(side=tk.RIGHT, padx=10, pady=10)

# Create a text widget for serial communication
serial_text = tk.Text(serial_frame, state=tk.DISABLED, width=50, height=20)
serial_text.pack()

# Create a frame for the buttons
button_frame = tk.Frame(serial_frame)
button_frame.pack(side=tk.BOTTOM, pady=10)

# Create and pack the send button
send_button = tk.Button(button_frame, text="Send", command=update_values)
send_button.pack(side=tk.LEFT, padx=5)

# Loop functionality
looping = False

def toggle_loop():
    global looping
    looping = not looping
    if looping:
        loop_button.config(text="Loop: ON")
        start_loop()
    else:
        loop_button.config(text="Loop: OFF")

def start_loop():
    if looping:
        update_values()
        root.after(1000, start_loop)  # Adjust the interval as needed

# Create and pack the loop button
loop_button = tk.Button(button_frame, text="Loop: OFF", command=toggle_loop)
loop_button.pack(side=tk.LEFT, padx=5)

# Start a thread to read from the serial port
if not test_mode:
    threading.Thread(target=read_serial, daemon=True).start()

# Run the Tkinter event loop
try:
    root.mainloop()
except Exception as e:
    print(f"Error in Tkinter event loop: {e}")

# Close the serial port when the program is terminated
try:
    ser.close()
except serial.SerialException as e:
    print(f"Error closing serial port: {e}")