/*
https://forum.arduino.cc/t/lag-encounter-when-interfacing-multiple-servos/1264408
*/

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

#define HALFSTEP 8
#define STEPPER_DEADBAND 75
#define STEPPER_MAX_ROTATION 1536
#define SERVO_MIN_PULSE_WIDTH 544
#define SERVO_MAX_PULSE_WIDTH 2400
#define SERVO_AXIS_MAX_ANGLE 30

#define POTENTIOMETER_PIN A0
#define SERVO_AXIS_PIN 3
#define SERVO_ARM_PIN 5
#define JOYSTICK_AXIS_X_PIN A4
#define JOYSTICK_AXIS_Y_PIN A5

int potentiometerValue = 0;
int previousPotentiometerValue = 0;
int stepperMoveToPosition = 0;

const int servoAxisBoundaries = (SERVO_MAX_PULSE_WIDTH + SERVO_MIN_PULSE_WIDTH) / (180 / SERVO_AXIS_MAX_ANGLE);
int servoAxisPosition = 1500;
int servoArmPosition = 1500;

int joystickXValue = 0;
int joystickYValue = 0;

AccelStepper stepper(HALFSTEP, 8, 10, 9, 11);
Servo servoAxis;
Servo servoArm;

void setup() {
  Serial.begin(9600);
  
  pinMode(POTENTIOMETER_PIN, INPUT);
  pinMode(JOYSTICK_AXIS_X_PIN, INPUT);
  pinMode(JOYSTICK_AXIS_Y_PIN, INPUT);

  stepper.setMaxSpeed(1000.0);
  stepper.setAcceleration(100.0);
  stepper.setSpeed(500);

  servoAxis.attach(SERVO_AXIS_PIN, SERVO_MIN_PULSE_WIDTH, SERVO_MAX_PULSE_WIDTH);
  servoArm.attach(SERVO_ARM_PIN, SERVO_MIN_PULSE_WIDTH, SERVO_MAX_PULSE_WIDTH);
  servoAxis.writeMicroseconds(servoAxisPosition);
  servoArm.writeMicroseconds(servoArmPosition);
}

void loop() {
  potentiometerValue = constrain(analogRead(POTENTIOMETER_PIN), 100, 900);
  joystickXValue = constrain(analogRead(JOYSTICK_AXIS_X_PIN), 100, 900);
  joystickYValue = constrain(analogRead(JOYSTICK_AXIS_Y_PIN), 100, 900);


  if (abs(previousPotentiometerValue - potentiometerValue) > STEPPER_DEADBAND) {
      previousPotentiometerValue = potentiometerValue;
      stepperMoveToPosition = map(potentiometerValue, 100, 900, -STEPPER_MAX_ROTATION , STEPPER_MAX_ROTATION);
      stepper.moveTo(stepperMoveToPosition);
  }

  if (joystickYValue < 525 && servoAxisPosition > SERVO_MIN_PULSE_WIDTH + servoAxisBoundaries) {
    servoAxisPosition -= (abs(500 - joystickYValue)) / 125;
  }
  if (joystickYValue > 475 && servoAxisPosition < SERVO_MAX_PULSE_WIDTH - servoAxisBoundaries) {
    servoAxisPosition += (abs(500 - joystickYValue)) / 125;
  }
  if (joystickXValue < 525) {
    servoArmPosition -= (abs(500 - joystickXValue)) / 125;
  }
  if (joystickXValue > 475) {
    servoArmPosition += (abs(500 - joystickXValue)) / 125;
  }
  
  servoAxis.writeMicroseconds(servoAxisPosition);
  servoArm.writeMicroseconds(servoArmPosition);
  stepper.run();
}