From b263f4b1b7d49921b3fda3db08db017bffdf4e47 Mon Sep 17 00:00:00 2001 From: "guillaume.bonabau" Date: Wed, 18 Dec 2024 16:51:28 +0100 Subject: [PATCH] Update step range for elbow joint , add stop command functionality, fixed cartesian movement --- python/config.json | 2 +- python/sliders.py | 12 ++ python/sliders_cartesian.py | 266 +++++++++++++++++++++--------------- src/main.cpp | 60 ++++---- 4 files changed, 206 insertions(+), 134 deletions(-) diff --git a/python/config.json b/python/config.json index 75197d2..3713fb6 100644 --- a/python/config.json +++ b/python/config.json @@ -30,7 +30,7 @@ { "id": 6, "name": "elbow", - "step_range": [-180, 180] + "step_range": [-450, 450] } ], "coordinates": { diff --git a/python/sliders.py b/python/sliders.py index e634f3b..73363ba 100644 --- a/python/sliders.py +++ b/python/sliders.py @@ -98,6 +98,16 @@ def home_position(): # Send the home position values update_values() +def send_stop_command(): + try: + if not test_mode: + ser.write("STOP\n".encode()) + log_serial("-> STOP") + else: + log_serial("-> STOP (test)") + except serial.SerialException as e: + print(f"Error writing to serial port: {e}") + # Create a frame for the buttons button_frame = tk.Frame(serial_frame) button_frame.pack(side=tk.BOTTOM, pady=10) @@ -110,6 +120,8 @@ send_button.pack(side=tk.LEFT, padx=5) home_button = tk.Button(button_frame, text="Home", command=home_position) home_button.pack(side=tk.LEFT, padx=5) +stop_button = tk.Button(button_frame, text="Stop", command=send_stop_command) +stop_button.pack(side=tk.LEFT, padx=5) # Loop functionality looping = False diff --git a/python/sliders_cartesian.py b/python/sliders_cartesian.py index 5d721e1..906b33b 100644 --- a/python/sliders_cartesian.py +++ b/python/sliders_cartesian.py @@ -1,34 +1,45 @@ import serial import time import tkinter as tk -import json import threading -import numpy as np +import math + +# Constants for easy maintenance +COM_PORT = 'COM9' # Replace with your Arduino's serial port +STEP_PER_REVOLUTION = 48 # Example value, adjust as needed +JOINTS = [ + {'id': 1, 'name': 'Base', 'step_range': [0, 2838], 'step_per_degree': 8}, + {'id': 2, 'name': 'Shoulder', 'step_range': [-1000, 1000], 'step_per_degree': 8}, + {'id': 6, 'name': 'Elbow', 'step_range': [-450, 450], 'step_per_degree': 5}, + {'id': 3, 'name': 'Gripper 1', 'step_range': [0, 250]}, + {'id': 4, 'name': 'Gripper 2', 'step_range': [-180, 180]}, + {'id': 5, 'name': 'Gripper 3', 'step_range': [-180, 180]}, +] +COORDINATES = { + 'x': {'min': -50, 'max': 50}, + 'y': {'min': -50, 'max': 50}, + 'z': {'min': 0, 'max': 200}, +} 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 configure_serial_port(): + global test_mode, ser + try: + ser = serial.Serial(COM_PORT, 9600) + time.sleep(2) + 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: @@ -36,9 +47,10 @@ def send_potentiometer_values(values): else: log_serial(f"-> {values_str} (test)") -def log_serial(message): +def log_serial(message, color="black"): serial_text.config(state=tk.NORMAL) - serial_text.insert(tk.END, message + '\n') + serial_text.insert(tk.END, message + '\n', ("color",)) + serial_text.tag_config("color", foreground=color) serial_text.config(state=tk.DISABLED) serial_text.see(tk.END) @@ -53,103 +65,135 @@ def read_serial(): 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] + try: + theta1 = math.atan2(y, x) + acos_arg = (x**2 + y**2 + z**2 - 53 * z - 420.75) / 900 + if acos_arg < -1 or acos_arg > 1: + raise ValueError(f"acos argument out of range: {acos_arg}") + theta3 = math.acos(acos_arg) + theta2 = math.atan2(z + 26.5, math.sqrt(x**2 + y**2)) - math.atan2(15 * math.sin(theta3), 30 + 15 * math.cos(theta3)) + theta2 = theta2 - math.pi / 2 + theta1 = theta1%(2 * math.pi) + theta2 = theta2%(2 * math.pi) + theta3 = theta3%(2 * math.pi) + + if theta2 > math.pi: + theta2 -= 2 * math.pi + alpha1 = theta1* 180 / math.pi + alpha2 = theta2 * 180 / math.pi + alpha3 = theta3 * 180 / math.pi + log_serial(f"Angles: {alpha1:.2f}, {alpha2:.2f}, {alpha3:.2f}", "blue") + return [alpha1, alpha2, alpha3] + except ValueError as e: + print(f"Error in cartesian_to_angles: {e}") + raise e + 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]))) + step_range = JOINTS[i]['step_range'] + try: + step_value = int(angle * JOINTS[i]["step_per_degree"]) + if step_value < step_range[0] or step_value > step_range[1]: + raise ValueError(f"Joint {JOINTS[i]['id']} out of range: {step_value} (Range: {step_range[0]} to {step_range[1]})") + steps.append(step_value) + except ValueError as e: + print(f"Error in angles_to_steps: {e}") + raise e 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']})")) +def create_main_window(): + root = tk.Tk() + root.title("Cartesian Potentiometer Sliders" + (f" ({COM_PORT} Not Found)" if test_mode else f"({COM_PORT})")) + return root -# Create a frame for the sliders -slider_frame = tk.Frame(root) -slider_frame.pack(side=tk.LEFT, padx=10, pady=10) +def create_slider_frame(root): + slider_frame = tk.Frame(root) + slider_frame.pack(side=tk.LEFT, padx=10, pady=10) + return slider_frame -# Create a list to store the slider values -slider_values = [tk.IntVar() for _ in range(6)] +def create_serial_frame(root): + serial_frame = tk.Frame(root) + serial_frame.pack(side=tk.RIGHT, padx=10, pady=10) + return serial_frame -# 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) +def create_sliders(slider_frame): + global sliders + slider_values = [tk.IntVar() for _ in range(6)] + sliders = [] + coordinates = ['x', 'y', 'z'] + for i, coord in enumerate(coordinates): + slider = tk.Scale( + slider_frame, + from_=COORDINATES[coord]['min'], + to=COORDINATES[coord]['max'], + orient=tk.HORIZONTAL, + variable=slider_values[i], + label=f"{coord.upper()} Coordinate", + length=400 + ) + slider.pack() + sliders.append(slider) + for i in range(3, 6): + slider = tk.Scale( + slider_frame, + from_=JOINTS[i]['step_range'][0], + to=JOINTS[i]['step_range'][1], + orient=tk.HORIZONTAL, + variable=slider_values[i], + label=f"Joint {JOINTS[i]['id']}: {JOINTS[i]['name']}", + length=400 + ) + slider.pack() + sliders.append(slider) + return slider_values -# 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 home_position(): + for slider in sliders: + slider.set(0) + send_potentiometer_values([0, 0, 0, 0, 0, 0]) + +def send_stop_command(): + try: + if not test_mode: + ser.write("STOP\n".encode()) + log_serial("-> STOP") + else: + log_serial("-> STOP (test)") + except serial.SerialException as e: + print(f"Error writing to serial port: {e}") + +def create_serial_text(serial_frame): + serial_text = tk.Text(serial_frame, state=tk.DISABLED, width=50, height=20) + serial_text.pack() + return serial_text + +def create_buttons(serial_frame, update_values, toggle_loop): + button_frame = tk.Frame(serial_frame) + button_frame.pack(side=tk.BOTTOM, pady=10) + send_button = tk.Button(button_frame, text="Send", command=update_values) + send_button.pack(side=tk.LEFT, padx=5) + loop_button = tk.Button(button_frame, text="Loop: OFF", command=toggle_loop) + loop_button.pack(side=tk.LEFT, padx=5) + home_button = tk.Button(button_frame, text="Home", command=home_position) + home_button.pack(side=tk.LEFT, padx=5) + stop_button = tk.Button(button_frame, text="Stop", command=send_stop_command) + stop_button.pack(side=tk.LEFT, padx=5) + return loop_button 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 + try: + x = slider_values[0].get() + y = slider_values[1].get() + z = slider_values[2].get() + angles = cartesian_to_angles(x, y, z) + steps = angles_to_steps(angles) + values = [steps[0], steps[1], slider_values[3].get(), slider_values[4].get(), slider_values[5].get(), steps[2]] + send_potentiometer_values(values) + except Exception as e: + print(f"Error updating values: {e}") + log_serial(f"Error updating values: {e}", "red") def toggle_loop(): global looping @@ -163,23 +207,25 @@ def toggle_loop(): def start_loop(): if looping: update_values() - root.after(1000, start_loop) # Adjust the interval as needed + root.after(1000, start_loop) -# 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) +# Main execution +configure_serial_port() +root = create_main_window() +slider_frame = create_slider_frame(root) +serial_frame = create_serial_frame(root) +slider_values = create_sliders(slider_frame) +serial_text = create_serial_text(serial_frame) +loop_button = create_buttons(serial_frame, update_values, toggle_loop) -# 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: diff --git a/src/main.cpp b/src/main.cpp index 90c8def..8db26b9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -79,10 +79,10 @@ Adafruit_StepperMotor *myMotor6 = AFMS_3.getStepper(48, 2); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 6th motor! void forwardstep6() { - myMotor6->onestep(FORWARD, MOTORTYPE); + myMotor6->onestep(FORWARD, DOUBLE); } void backwardstep6() { - myMotor6->onestep(BACKWARD, MOTORTYPE); + myMotor6->onestep(BACKWARD, DOUBLE); } // Now we'll wrap the 6 steppers in an AccelStepper object @@ -144,29 +144,43 @@ void setup() { void loop() { if (Serial.available() > 0) { String data = Serial.readStringUntil('\n'); - int jointValues[6]; - int index = 0; - int start = 0; - int end = data.indexOf(','); + if (data == "STOP") { + stepper1.moveTo(stepper1.currentPosition()); + stepper2.moveTo(stepper2.currentPosition()); + stepper3.moveTo(stepper3.currentPosition()); + stepper4.moveTo(stepper4.currentPosition()); + stepper5.moveTo(stepper5.currentPosition()); + stepper6.moveTo(stepper6.currentPosition()); + Serial.println("Robot stopped."); + } else { + int jointValues[6]; + int index = 0; + int start = 0; + int end = data.indexOf(','); - while (end != -1 && index < 6) { - jointValues[index] = data.substring(start, end).toInt(); - start = end + 1; - end = data.indexOf(',', start); - index++; + while (end != -1 && index < 6) { + jointValues[index] = data.substring(start, end).toInt(); + start = end + 1; + end = data.indexOf(',', start); + index++; + } + if (index < 6) { + jointValues[index] = data.substring(start).toInt(); + } + + stepper1.moveTo(jointValues[0]); + stepper2.moveTo(jointValues[1]); + stepper3.moveTo(-jointValues[2]); + + int step4 = jointValues[3] + jointValues[4]; + int step5 = -jointValues[3] + jointValues[4]; + stepper4.moveTo(step4); + stepper5.moveTo(step5); + + stepper6.moveTo(jointValues[5]); + + Serial.println("Moving to position: D1: " + String(jointValues[0]) + " D2: " + String(jointValues[1]) + " D3: " + String(jointValues[2]) + " D4: " + String(step4) + " D5: " + String(step5) + " D6: " + String(jointValues[5])); } - if (index < 6) { - jointValues[index] = data.substring(start).toInt(); - } - - stepper1.moveTo(jointValues[0]); - stepper2.moveTo(jointValues[1]); - stepper3.moveTo(jointValues[2]); - stepper4.moveTo(jointValues[3]); - stepper5.moveTo(jointValues[4]); - stepper6.moveTo(jointValues[5]); - - Serial.println("Moving to position: D1: " + String(jointValues[0]) + " D2: " + String(jointValues[1]) + " D3: " + String(jointValues[2]) + " D4: " + String(jointValues[3]) + " D5: " + String(jointValues[4]) + " D6: " + String(jointValues[5])); } if (stepper1.distanceToGo() != 0) {