Robotic 3D Part Assembly Robot

// Guide Video: https://www.youtube.com/watch?v=zOtd8Tih-BI&list=PLP-VDZmS6bFoh96ohE4qmqoXlb6si--EM
//  Example sketch to control a stepper motor with A4988 stepper motor driver, 
//  AccelStepper library and Arduino: continuous rotation. 
//  More info: https://www.makerguides.com 


// Include the AccelStepper library:
#include "AccelStepper.h"

// Define stepper motor connections and motor interface type. 
// Motor interface type must be set to 1 when using a driver
#define dirPin1 32
#define stepPin1 47
#define enablePin1 38
#define limitPin1 3

#define dirPinX 55
#define stepPinX 54
#define enablePinX 56
#define limitPinX 2

#define dirPinY 61
#define stepPinY 60
#define enablePinY 56
#define limitPinY 14

#define dirPinZ 48
#define stepPinZ 46
#define enablePinZ 62
#define limitPinZ 15

#define dirPinE0 28
#define stepPinE0 26
#define enablePinE0 24
#define limitPinE0 19

#define dirPinE1 34
#define stepPinE1 36
#define enablePinE1 30
#define limitPinE1 18

#define motorInterfaceType 1
#define statusPin 13

int offsetX = 220;
int offset1 = 200;
int offsetY = 200;
int offsetZ = 200;
int offsetE0 = 200;
int offsetE1 = 200;

int Serial_In;

// Create a new instance of the AccelStepper class:
AccelStepper stepper1 = AccelStepper(motorInterfaceType, stepPin1, dirPin1);
AccelStepper stepperX = AccelStepper(motorInterfaceType, stepPinX, dirPinX);
AccelStepper stepperY = AccelStepper(motorInterfaceType, stepPinY, dirPinY);
AccelStepper stepperZ = AccelStepper(motorInterfaceType, stepPinZ, dirPinZ);
AccelStepper stepperE0 = AccelStepper(motorInterfaceType, stepPinE0, dirPinE0);
AccelStepper stepperE1 = AccelStepper(motorInterfaceType, stepPinE1, dirPinE1);

AccelStepper *stepper[6] = {&stepper1,&stepperX,&stepperY,&stepperZ,&stepperE0,&stepperE1};

int limitPin[] = {limitPin1,limitPinX,limitPinY,limitPinZ,limitPinE0,limitPinE1};
int enablePin[] = {enablePin1,enablePinX,enablePinY,enablePinZ,enablePinE0,enablePinE1};
int startPos[] = {};

int calibrationSpeed[6] = {3000,3000,1500,800,800,800};
int calibrationAccel[6] = {25000,55000,5000,5000,5000,5000};
int stepperSpeed[6] = {12000,12000,12000,5000,5000,1000};
int stepperAccel[6] = {15000,60000,15000,15000,15000,15000};
int calbrationPos[6] = {0,11000,-3100,-435,-200,0};
int lLimitAngle[6] = {0,-45,5,0,0,0};
int uLimitAngle[6] = {170,90,180,350,180,350};


double stepsPerRev = 0.5556;
int reduction[6] = {16,25,25,4,4,4};
int microsteps[6] = {32,16,16,16,16,16};
int stepperDir[] = {-1,1,1,1,1,1};

void moveToAngle(int nStepper, double angle){
  if ((angle>=lLimitAngle[nStepper]) && (angle<=uLimitAngle[nStepper])){
  double steps = -angle*(double)stepsPerRev*(double)reduction[nStepper]*(double)microsteps[nStepper]*(double)stepperDir[nStepper];
  Serial.println(steps);
  stepper[nStepper]->moveTo(steps);  
  }
}

//double x = 150;
//double z = 200;

double L1 = 247.65;
double La = 66.657;
double Lb = 250.825;
double L2 = sqrt(La*La + Lb*Lb);
double thetaA = asin(La/L2);

void inverseKinematics(double x, double y, double z){

double thetaX = atan(y/x) * (180/3.14);;
   
double r = sqrt(x*x+y*y);

double beta = acos((L1*L1 + L2*L2 - r*r - z*z)/(2*L1*L2));
double alpha = acos((r*r + z*z + L1*L1 - L2*L2)/(2*L1*sqrt(r*r+z*z)));
double gamma = atan2(z,r);

double theta1 = (-(gamma-alpha)+3.14) * (180/3.14);
double theta2 = (-beta-thetaA) * (180/3.14);

Serial.println(beta);
Serial.println(alpha);
Serial.println(gamma);
Serial.println(theta1);
Serial.println(theta2);

if ((theta1 >= lLimitAngle[0]) && (theta1 <= uLimitAngle[0]) && (theta2 >= lLimitAngle[2]) && (theta2 <= uLimitAngle[2])){
   moveToAngle(0,theta1);
   moveToAngle(2,theta2);
   moveToAngle(1,thetaX);
   double thetaG = theta1-theta2+3.14;
   moveToAngle(4,thetaG);
} else {
  double theta1 = (-(gamma+alpha)+3.14) * (180/3.14);
  double theta2 = (beta-thetaA) * (180/3.14);  
  if ((theta1 >= lLimitAngle[0]) && (theta1 <= uLimitAngle[0]) && (theta2 >= lLimitAngle[2]) && (theta2 <= uLimitAngle[2])){
    moveToAngle(0,theta1);
    moveToAngle(2,theta2);
    moveToAngle(1,thetaX);
    double thetaG = theta1-theta2+3.14;
    moveToAngle(4,thetaG);
  }
}


}

void setup() {
  Serial.begin(115200);
  pinMode(statusPin,OUTPUT);
  
  // Set the maximum speed in steps per second:
  for (int i =0;i<6;i++){
  pinMode(enablePin[i],OUTPUT);
  digitalWrite(enablePin[i],LOW);
  pinMode(limitPin[i], INPUT);
  }
  
  for (int i=0; i<6;i++){
  stepper[i]->setMaxSpeed(calibrationSpeed[i]);
  stepper[i]->setAcceleration(calibrationAccel[i]);  
  }
  
  calibrateArm();

  //moveToAngle(0,70);
  //moveToAngle(1,0);
  //moveToAngle(2,30);
  moveToAngle(3,90);
  for (int i=0; i<6;i++){
    stepper[i]->runToPosition();
    
  }
  inverseKinematics(150,0,200);
}

int running_stepper = 0;


void calibrateArm(){
  for (int i =0;i<4;i++){
  digitalWrite(statusPin,HIGH);
  delay(400);
  digitalWrite(statusPin,LOW);
  delay(400);
  }

  for (int i =0;i<5;i++){
    // CALIBRATE X STEPPER
    int running_stepper = 1;
  
    // Coarse Calibration ------------------------------------------------------------------------------------------------------------------
    stepper[i]->moveTo(stepper[i]->currentPosition()+5000000*stepperDir[i]);
    while (running_stepper == 1){
      if (digitalRead(limitPin[i])){
      stepper[i]->run();
      } else {
      stepper[i]->stop();
      running_stepper = 0;
      }
    }
    // To Ensure Stepper is no longer in contact with limit switch in fine calibration  ---------------------------------------------------
    
    stepper[i]->moveTo(stepper[i]->currentPosition()-5000000*stepperDir[i]);
    running_stepper = 1;
    while (running_stepper == 1){
      if (digitalRead(limitPin[i])){
      stepper[i]->stop();
      running_stepper = 0;
      } else {
      stepper[i]->run();
      }
    }  
    stepper[i]->runToNewPosition(stepper[i]->currentPosition()-500*stepperDir[i]);
    
    // Fine Calibration --------------------------------------------------------------------------------------------------------------------
    double fineSpeed = (double)calibrationSpeed[i]/3;
    Serial.println(fineSpeed);
    stepper[i]->stop();
    stepper[i]->setMaxSpeed(fineSpeed);  
    stepper[i]->moveTo(stepper[i]->currentPosition()+5000000*stepperDir[i]);
    running_stepper = 1;
    while (running_stepper == 1){
      if (digitalRead(limitPin[i])){
      stepper[i]->run();
      } else {
      stepper[i]->stop();
      running_stepper = 0;
      }
    }
    stepper[i]->setMaxSpeed(stepperSpeed[i]);
    stepper[i]->setAcceleration(stepperAccel[i]);  
    stepper[i]->setCurrentPosition(calbrationPos[i]);
    //stepper[i]->runToNewPosition(0);  // -- Just Used to Determine if Calibration Position is Correct May cause issues.
  }
}

double prevTime = 0;
int point = 0;

void loop() {
  // Set the speed in steps per second:
 
  if (Serial.available()) {     //wait for data available

      String teststr = Serial.readString();
      char cmd = teststr.charAt(0);
      Serial.println("Recieved:" + teststr);
      
      switch (cmd) {
      case 'p':    
      
      Serial.println(teststr);
      int indexX = teststr.indexOf('x');
      int indexY = teststr.indexOf('y');
      int indexZ = teststr.indexOf('z');
      int indexEnd = teststr.indexOf('l');

      double x = teststr.substring(indexX+1,indexY).toDouble();
      double y = teststr.substring(indexY+1,indexZ).toDouble();
      double z = teststr.substring(indexZ+1,indexEnd).toDouble();

      inverseKinematics(x,y,z);
      
      break;
      }
  }

  /*
  if (millis()-prevTime >= 10000){
    prevTime = millis();
    switch (point) {
      case 0:
      inverseKinematics(100,150,100);
      break;
      case 1:
      inverseKinematics(100,-150,100);
      break;
      case 2:
      inverseKinematics(400,-150,100);
      break;
      case 3:
      inverseKinematics(400,150,100);
      point = 0;
      break;
    }
    point++;
  }
  */
  
  stepper1.run();
  stepperX.run();
  stepperY.run();
  stepperZ.run();
  stepperE0.run();
  stepperE1.run();
}