// Arduino Mega 2560 Code for Robot Gripper Arm 
// with 2 Joysticks, 3 Servos, and 1 Stepper 

// Libraries
#include <Servo.h>
#include <AccelStepper.h>

// --- Pin Definitions ---

// Joystick 1 (X and Y Axis)
const int joy1XPin = A0;
const int joy1YPin = A1;

// Joystick 2 (X and Y Axis)
const int joy2XPin = A2;
const int joy2YPin = A3;

// Servo 1 (Shoulder Joint)
const int servo1Pin = 9;

// Servo 2 (Elbow Joint)
const int servo2Pin = 10;

// Servo 3 (Gripper)
const int servo3Pin = 11;

// Stepper Motor (Base Rotation)
const int stepPin = 5;
const int dirPin = 6;

// --- Global Variables ---

// Servo Objects
Servo servo1;
Servo servo2;
Servo servo3;

// Stepper Motor Object
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// Joystick Readings
int joy1XValue;
int joy1YValue;
int joy2XValue;
int joy2YValue;

// Servo Positions
int servo1Pos;
int servo2Pos;
int servo3Pos;

// Stepper Position
long stepperPos = 0;

// --- Constants ---

// Joystick Calibration Values
const int joy1XCenter = 512;
const int joy1YCenter = 512;
const int joy2XCenter = 512;
const int joy2YCenter = 512;

// Joystick Dead Zone
const int joyDeadZone = 50;

// Servo Position Limits
const int servo1Min = 0;
const int servo1Max = 180;
const int servo2Min = 0;
const int servo2Max = 180;
const int servo3Min = 0;
const int servo3Max = 180;

// Stepper Control
const int stepperMin = -1000; // Adjust as needed for your setup
const int stepperMax = 1000;  // Adjust as needed for your setup
const int stepperSpeed = 500;
const int stepperAccel = 200;

// --- Timer Variables ---
unsigned long previousMillis = 0;        // Store the last time the joystick was read
const long interval = 20;               // Interval at which to read joystick and update (milliseconds)

// --- Setup Function ---

void setup() {
  // Initialize Serial Communication
  Serial.begin(9600);

  // Attach Servo Objects to Pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);

  // Set Stepper Motor Speed and Acceleration
  stepper.setMaxSpeed(stepperSpeed);
  stepper.setAcceleration(stepperAccel);

  // Initialize Servo Positions
  servo1.write(90);
  servo2.write(90);
  servo3.write(90); 

  // Initialize Stepper to a known position (e.g., center)
  stepper.setCurrentPosition(0); 
}

// --- Loop Function ---

void loop() {
  // --- Timer Logic ---
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    // Save the last time the update was performed
    previousMillis = currentMillis;

    // --- Read Joystick Values ---
    joy1XValue = analogRead(joy1XPin);
    joy1YValue = analogRead(joy1YPin);
    joy2XValue = analogRead(joy2XPin);
    joy2YValue = analogRead(joy2YPin);

    // --- Control Stepper Motor (Base Rotation) ---
    if (abs(joy1XValue - joy1XCenter) > joyDeadZone) {
      stepperPos = map(joy1XValue, 0, 1023, stepperMax, stepperMin); 
      stepper.moveTo(stepperPos);
    }

    // --- Control Servo 1 (Shoulder) ---
    if (abs(joy1YValue - joy1YCenter) > joyDeadZone) {
      servo1Pos = map(joy1YValue, 0, 1023, servo1Max, servo1Min);
      servo1.write(servo1Pos);
    }

    // --- Control Servo 2 (Elbow) ---
    if (abs(joy2XValue - joy2XCenter) > joyDeadZone) {
      servo2Pos = map(joy2XValue, 0, 1023, servo2Max, servo2Min);
      servo2.write(servo2Pos);
    }

    // --- Control Servo 3 (Gripper) ---
    if (abs(joy2YValue - joy2YCenter) > joyDeadZone) {
      servo3Pos = map(joy2YValue, 0, 1023, servo3Max, servo3Min);
      servo3.write(servo3Pos);
    }
  }

  // --- Stepper Motor Control (Non-blocking) ---
  stepper.run(); // This needs to be called frequently to move the stepper smoothly
}