David
Published © MIT

Pick and Place Delta Robot (Controlled by Arduino Mega)

An Arduino-controlled delta robot built with fischertechnik: fast and precise pick and place robot.

AdvancedShowcase (no instructions)4 days7,775

Things used in this project

Hardware components

Arduino Mega 2560 & Genuino Mega 2560
Arduino Mega 2560 & Genuino Mega 2560
×1
Stepper Motor, Mini Step
Stepper Motor, Mini Step
×3

Software apps and online services

Arduino IDE
Arduino IDE
Arduino Web Editor
Arduino Web Editor
Visual Studio Code Extension for Arduino
Microsoft Visual Studio Code Extension for Arduino

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)

Story

Read more

Custom parts and enclosures

Vacuum Gripper

Mouint vaccum gripper

Schematics

Arduino Mega Shield

PCB

Code

DeltaRobot2

Arduino
Vacuum gripper
June 2019
- pressure control
- adaptive conveyor speed

July 2019
- stack
- color filter
#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();
  }
}

Credits

David

David

4 projects • 84 followers
Engineering student

Comments