Robot Arms for Accessibility

#include <WiFi.h>
#include <WebServer.h>
#include <ESP32Servo.h>
#include <ArduinoJson.h>
#include <Wire.h>

const char* ssid = "< Your wifi >";
const char* password = "< Your wifi password >";
WebServer server(80);

Servo servos[4];

//For anyone wondering about the crazy pin numbering, that's apparently how ESP32's roll - they're all over the place
int servoPins[4] = { 5, 27, 21, 18 };  // Base, Shoulder, Elbow, Grip

// Button pins
const int buttonPin1 = 4;
const int buttonPin2 = 14;
const int buttonPin3 = 13;
const int buttonPin4 = 12;

const int joyButtonPin = 23;

// Linear actuator control pins
const int actuatorPin1 = 25;  // Linear actuator extend
const int actuatorPin2 = 26;  // Linear actuator retract
const int joyXPin = 33;
const int joyYPin = 32;

int joyLowThreshold = 2250;
int joyHighThreshold = 2900;

int currentPositions[4] = { 90, 100, 110, 50 };
int desiredPositions[4] = { 90, 100, 110, 110 };

bool gripperIsClosed = true;

int moveAmount = 5;  // change in degrees for each move command
int currentPreplannedStep = 0;

void setup() {
  Serial.begin(9600);
  for (int i = 0; i < 4; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(currentPositions[i]);
  }

  //Don't indefinitely loop - we can use it with buttons and the joystick offline
  if (connectToWiFi()) {
    Serial.print("Connected to Wi-Fi with IP: ");
    Serial.println(WiFi.localIP());
    server.on("/setPosition", HTTP_GET, setPosition);
    server.on("/preplanned", HTTP_GET, preplannedMovement);
    server.on("/move", HTTP_GET, callMove);
    server.on("/controlGrip", HTTP_GET, controlGrip);
    server.on("/getCurrentPositions", HTTP_GET, getCurrentPositions);  // Add the new route

    server.onNotFound(handleNotFound);
    server.begin();
  }

  // Setup button pins
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);
  pinMode(buttonPin4, INPUT_PULLUP);

  // Setup joystick pins
  pinMode(joyXPin, INPUT);              // Set the joystick X axis as input
  pinMode(joyYPin, INPUT);              // Set the joystick Y axis as input
  pinMode(joyButtonPin, INPUT_PULLUP);  // Set the joystick button as input with pull-up resistor


  pinMode(actuatorPin1, OUTPUT);
  pinMode(actuatorPin2, OUTPUT);

  gripperToggle();  //Looks cool and makes it clear the arm is ready for use
}

void moveActuator(bool moveOut) {
  digitalWrite(actuatorPin1, moveOut ? HIGH : LOW);
  digitalWrite(actuatorPin2, moveOut ? LOW : HIGH);
  delay(2000);                      // Active movement duration
  digitalWrite(actuatorPin1, LOW);  // Stop the actuator
  digitalWrite(actuatorPin2, LOW);
}

void loop() {
  server.handleClient();

  processControls();
}

void gripperToggle() {
  moveServoToPosition(servos[3], currentPositions[3], gripperIsClosed ? 50 : 110);
  gripperIsClosed = !gripperIsClosed;
}

void processControls() {

  //All the buttons!  If you want them to do something else, just change what function they call here -
  if (digitalRead(buttonPin1) == LOW) {
    gripperToggle();
    Serial.println("Button1 pressed - toggle grip");
  }

  if (digitalRead(buttonPin2) == LOW) {
    Serial.println("Button2 pressed - preplannedMovement");
    preplannedMovement();
  }

  if (digitalRead(buttonPin3) == LOW) {
    Serial.println("Button3 pressed - actuator out");
    moveActuator(true);
  }

  if (digitalRead(buttonPin4) == LOW) {
    moveActuator(false);
    Serial.println("Button4 pressed - actuator in");
  }

  // Read joystick position
  int joyX = analogRead(joyXPin);  // Read the joystick X axis
  int joyY = analogRead(joyYPin);  // Read the joystick Y axis

  if (joyX < joyLowThreshold) {
    handleMove("left");
    Serial.println("Joystick activated - Move LEFT");
    Serial.print("Joystick X Read: ");
    Serial.println(joyX);
  } else if (joyX > joyHighThreshold) {
    handleMove("right");
    Serial.println("Joystick activated - Move RIGHT");
    Serial.print("Joystick X Read: ");
    Serial.println(joyX);
  }

  if (joyY < joyLowThreshold) {
    handleMove("up");
    Serial.println("Joystick activated - Move UP");
    Serial.print("Joystick Y Read: ");
    Serial.println(joyY);
  } else if (joyY > joyHighThreshold) {
    handleMove("down");
    Serial.println("Joystick activated - Move DOWN");
    Serial.print("Joystick Y Read: ");
    Serial.println(joyY);
  }

  if (digitalRead(joyButtonPin) == LOW) {
    gripperToggle();
    Serial.println("Joystick pressed - toggle grip");
  }

  delay(200);  // Delay to avoid flooding the serial output
}

bool connectToWiFi() {
  WiFi.begin(ssid, password);
  unsigned long startTime = millis();
  while (WiFi.status() != WL_CONNECTED) {
    if (millis() - startTime > 15000) {  // 15 seconds timeout
      Serial.println("Failed to connect to WiFi.");
      return false;
    }
    delay(100);
  }
  return true;
}

void getCurrentPositions() {
  StaticJsonDocument<200> doc;  // Create a JSON document to store position data
  doc["base"] = currentPositions[0];
  doc["shoulder"] = currentPositions[1];
  doc["elbow"] = currentPositions[2];
  doc["grip"] = currentPositions[3];

  String response;
  serializeJson(doc, response);  // Serialize the JSON document to a string

  server.send(200, "application/json", response);  // Send the JSON response
}

void controlGrip() {
  if (server.hasArg("action")) {
    String action = server.arg("action");
    if (action == "open") {
      moveServoToPosition(servos[3], currentPositions[3], 40);
      gripperIsClosed = false;
    } else if (action == "close") {
      moveServoToPosition(servos[3], currentPositions[3], 110);
      gripperIsClosed = true;
    }
    server.send(200, "text/plain", "Grip " + action);
  } else {
    server.send(400, "text/plain", "Action parameter is missing.");
  }
}

void moveServoToPosition(Servo& servo, int& currentPosition, int desiredPosition) {
  Serial.println("moveServoToPosition");
  while (currentPosition != desiredPosition) {
    Serial.println(currentPosition);
    currentPosition += (currentPosition < desiredPosition) ? 1 : -1;
    servo.write(currentPosition);
    delay(15);
  }
}

void setPosition() {
  if (server.hasArg("index") && server.hasArg("position")) {
    int index = server.arg("index").toInt();
    int position = server.arg("position").toInt();
    if (index >= 0 && index < 4) {  // Check if the index is within the range of your servos array
      moveServoToPosition(servos[index], currentPositions[index], position);
      server.send(200, "text/plain", "Servo " + String(index) + " set to position " + String(position));
    } else {
      server.send(400, "text/plain", "Invalid servo index.");
    }
  } else {
    server.send(400, "text/plain", "Missing servo index or position.");
  }
}

void handleNotFound() {
  server.send(404, "text/plain", "Command not found.");
}

void handleMove(String direction) {

  if (direction == "left") {
    desiredPositions[0] = max(0, min(180, desiredPositions[0] + moveAmount));
    Serial.println("Moving LEFT");
  } else if (direction == "right") {
    desiredPositions[0] = max(0, min(180, desiredPositions[0] - moveAmount));
    Serial.println("Moving RIGHT");
  } else if (direction == "up") {
    desiredPositions[1] = max(0, min(180, desiredPositions[1] - moveAmount));
    desiredPositions[1] = max(0, min(180, desiredPositions[1] - moveAmount));
  } else if (direction == "down") {
    desiredPositions[1] = max(0, min(180, desiredPositions[1] + moveAmount));
    desiredPositions[1] = max(0, min(180, desiredPositions[1] + moveAmount));
  }

  moveServoToPosition(servos[0], currentPositions[0], desiredPositions[0]);
  moveServoToPosition(servos[1], currentPositions[1], desiredPositions[1]);
  moveServoToPosition(servos[2], currentPositions[2], desiredPositions[2]);
}

void callMove() {
  if (server.hasArg("direction")) {
    handleMove(server.arg("direction"));
    server.send(200, "text/plain", "Move command executed.");
  } else {
    server.send(400, "text/plain", "Direction parameter is missing.");
  }
}

struct MovementStep {
  int basePos;
  int shoulderPos;
  int elbowPos;
  int gripPos;
};

void preplannedMovement() {
  if (currentPreplannedStep == 0) {
    Serial.println("Executing Step 1: Open grip, move shoulder and elbow forward.");
    // Open the grip and move the shoulder and elbow forward
    moveServoToPosition(servos[3], currentPositions[3], 0);    // Open grip
    moveServoToPosition(servos[1], currentPositions[1], 120);  // Shoulder forward
    moveServoToPosition(servos[2], currentPositions[2], 90);   // Elbow forward
    gripperIsClosed = false;
    currentPreplannedStep = 1;  // Set next step as step 2
  } else {
    Serial.println("Executing Step 2: Close grip, move base, adjust shoulder/elbow, and open grip.");
    // Close the grip and move the base to the left 90 degrees
    moveServoToPosition(servos[3], currentPositions[3], 110);  // Close grip
    gripperIsClosed = true;
    moveServoToPosition(servos[0], currentPositions[0], 150);  // Base to the left position

    // Adjust the shoulder and elbow, then open the grip
    moveServoToPosition(servos[1], currentPositions[1], 140);  // Slight shoulder down
    moveServoToPosition(servos[2], currentPositions[2], 80);   // Slight elbow down
    moveServoToPosition(servos[3], currentPositions[3], 0);    // Open grip again
    gripperIsClosed = false;

    // Immediately return to rest position after step 2
    Serial.println("Returning to rest position.");

    // Go down the list the other way so we avoid bumping into obstacles (easy to loop through the other way if you are getting uppercut or some such)
    for (int j = 3; j >= 0; j--) {
      moveServoToPosition(servos[j], currentPositions[j], 90);  // Reset to rest position
    }
    currentPreplannedStep = 0;  // Set next step as step 1
  }
  server.send(200, "text/plain", "Preplanned movement completed.");
}

import requests
import tkinter as tk
from tkinter import ttk
import time
import json
import os

# Base URL of the ESP32 server
BASE_URL = "http://192.168.86.30"

# Global variables
recordings = {}
current_recording = []  # List to store current recording steps
recording = False
selected_recording_name = None
recordings_file = 'recordings.json'  # File to store recordings

# Function to load recordings from a file
def load_recordings():
    global recordings
    if os.path.isfile(recordings_file):
        with open(recordings_file, 'r') as file:
            recordings = json.load(file)

# Function to save recordings to a file
def save_recordings():
    with open(recordings_file, 'w') as file:
        json.dump(recordings, file, indent=4)

# Function to delete a recording
def delete_selected_recording():
    global selected_recording_name
    if selected_recording_name and selected_recording_name in recordings:
        del recordings[selected_recording_name]
        selected_recording_name = None
        save_recordings()  # Save the updated recordings to the file
        update_recording_listbox()
        print(f"Deleted recording '{selected_recording_name}'.")
        
# Function to fetch and store the initial positions of all servos at the start of recording
def fetch_and_store_initial_positions():
    positions = get_initial_positions()
    if positions:
        current_recording.append(('setPosition', positions))
        print("Initial positions stored:", positions)
    else:
        print("Failed to get initial positions.")

# Function to get the initial servo positions from the Arduino
def get_initial_positions():
    url = f"{BASE_URL}/getCurrentPositions"
    try:
        response = requests.get(url)
        if response.status_code == 200:
            return response.json()  # Parse JSON response into a dictionary
        else:
            print("Failed to get initial positions. Status code:", response.status_code)
            return None
    except requests.exceptions.RequestException as e:
        print("Request failed:", e)
        return None

# Function to start/stop recording
def toggle_recording():
    global recording, current_recording
    recording = not recording
    if recording:  # Start recording
        current_recording = []  # Clear previous recording
        fetch_and_store_initial_positions()  # Fetch and store initial positions
        record_button.config(text="Stop Recording")
        print("Recording started.")
    else:  # Stop recording
        if current_recording:  # If actions were recorded, save them
            recording_name = "Recording " + str(len(recordings) + 1)
            recordings[recording_name] = current_recording
            update_recording_listbox()
            print(f"Recording '{recording_name}' saved with {len(current_recording)} actions.")
            save_recordings()
        else:
            print("No actions recorded.")
        record_button.config(text="Start Recording")
        current_recording = []
        
# Function to play the selected recording
def play_recording():
    global selected_recording_name
    if selected_recording_name and selected_recording_name in recordings:
        actions = recordings[selected_recording_name]
        if actions:
            # Set initial position
            action_type, positions = actions[0]
            if action_type == 'setPosition':
                print(f"Setting initial positions for recording {selected_recording_name}: {positions}")
                set_positions(**positions)  # Set positions
                time.sleep(2)  # Wait for servos to reach the starting positions

            # Play actions
            for action_type, action in actions[1:]:  # Skip the initial setPosition
                if action_type == 'move':
                    print(f"Moving servo {action}")
                    move_servo(action)
                elif action_type == 'grip':
                    print(f"Gripping action {action}")
                    control_grip(action)
                time.sleep(0.5)  # Delay for the servo to move
        else:
            print(f"Recording {selected_recording_name} is empty.")
    else:
        print("No recording selected or found.")

# Function to move the servo in a direction and record the action
def move_servo(direction):
    """Move the servo in the given direction and record the movement if recording is active."""
    url = f"{BASE_URL}/move"
    params = {"direction": direction}
    response = requests.get(url, params=params)
    if recording:  # Only record if we are in recording mode
        current_recording.append(('move', direction))  # Append the move action to the current recording
    print(response.text)

# Function to control the grip and record the action
def control_grip(action):
    """Send command to the ESP32 to control the grip and record the action."""
    url = f"{BASE_URL}/controlGrip"
    params = {"action": action}
    response = requests.get(url, params=params)
    if recording:  # Only record if we are in recording mode
        current_recording.append(('grip', action))  # Append the grip action to the current recording
    print(response.text)
    
# Function to set positions of servos
def set_positions(base=None, shoulder=None, elbow=None, grip=None):
    for i, position in enumerate([base, shoulder, elbow, grip]):
        if position is not None:
            url = f"{BASE_URL}/setPosition"
            params = {"index": i, "position": position}
            response = requests.get(url, params=params)
            print(response.text)
            time.sleep(0.1)  # Short delay between HTTP requests to avoid overloading the Arduino


def create_grip_control_frame(parent):
    frame = ttk.LabelFrame(parent, text="Grip Control", padding=(10, 10))

    # Create 'Open' button
    open_button = ttk.Button(frame, text="Open Grip", command=lambda: control_grip("open"))
    open_button.pack(side=tk.LEFT, padx=5, pady=5)

    # Create 'Close' button
    close_button = ttk.Button(frame, text="Close Grip", command=lambda: control_grip("close"))
    close_button.pack(side=tk.RIGHT, padx=5, pady=5)

    return frame

def create_servo_control_frame(parent):
    frame = ttk.LabelFrame(parent, text="Servo Control", padding=(10, 10))
    buttons = {'left': (0, 0), 'right': (0, 1), 'up': (0, 2), 'down': (0, 3)}
    for direction, pos in buttons.items():
        button = ttk.Button(frame, text=direction.capitalize(), command=lambda d=direction: move_servo(d))
        button.grid(row=pos[0], column=pos[1], padx=5, pady=5, sticky=(tk.W, tk.E))
    return frame

def create_record_playback_frame(parent):
    frame = ttk.Frame(parent, padding=(10, 10))
    global record_button
    record_button = ttk.Button(frame, text="Start Recording", command=toggle_recording)
    record_button.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=(0, 5))

    play_button = ttk.Button(frame, text="Play Recording", command=play_recording)
    play_button.pack(side=tk.RIGHT, fill=tk.X, expand=True, padx=(5, 0))
    delete_button = ttk.Button(frame, text="Delete Recording", command=delete_selected_recording)
    delete_button.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=(5, 0))
    return frame

# Function to select a recording from the list
def select_recording(event):
    global selected_recording_name
    selection = event.widget.curselection()
    if selection:
        index = selection[0]
        selected_recording_name = event.widget.get(index)
      
# Function to update the Listbox with the recordings' names
def update_recording_listbox():
    recording_listbox.delete(0, tk.END)  # Clear the current list
    for name in recordings.keys():
        recording_listbox.insert(tk.END, name)
        

# GUI window setup
def gui():
    global window, recording_listbox, record_button
    load_recordings()  # Load recordings at the start of the program
    window = tk.Tk()
    window.title("Servo Control Panel")

    control_frame = create_servo_control_frame(window)
    control_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=(10, 0))

    grip_control_frame = create_grip_control_frame(window)
    grip_control_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))

    record_playback_frame = create_record_playback_frame(window)
    record_playback_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))

    # Listbox for recordings
    recording_listbox = tk.Listbox(window)
    recording_listbox.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))
    recording_listbox.bind('<<ListboxSelect>>', select_recording)

    update_recording_listbox()  # Initial update of the listbox

    # Entry for renaming recordings
    recording_name_entry = tk.Entry(window)
    recording_name_entry.pack(fill=tk.X, padx=10, pady=(0, 10))

    window.mainloop()

if __name__ == "__main__":
    gui()