//this project was created for VTI Deinze for proj with robotarm here we used a arduino mega with a ramps shield
//DRV8825's stepper motor and servo motor
//©maarten 2022-23
#include <AccelStepper.h>
#include <Servo.h>

// For RAMPS 1.4
#define X_STEP_PIN         54  //elbow
#define X_DIR_PIN          55  // grijs
#define X_ENABLE_PIN       38
//#define X_MIN_PIN           3
//#define X_MAX_PIN           2
 
#define Y_STEP_PIN         60 //control for wrist 
#define Y_DIR_PIN          61 //cyan
#define Y_ENABLE_PIN       56
//#define Y_MIN_PIN          14
//#define Y_MAX_PIN          15
 
#define Z_STEP_PIN         46 //shoulder
#define Z_DIR_PIN          48 //groen
#define Z_ENABLE_PIN       62
//#define Z_MIN_PIN          18
//#define Z_MAX_PIN          19
 
#define E_STEP_PIN         26 //base_lefT-RIGHT
#define E_DIR_PIN          28 //geel oranje
#define E_ENABLE_PIN       24

#define R_STEP_PIN         36 //wrist-Rotate
#define R_DIR_PIN          34 //geel oranje
#define R_ENABLE_PIN       30

// Define servo motor pin
#define SERVO_PIN 5

// Define servo angles for each position
#define SERVO_ANGLE_1 90
#define SERVO_ANGLE_2 0
#define SERVO_ANGLE_3 180

// Create stepper motor objects
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);
AccelStepper stepperE(AccelStepper::DRIVER, E_STEP_PIN, E_DIR_PIN);
AccelStepper stepperR(AccelStepper::DRIVER, R_STEP_PIN, R_DIR_PIN);

// Create servo motor object
Servo servoMotor;

// Define variables to store current positions
int currentPositionX = 1;
int currentPositionY = 1;
int currentPositionZ = 1;
int currentPositionE = 1;
int currentPositionR = 1;


void setup() {
  // Initialize the Serial Monitor
  Serial.begin(9600);
  //©maarten2022-2023 thi
   Serial.print("starting up arduino");
   delay(250);
   Serial.print("..");
   delay(250);
   Serial.print("..");
   delay(250);
   Serial.println("..");
   delay(500);
  Serial.println("no arduino found :(");
  delay(500);
   Serial.println("Guillaume stole it again ╯︿╰ ");
   delay(500);
   Serial.println("kies motor en dan waarde tussen 100 - 0 - -100");
  // Set the maximum speed and acceleration for the stepper motors
  stepperX.setMaxSpeed(2000);
  stepperX.setAcceleration(1000);

  stepperY.setMaxSpeed(2000);
  stepperY.setAcceleration(1000);

  stepperZ.setMaxSpeed(2000);
  stepperZ.setAcceleration(1000);

  stepperE.setMaxSpeed(2000);
  stepperE.setAcceleration(1000);

  stepperR.setMaxSpeed(2000);
  stepperR.setAcceleration(1000);

  // Attach the servo motor to the servo pin
  servoMotor.attach(SERVO_PIN);
// Set the enable pins for each motor to output mode
  pinMode(X_ENABLE_PIN, OUTPUT);
  pinMode(Y_ENABLE_PIN, OUTPUT);
  pinMode(Z_ENABLE_PIN, OUTPUT);
  pinMode(E_ENABLE_PIN, OUTPUT);
  pinMode(R_ENABLE_PIN, OUTPUT);
  // Enable each motor
  digitalWrite(X_ENABLE_PIN, LOW);
  digitalWrite(Y_ENABLE_PIN, LOW);
  digitalWrite(Z_ENABLE_PIN, LOW);
  digitalWrite(E_ENABLE_PIN, LOW);
  digitalWrite(R_ENABLE_PIN, LOW);
}

void loop() {
 

  // Check if there is any input from the Serial Monitor
  if (Serial.available() > 0) {
    // Read the input and convert it to an integer
    int motorNumber = Serial.parseInt();
    int position = Serial.parseInt();
    

    // Move the appropriate motor to the specified position
    if (motorNumber == 1) {
      if 
      stepperX.moveTo(position);
      // Store the current position of the motor
      currentPositionX = position;
      // Print the current position of the motor
      Serial.println("Stepper motor Base is now at position " + String(currentPositionX));
    } else if (motorNumber == 2) {
      stepperY.moveTo(position);
      // Store the current position of the motor
      currentPositionY = position;
      // Print the current position of the motor
      Serial.println("Stepper motor Shoulder is now at position " + String(currentPositionY));
    } else if (motorNumber == 3) {
      stepperZ.moveTo(position);
      // Store the current position of the motor
      currentPositionZ = position;
      // Print the current position of the motor
      Serial.println("Stepper motor elbow is now at position " + String(currentPositionZ));
    } else if (motorNumber == 4) {
      stepperE.moveTo(position);
      // Store the current position of the motor
      currentPositionE = position;
      // Print the current position of the motor
Serial.println("Stepper motor wrist is now at position " + String(currentPositionE));
} else if (motorNumber == 5) {
      stepperR.moveTo(position);
      // Store the current position of the motor
      currentPositionR = position;
      // Print the current position of the motor
Serial.println("Stepper motor wrist is now at position " + String(currentPositionR));
} else if (motorNumber == 6) {
// Set the servo angle based on the specified position
if (position == 1) {
servoMotor.write(SERVO_ANGLE_1);
} else if (position == 2) {
servoMotor.write(SERVO_ANGLE_2);
} else if (position == 3) {
servoMotor.write(SERVO_ANGLE_3);
}
// Print the current position of the servo motor
Serial.println("Servo motor is now at position " + String(position));
}
else if (motorNumber >= 7) {
     Serial.println("are you stupid there are only 6 motors to control");
}
}


// Move the stepper motors to their target positions
stepperX.run();
stepperY.run();
stepperZ.run();
stepperE.run();
stepperR.run();
}