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

python/config.json

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

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

python/sliders_cartesian.py

@@ -1,20 +1,33 @@
 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
+def configure_serial_port():
-with open('config.json', 'r') as f:
+    global test_mode, ser
-    config = json.load(f)
-
-# Configure the serial port
     try:
-    ser = serial.Serial(config["COM_PORT"], 9600)  # Replace 'COM9' with your Arduino's serial port
+        ser = serial.Serial(COM_PORT, 9600)
-    time.sleep(2)  # Wait for the serial connection to initialize
+        time.sleep(2)
     except serial.SerialException as e:
         print(f"Error opening serial port: {e}")
         print("Starting Test Mode")
@@ -24,11 +37,9 @@ 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,43 +65,69 @@ 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
+    try:
-    # Replace this with the actual implementation
+        theta1 = math.atan2(y, x)
-    # Example: Inverse kinematics calculations using Jacobian matrix
+        acos_arg = (x**2 + y**2 + z**2 - 53 * z - 420.75) / 900
-    theta1 = np.arctan2(y, x)
+        if acos_arg < -1 or acos_arg > 1:
-    r = np.sqrt(x**2 + y**2)
+            raise ValueError(f"acos argument out of range: {acos_arg}")
-    theta2 = np.arctan2(z, r)
+        theta3 = math.acos(acos_arg)
-    theta3 = np.arctan2(z, r)  # Simplified example
+        theta2 = math.atan2(z + 26.5, math.sqrt(x**2 + y**2)) - math.atan2(15 * math.sin(theta3), 30 + 15 * math.cos(theta3))
-    return [theta1, theta2, 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']
+        step_range = JOINTS[i]['step_range']
-        steps_per_revolution = config['step_per_revolution']
+        try:
-        steps.append(int(angle * steps_per_revolution / (2 * np.pi) * (step_range[1] - step_range[0])))
+            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
+def create_main_window():
     root = tk.Tk()
-root.title("Cartesian Potentiometer Sliders" + (f" ({config['COM_PORT']} Not Found)" if test_mode else f"({config['COM_PORT']})"))
+    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
+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
+def create_serial_frame(root):
+    serial_frame = tk.Frame(root)
+    serial_frame.pack(side=tk.RIGHT, padx=10, pady=10)
+    return serial_frame
+
+def create_sliders(slider_frame):
+    global sliders
     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'], 
+            from_=COORDINATES[coord]['min'], 
-        to=config['coordinates'][coord]['max'], 
+            to=COORDINATES[coord]['max'], 
             orient=tk.HORIZONTAL, 
             variable=slider_values[i], 
             label=f"{coord.upper()} Coordinate", 
@@ -97,59 +135,65 @@ for i, coord in enumerate(coordinates):
         )
         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], 
+            from_=JOINTS[i]['step_range'][0], 
-        to=config['joints'][i]['step_range'][1], 
+            to=JOINTS[i]['step_range'][1], 
             orient=tk.HORIZONTAL, 
             variable=slider_values[i], 
-        label=f"Joint {config['joints'][i]['id']}: {config['joints'][i]['name']}", 
+            label=f"Joint {JOINTS[i]['id']}: {JOINTS[i]['name']}", 
             length=400
         )
         slider.pack()
         sliders.append(slider)
+    return slider_values
+
+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
+    try: 
         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)
-    
+        values = [steps[0], steps[1], slider_values[3].get(), slider_values[4].get(), slider_values[5].get(), steps[2]]
-    # 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)
-
+    except Exception as e:
-# Create a frame for the serial communication
+        print(f"Error updating values: {e}")
-serial_frame = tk.Frame(root)
+        log_serial(f"Error updating values: {e}", "red")
-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
@@ -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
+# Main execution
-loop_button = tk.Button(button_frame, text="Loop: OFF", command=toggle_loop)
+configure_serial_port()
-loop_button.pack(side=tk.LEFT, padx=5)
+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:

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,6 +144,15 @@ void setup() {
 void loop() {
   if (Serial.available() > 0) {
     String data = Serial.readStringUntil('\n');
+    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;
@@ -161,12 +170,17 @@ void loop() {
 
       stepper1.moveTo(jointValues[0]);
       stepper2.moveTo(jointValues[1]);
-    stepper3.moveTo(jointValues[2]);
+      stepper3.moveTo(-jointValues[2]);
-    stepper4.moveTo(jointValues[3]);
+
-    stepper5.moveTo(jointValues[4]);
+      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(jointValues[3]) + " D5: " + String(jointValues[4]) + " D6: " + String(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 (stepper1.distanceToGo() != 0) {