//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>

// 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//E0

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

#define U_STEP_PIN         42 
#define U_DIR_PIN          40 //geel oranje
#define U_ENABLE_PIN       44//expation board


// Create stepper motor objects
AccelStepper stepperD(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperF(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperR(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);
AccelStepper stepperB(AccelStepper::DRIVER, E_STEP_PIN, E_DIR_PIN);
AccelStepper stepperL(AccelStepper::DRIVER, R_STEP_PIN, R_DIR_PIN);
AccelStepper stepperU(AccelStepper::DRIVER, U_STEP_PIN, U_DIR_PIN);



// Define variables to store current positions
int currentPositionD = 1;
int currentPositionF = 1;
int currentPositionR = 1;
int currentPositionB = 1;
int currentPositionL = 1;
int currentPositionU = 1;


void setup() {
  // Initialize the Serial Monitor
  Serial.begin(115200);
  Serial.println(" ");
  // Set the maximum speed and acceleration for the stepper motors
  stepperD.setMaxSpeed(2000);
  stepperD.setAcceleration(1000);

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

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

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

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

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

  // Attach the servo motor to the 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);
  pinMode(U_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);
  digitalWrite(U_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) {
      stepperD.moveTo(position);
      // Store the current position of the motor
      currentPositionD = position;
      // Print the current position of the motor
      Serial.println("Stepper motor Base is now at position " + String(currentPositionD));
    } else if (motorNumber == 2) {
      stepperF.moveTo(position);
      // Store the current position of the motor
      currentPositionF = position;
      // Print the current position of the motor
      Serial.println("Stepper motor Shoulder is now at position " + String(currentPositionF));
    } else if (motorNumber == 3) {
      stepperL.moveTo(position);
      // Store the current position of the motor
      currentPositionL = position;
      // Print the current position of the motor
      Serial.println("Stepper motor elbow is now at position " + String(currentPositionL));
    } else if (motorNumber == 4) {
      stepperB.moveTo(position);
      // Store the current position of the motor
      currentPositionB = position;
      // Print the current position of the motor
Serial.println("Stepper motor wrist is now at position " + String(currentPositionB));
} 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) {
stepperU.moveTo(position);
      // Store the current position of the motor
      currentPositionU = position;
      // Print the current position of the motor
Serial.println("stapper motor is now at position " + String(position));
}
}


// Move the stepper motors to their target positions
stepperF.run();
stepperD.run();
stepperR.run();
stepperL.run();
stepperB.run();
stepperU.run();

}