first commit

.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

platformio.ini

@@ -0,0 +1,17 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:uno]
+platform = atmelavr
+board = uno
+framework = arduino
+lib_deps = 
+	waspinator/AccelStepper@^1.64
+	adafruit/Adafruit Motor Shield V2 Library@^1.1.3

python/config.json

@@ -0,0 +1,50 @@
+{
+    "COM_PORT": "COM9",
+    "step_per_revolution": 48,
+    "joints": [
+        {
+            "id": 1,
+            "name": "base",
+            "step_range": [0, 2838]
+        },
+        {
+            "id": 2,
+            "name": "shoulder",
+            "step_range": [0, 2838]
+        },
+        {
+            "id": 3,
+            "name": "gripper_1",
+            "step_range": [0, 144]
+        },
+        {
+            "id": 4,
+            "name": "gripper_2",
+            "step_range": [-180, 180]
+        },
+        {
+            "id": 5,
+            "name": "gripper_3",
+            "step_range": [-180, 180]
+        },
+        {
+            "id": 6,
+            "name": "elbow",
+            "step_range": [-180, 180]
+        }
+    ],
+    "coordinates": {
+        "x": {
+            "min": 0,
+            "max": 100
+        },
+        "y": {
+            "min": 0,
+            "max": 100
+        },
+        "z": {
+            "min": 0,
+            "max": 100
+        }
+    }
+}

python/sliders.py

@@ -0,0 +1,147 @@
+import serial
+import time
+import tkinter as tk
+import json
+import threading
+
+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)
+
+# Create the main window
+root = tk.Tk()
+root.title("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 using the configuration from config.json
+sliders = []
+for joint in config['joints']:
+    slider = tk.Scale(
+        slider_frame, 
+        from_=joint['step_range'][0], 
+        to=joint['step_range'][1], 
+        orient=tk.HORIZONTAL, 
+        variable=slider_values[joint['id'] - 1], 
+        label=f"Joint {joint['id']}: {joint['name']}", 
+        length=400
+    )
+    slider.pack()
+    sliders.append(slider)
+
+def update_values():
+    values = [var.get() for var in slider_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()
+
+def home_position():
+    # Set all sliders to 0
+    for slider in sliders:
+        slider.set(0)
+    # Send the home position values
+    update_values()
+
+# 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)
+
+# Create and pack the home button
+home_button = tk.Button(button_frame, text="Home", command=home_position)
+home_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}")

python/sliders_cartesian.py

@@ -0,0 +1,186 @@
+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}")

src/main.cpp

@@ -0,0 +1,169 @@
+#include <SPI.h>
+#include <AccelStepper.h>
+#include <Adafruit_MotorShield.h>
+
+#define STEPSBYREVOLUTION 48
+
+// Create the motor shield object with 0x60 address for the top shield
+Adafruit_MotorShield AFMS_1 = Adafruit_MotorShield(0x60);
+// Create the motor shield object with 0x61 address for the bottom shield
+Adafruit_MotorShield AFMS_2 = Adafruit_MotorShield(0x61);
+// Create the motor shield object with 0x61 address for the bottom shield
+Adafruit_MotorShield AFMS_3 = Adafruit_MotorShield(0x63);
+
+
+// to shield 1, motor port #1 (M1 and M2)
+Adafruit_StepperMotor *myMotor1 = AFMS_1.getStepper(48, 1);
+
+// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
+// wrappers for the 1st motor!
+void forwardstep1() {
+  myMotor1->onestep(FORWARD, SINGLE);
+}
+void backwardstep1() {
+  myMotor1->onestep(BACKWARD, SINGLE);
+}
+
+
+// to shield 1, motor port #2 (M3 and M4)
+Adafruit_StepperMotor *myMotor2 = AFMS_1.getStepper(48, 2);
+
+// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
+// wrappers for the 2nd motor!
+void forwardstep2() {
+  myMotor2->onestep(FORWARD, SINGLE);
+}
+void backwardstep2() {
+  myMotor2->onestep(BACKWARD, SINGLE);
+}
+
+
+// to shield 2, motor port #1 (M1 and M2)
+Adafruit_StepperMotor *myMotor3 = AFMS_2.getStepper(48, 1);
+
+// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
+// wrappers for the 3rd motor!
+void forwardstep3() {
+  myMotor3->onestep(FORWARD, SINGLE);
+}
+void backwardstep3() {
+  myMotor3->onestep(BACKWARD, SINGLE);
+}
+
+
+// to shield 2, motor port #2 (M3 and M4)
+Adafruit_StepperMotor *myMotor4 = AFMS_2.getStepper(48, 2);
+
+// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
+// wrappers for the 4th motor!
+void forwardstep4() {
+  myMotor4->onestep(FORWARD, SINGLE);
+}
+void backwardstep4() {
+  myMotor4->onestep(BACKWARD, SINGLE);
+}
+
+
+// to shield 3, motor port #1 (M1 and M2)
+Adafruit_StepperMotor *myMotor5 = AFMS_3.getStepper(48, 1);
+
+// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
+// wrappers for the 5th motor!
+void forwardstep5() {
+  myMotor5->onestep(FORWARD, SINGLE);
+}
+void backwardstep5() {
+  myMotor5->onestep(BACKWARD, SINGLE);
+}
+
+
+// to shield 3, motor port #2 (M3 and M4)
+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, SINGLE);
+}
+void backwardstep6() {
+  myMotor6->onestep(BACKWARD, SINGLE);
+}
+
+// Now we'll wrap the 6 steppers in an AccelStepper object
+AccelStepper stepper1(forwardstep1, backwardstep1);
+AccelStepper stepper2(forwardstep2, backwardstep2);
+AccelStepper stepper3(forwardstep3, backwardstep3);
+AccelStepper stepper4(forwardstep4, backwardstep4);
+AccelStepper stepper5(forwardstep5, backwardstep5);
+AccelStepper stepper6(forwardstep6, backwardstep6);
+
+unsigned long lastUpdate = 0; // Last time the position was updated
+const int updateInterval = 0; // Update interval in milliseconds
+int newTarget = 0;
+int motorSpeed = 10;
+
+int potValue1 = 0;
+int potValue2 = 0;
+int potValue3 = 0;
+int potValue4 = 0;
+int potValue5 = 0;
+int potValue6 = 0;
+int targetPosition1 = 0;
+int targetPosition2 = 0;
+int targetPosition3 = 0;
+int targetPosition4 = 0;
+int targetPosition5 = 0;
+int targetPosition6 = 0;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+
+  AFMS_1.begin(); // Start the bottom shield
+  AFMS_2.begin(); // Start the bottom shield
+  AFMS_3.begin(); // Start the top shield
+
+  stepper1.setSpeed(motorSpeed);
+  stepper2.setSpeed(motorSpeed);
+  stepper3.setSpeed(motorSpeed);
+  stepper4.setSpeed(motorSpeed);
+  stepper5.setSpeed(motorSpeed);
+  stepper6.setSpeed(motorSpeed);
+
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  if (Serial.available() > 0) {
+    String data = Serial.readStringUntil('\n');
+    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++;
+    }
+    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]);
+  }
+
+  stepper1.runSpeedToPosition();
+  stepper2.runSpeedToPosition();
+  stepper3.runSpeedToPosition();
+  stepper4.runSpeedToPosition();
+  stepper5.runSpeedToPosition();
+  stepper6.runSpeedToPosition();
+ 
+}

test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html