Pick and Place Delta Robot (Controlled by Arduino Mega)

#include <TimerOne.h>
#include <DeltaRobotKinematics.h>
#include <MegaDueShield.h>

#define LOW_SPEED 50
#define MEDIUM_SPEED 80
#define MAX_SPEED 100

/* Vacuum gripper 1_1
    June 2019
    - pressure control
    - adaptive conveyor speed

    July 2019
    - stack
    - color filter
*/

DeltaRobotKinematics delta;

StepperMotor & mot1 = *shield.getStepper(2); // pre-configured stepper
StepperMotor & mot2 = *shield.getStepper(1);
StepperMotor mot3;  // half-bridge based stepper motor

DCMotor coil31;
DCMotor coil32;

DCMotor conveyor;
DCMotor gripper;
DCMotor compressor;
DCMotor laser;
DCMotor separator;

long posM1 = 0;
long posM2 = 0;
long posM3 = 0;

volatile int posConveyor = 0;
volatile int pressure;

const uint8_t limitSwitch1 = I5;
const uint8_t limitSwitch2 = I6;
const uint8_t limitSwitch3 = I8;

const uint8_t conveyorEncoder = C3;
const uint8_t photocell = C4;
const uint8_t stack = D4;
const uint8_t MPX_pin = I3;
const uint8_t colorSensor = I4;

const int whiteLowerThreshold = 100;
const int whiteUpperThreshold = 175;
const int redLowerThreshold = 196;
const int redUpperThreshold = 270;
const int blueLowerThreshold = 480;
const int blueUpperThreshold = 510;

bool pass = false;
long passTime = 0;

uint8_t currentSpeedProfile = LOW_SPEED;


void pressureControl()
{
  pressure = analogRead(MPX_pin);
  if (compressor.getSpeed() < 1)
  {
    
    if (pressure < 140)
    {
      compressor.on();
    }
  }
  else
  {
    if (pressure > 187)
    {
      compressor.off();
    }
  }
}

int rad2StepM1(double radian)
{
  return (int) ((radian + 0.785) * 95.51);
}
int rad2StepM2(double radian)
{
  return (int) ((radian + 0.785) * 127.35);
}
int rad2StepM3(double radian)
{
  return (int) ((radian + 0.785) * 127.35);
}

void posP2P(double x, double y, double z)
{
  delta.inverseKinematics(x, y, z);
  bool t = false;
  while (!t)
  {
    t = mot1.stepping(posM1, rad2StepM1(delta.getPhi2()));
    t &= mot2.stepping(posM2, rad2StepM2(delta.getPhi3()));
    t &= mot3.stepping(posM3, rad2StepM3(delta.getPhi1()));
  }
}

void posCP(double x1, double y1, double z1, double x2, double y2, double z2)
{
  posP2P(x1, y1, z1);
  double vect[] = {x2 - x1, y2 - y1, z2 - z1};
  double vectLength = sqrt(pow(vect[0], 2) + pow(vect[1], 2) + pow(vect[2], 2));
  double unitVect[3];
  for (int i = 0; i < 3; i++)
  {
    unitVect[i] = vect[i] / vectLength / 2.0; // unitVect: Length 0.5mm
  }
  vect[0] = x1;
  vect[1] = y1;
  vect[2] = z1;
  int iterations = vectLength * 2; // equivalent to vectLength / 0.5
  for (int i = 0; i < iterations; i++)
  {
    vect[0] += unitVect[0];
    vect[1] += unitVect[1];
    vect[2] += unitVect[2];
    posP2P(vect[0], vect[1], vect[2]);
  }
  posP2P(x2, y2, z2);


}

void speedProfile(int profile)
{
  mot1.velocity(profile);
  mot2.velocity(profile);
  mot3.velocity(profile);
}

int colorDetection()
{
  int n = 4; // #of measurements
  long measurements[n];
  for (int i = 0; i < n; i++)
  {
    measurements[i] = analogRead(colorSensor);
    delay(30);
  }
  // calc average
  long avg = 0;
  for (int i = 0; i < n; i++)
  {
    avg += measurements[i];
  }
  avg /= n;
  // calc error
  long error = 0;
  for (int i = 0; i < n; i++)
  {
    error += (avg-measurements[i])*(avg-measurements[i]);
  }
  Serial.print("Error: ");
  error /= n;
  Serial.println(error);

  if (error < 3)
  {
    // measurement accurate, return average
    Serial.println(avg);
    return avg;
  }
  else
  {
    // measurement inaccurate, return 1023
    return 1023;
  }
}

void referencing()
{
  delay(50);
  bool ref = false;
  while (!ref)
  {
    ref = mot1.referencing(posM1);
    ref &= mot2.referencing(posM2);
    ref &= mot3.referencing(posM3);
  }
  delay(50);
}

// ISR
void conveyorISR()
{
  posConveyor++;
}

void setup() {
  Serial.begin(115200);
  Serial.println("Delta Robot @ MegaDueShield");

  delta.setUpperLegsLength(75);
  delta.setLowerLegsLength(180);
  delta.setBaseSize(41.83);
  delta.setPlatformSize(26);

  coil31 = *shield.getDCMotor(5);
  coil32 = *shield.getDCMotor(6);

  conveyor = *shield.getDCMotor(1);
  gripper = *shield.getDCMotor(7);
  compressor = *shield.getDCMotor(8);
  laser = *shield.getDCMotor(2);
  separator = *shield.getDCMotor(4);

  mot3.setHalfBridge(coil31, coil32);

  mot1.motorConfig(200, 80, 600, 400);
  mot2.motorConfig(200, 120, 800, 400);
  mot3.motorConfig(200, 60, 800, 400);

  mot1.attachLimitSwitch(limitSwitch1);
  mot2.attachLimitSwitch(limitSwitch2);
  mot3.attachLimitSwitch(limitSwitch3);

  pinMode(photocell, INPUT_PULLUP);
  pinMode(stack, INPUT_PULLUP);
  pinMode(conveyorEncoder, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(conveyorEncoder), conveyorISR, CHANGE);

  shield.rgb(0, 0, 150);
  delay(200);
  shield.rgbOff();

  referencing();
  delay(500);

  speedProfile(80);
  laser.on();

  
  Timer1.initialize(300000);  
  Timer1.attachInterrupt(pressureControl);  
}

void loop() {
  mot1.release();
  mot2.release();
  mot3.release();

  while (!digitalRead(stack))
  {
    // no item in stock
    if (!pass)
    {
       conveyor.off();
    }
    else
    {
      if ((millis() - passTime) > 2000)
      {
        // conveyor on until white item has passed
        pass = false;
      }
    } 
    delay(300);  
  }
  pass = false;
  conveyor.cw(135);
  delay(400);
  
  int colorVal = 1023;
  while (colorVal > 1000)
  {
    colorVal= colorDetection();
  }
  // separate
  separator.on();
  delay(80);
  separator.off();

  if (colorVal < whiteUpperThreshold)
  {
    // color is white
    delay(300);
    pass = true;
    passTime = millis();
  }
  else
  {
    // color is either red or blue, remove item from conveyor
    delay(20); // wait to prevent light barrier from triggering falsely
    while (!digitalRead(photocell))
    {

    }
    posConveyor = 0;
    long t0 = millis();

    while (posConveyor < 140)
    {}
    long t1 = millis();

    double xTrig = 285.0 - 37940.0 / (t1 - t0);

    while (posConveyor < xTrig)
    {
    }


    long t = millis();
    posP2P(-18, 10, -165);
    long t2 = millis();
    //delay(4000);
    //posCP(-21, 10, -167, -21, 10, -176);
    posP2P(-18, 10, -176);
    long t3 = millis();

    gripper.on();
    delay(200);
    

    // choose where to place the item based on color
    if (colorVal < blueLowerThreshold)
    {
      // color is red
      posP2P(0, 0, -160);
      posP2P(60, 0, -160);
    }
    else
    {
      // color is blue
      posP2P(-20, 30, -160);
      posP2P(-70, -20, -160);
    }

    gripper.off();
    delay(300);    
    posP2P(0, 0, -140);
    referencing();
  }
}