Project showcase

Pick and Place Delta Robot (Controlled by Arduino Mega) © MIT

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

  • 5,728 views
  • 5 comments
  • 63 respects

Components and supplies

Necessary tools and machines

3drag
3D Printer (generic)

Apps and online services

About this project

Building industrial robots do not necessarily mean spending lots of money on expensive hardware. Built with fischertechnik (a construction toy originating from Germany) and controlled by an Arduino Mega, this delta robot is a fairly simple machine. Nonetheless, it does not lack speed or precision to fulfill its job: The robot picks items from a conveyor and sorts them by color similarly to industrial pick and place robots.

Delta Robot as pick and place robot

What are the main challenges of this project?

  • Delta robot kinematics
  • Motion tracking of items on conveyor
  • Vacuum gripping

Delta Robot Kinematics

Unlike simple kinematics such as linear kinematics that are often used in 3D printers, delta robot kinematics are more complicated and not straightforward to understand.To control a delta robot with cartesian coordinates, so-called inverse kinematics is required. I've implemented inverse kinematics for delta robots according to a paper published by Robert L. Williams II: "The Delta Parallel Robot: Kinematics Solutions" Although this seems to be computational expensive, delta kinematics have some good advanteges over other robot kinematics. Firstly, they are astoundingly fast and precise, secondly the have quite a good range of operation. However, vibrations are a serious issue and moving load should be as low as possible.

The following video gives you an intuition of what is possible with delta robots:

Delta Robot kinematics

Motion tracking The position of every item is captured by a laser light barrier. This lets the robot know when the item has passed a certain position on the conveyor. Furthermore, the conveyor speed is measured by reading a encoder signal. This lets the robot know how fast the item is moving. Given that imformation, the robot's movement can be calculated so that it picks the item at the right time.

Vacuum gripper The items are manipulated by an vacuum gripper. Since I do not have a vacuum pump, I've looked for another solution to this task:

There are two pneumatic cylinders mounted next to each other. The first cylinder is connected to a valve the either lets pressure in or pressure out. The second cylinder is connected to a vacuum cup. When pressure is applied to the first cylinder it extends. As both pistons are mechanically connected to each other, the movement of the first cylinder also force the second cylinder to extend. However, this movement generates a vacuum inside the second cylinder. This vacuum is used for picking the items.

Code

DeltaRobot2Arduino
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();
  }
}

Custom parts and enclosures

Vacuum Gripper
Mouint vaccum gripper

Schematics

Arduino Mega Shield
PCB
Uploads2ftmp2ff0ef069d 3eeb 4095 8de1 406df1c129cc2fimg 3671 pjxv6dtscm

Comments

Similar projects you might like

Amazing 6WD Off-Road Robot | Arduino RC Robot

Project tutorial by Jithin Sanal

  • 6,018 views
  • 1 comment
  • 46 respects

Arduino Controlled Delta Robot

Project showcase by geordag

  • 2,424 views
  • 2 comments
  • 12 respects

OttoDIY Build Your Own Robot in One Hour!

Project tutorial by Camilo Parra Palacio

  • 123,683 views
  • 135 comments
  • 350 respects

Otto DIY+ Arduino Bluetooth Robot Easy to 3D Print

Project tutorial by Team Otto builders

  • 65,676 views
  • 131 comments
  • 205 respects

MeArm Robot Arm - Your Robot - V1.0

Project tutorial by Benjamin Gray

  • 25,474 views
  • 4 comments
  • 41 respects

Control your Human-Like Robot from Anywhere you Are!

Project showcase by Team assemblypro

  • 3,349 views
  • 1 comment
  • 16 respects
Add projectSign up / Login