guillaume.bonabau
/
Cyber_301
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update step range for elbow joint , add stop command functionality, fixed cartesian movement

This commit is contained in:
Guillaume BONABAU 2024-12-18 16:51:28 +01:00
parent b29326e77a
commit b263f4b1b7
4 changed files with 206 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
python/config.json
View File

@ -30,7 +30,7 @@
{
"id": 6,
"name": "elbow",
"step_range": [-180, 180]
"step_range": [-450, 450]
}
],
"coordinates": {

12
python/sliders.py
View File

@ -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

266
python/sliders_cartesian.py
View File

@ -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:

60
src/main.cpp
View File

@ -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) {