#include <AccelStepper.h>

// Define the pin connections for the A4988 drivers
#define LEFT_STEP_PIN 2
#define LEFT_DIR_PIN 3
#define RIGHT_STEP_PIN 4
#define RIGHT_DIR_PIN 5

// Create AccelStepper objects for each motor
AccelStepper leftMotor(AccelStepper::DRIVER, LEFT_STEP_PIN, LEFT_DIR_PIN);
AccelStepper rightMotor(AccelStepper::DRIVER, RIGHT_STEP_PIN, RIGHT_DIR_PIN);

void setup() {
  // Set maximum speed and acceleration for both motors
  leftMotor.setMaxSpeed(1000);  // Adjust max speed as needed
  leftMotor.setAcceleration(500);  // Adjust acceleration as needed
  rightMotor.setMaxSpeed(1000);  // Adjust max speed as needed
  rightMotor.setAcceleration(500);  // Adjust acceleration as needed

  // Start serial communication
  Serial.begin(9600);

  // Send initial messages to the Serial Monitor
  Serial.println("AccelStepper motor control ready. Send commands:");
  Serial.println("L+<steps>: Left motor forward");
  Serial.println("L-<steps>: Left motor backward");
  Serial.println("R+<steps>: Right motor forward");
  Serial.println("R-<steps>: Right motor backward");
}

void loop() {
  // Check if data is available to read
  if (Serial.available() > 0) {
    // Read the incoming command
    String command = Serial.readStringUntil('\n'); // Read until newline character

    // Parse the command
    char motor = command.charAt(0);  // 'L' or 'R'
    char direction = command.charAt(1);  // '+' or '-'
    int steps = command.substring(2).toInt();  // Convert the remaining part to an integer

    // Execute the command for the appropriate motor
    if (motor == 'L') {
      leftMotor.move((direction == '+') ? steps : -steps);  // Set the target position for left motor
      leftMotor.runToPosition();  // Move the left motor to the target position
    } else if (motor == 'R') {
      rightMotor.move((direction == '+') ? steps : -steps);  // Set the target position for right motor
      rightMotor.runToPosition();  // Move the right motor to the target position
    }

    // Send a confirmation back to the Serial Monitor
    Serial.print("Command executed: ");
    Serial.println(command);
  }

  // Continuously run the motors to their target positions
  leftMotor.run();
  rightMotor.run();
}