//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         6 
#define U_DIR_PIN          5 //geel oranje
#define U_ENABLE_PIN       4//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(40000);
  stepperD.setAcceleration(10000);

  stepperF.setMaxSpeed(40000);
  stepperF.setAcceleration(10000);

  stepperR.setMaxSpeed(40000);
  stepperR.setAcceleration(10000);

  stepperB.setMaxSpeed(40000);
  stepperB.setAcceleration(10000);

  stepperL.setMaxSpeed(40000);
  stepperL.setAcceleration(10000);

  stepperU.setMaxSpeed(40000);
  stepperU.setAcceleration(10000);

  // 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) {
//inlezen van motor en hoeveel stappen
    int motorNumber = Serial.parseInt();
    int position = Serial.parseInt();
    
//verplaatsen van de motor naar de juiste plaats
    if (motorNumber == 1) {
      stepperD.move(position);
      currentPositionD = position;
    } else if (motorNumber == 2) {
      stepperF.move(position);
      currentPositionF = position;
    } else if (motorNumber == 3) {
      stepperL.move(position);
      currentPositionL = position;
    } else if (motorNumber == 4) {
      stepperB.move(position);
      currentPositionB = position;
} else if (motorNumber == 5) {
      stepperR.move(position);
      currentPositionR = position;
} else if (motorNumber == 6) {
      stepperU.move(position);
      currentPositionU = position;
}else if (motorNumber > 6) {
Serial.println("error no drivers found");
}
}


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

}