commit 6273223e89ca5534dde5888c3e171b7c7b2d7678 Author: guillaume.bonabau Date: Wed Dec 11 17:53:27 2024 +0100 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/.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" + ] +} diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/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 diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/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 +#include + +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 diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..b1535e6 --- /dev/null +++ b/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 diff --git a/python/config.json b/python/config.json new file mode 100644 index 0000000..d79f6c7 --- /dev/null +++ b/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 + } + } +} \ No newline at end of file diff --git a/python/sliders.py b/python/sliders.py new file mode 100644 index 0000000..9080ae0 --- /dev/null +++ b/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}") \ No newline at end of file diff --git a/python/sliders_cartesian.py b/python/sliders_cartesian.py new file mode 100644 index 0000000..5d721e1 --- /dev/null +++ b/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}") \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..fb22c51 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,169 @@ +#include +#include +#include + +#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(); + +} diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/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