Pick and Place Delta Robot (Controlled by Arduino Mega)

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

Jul 28, 2019

•

31885 views

•

47 respects

Devices & Components

Arduino Mega 2560 Rev3

Stepper Motor, Mini Step

Hardware & Tools

3D Printer (generic)

Software & Tools

Visual Studio Code Extension for Arduino

Arduino Web Editor

Arduino IDE

Project description

Code

DeltaRobot2

arduino

Vacuum gripper June 2019 - pressure control - adaptive conveyor speed July 2019 - stack - color filter

1#include <TimerOne.h>
2#include <DeltaRobotKinematics.h>
3#include  <MegaDueShield.h>
4
5#define LOW_SPEED 50
6#define MEDIUM_SPEED 80
7#define  MAX_SPEED 100
8
9/* Vacuum gripper 1_1
10    June 2019
11    - pressure control
12    - adaptive conveyor speed
13
14    July 2019
15    - stack
16    - color  filter
17*/
18
19DeltaRobotKinematics delta;
20
21StepperMotor & mot1 = *shield.getStepper(2);  // pre-configured stepper
22StepperMotor & mot2 = *shield.getStepper(1);
23StepperMotor  mot3;  // half-bridge based stepper motor
24
25DCMotor coil31;
26DCMotor coil32;
27
28DCMotor  conveyor;
29DCMotor gripper;
30DCMotor compressor;
31DCMotor laser;
32DCMotor  separator;
33
34long posM1 = 0;
35long posM2 = 0;
36long posM3 = 0;
37
38volatile  int posConveyor = 0;
39volatile int pressure;
40
41const uint8_t limitSwitch1  = I5;
42const uint8_t limitSwitch2 = I6;
43const uint8_t limitSwitch3 = I8;
44
45const  uint8_t conveyorEncoder = C3;
46const uint8_t photocell = C4;
47const uint8_t  stack = D4;
48const uint8_t MPX_pin = I3;
49const uint8_t colorSensor = I4;
50
51const  int whiteLowerThreshold = 100;
52const int whiteUpperThreshold = 175;
53const  int redLowerThreshold = 196;
54const int redUpperThreshold = 270;
55const int  blueLowerThreshold = 480;
56const int blueUpperThreshold = 510;
57
58bool pass  = false;
59long passTime = 0;
60
61uint8_t currentSpeedProfile = LOW_SPEED;
62
63
64void  pressureControl()
65{
66  pressure = analogRead(MPX_pin);
67  if (compressor.getSpeed()  < 1)
68  {
69    
70    if (pressure < 140)
71    {
72      compressor.on();
73    }
74  }
75  else
76  {
77    if (pressure > 187)
78    {
79      compressor.off();
80    }
81  }
82}
83
84int rad2StepM1(double radian)
85{
86  return (int) ((radian  + 0.785) * 95.51);
87}
88int rad2StepM2(double radian)
89{
90  return (int)  ((radian + 0.785) * 127.35);
91}
92int rad2StepM3(double radian)
93{
94  return  (int) ((radian + 0.785) * 127.35);
95}
96
97void posP2P(double x, double y, double  z)
98{
99  delta.inverseKinematics(x, y, z);
100  bool t = false;
101  while (!t)
102  {
103    t = mot1.stepping(posM1, rad2StepM1(delta.getPhi2()));
104    t &= mot2.stepping(posM2,  rad2StepM2(delta.getPhi3()));
105    t &= mot3.stepping(posM3, rad2StepM3(delta.getPhi1()));
106  }
107}
108
109void posCP(double x1, double y1, double z1, double x2, double y2,  double z2)
110{
111  posP2P(x1, y1, z1);
112  double vect[] = {x2 - x1, y2 - y1,  z2 - z1};
113  double vectLength = sqrt(pow(vect[0], 2) + pow(vect[1], 2) + pow(vect[2],  2));
114  double unitVect[3];
115  for (int i = 0; i < 3; i++)
116  {
117    unitVect[i]  = vect[i] / vectLength / 2.0; // unitVect: Length 0.5mm
118  }
119  vect[0] = x1;
120  vect[1] = y1;
121  vect[2] = z1;
122  int iterations = vectLength * 2; // equivalent  to vectLength / 0.5
123  for (int i = 0; i < iterations; i++)
124  {
125    vect[0]  += unitVect[0];
126    vect[1] += unitVect[1];
127    vect[2] += unitVect[2];
128    posP2P(vect[0], vect[1], vect[2]);
129  }
130  posP2P(x2, y2, z2);
131
132
133}
134
135void  speedProfile(int profile)
136{
137  mot1.velocity(profile);
138  mot2.velocity(profile);
139  mot3.velocity(profile);
140}
141
142int colorDetection()
143{
144  int n = 4;  // #of measurements
145  long measurements[n];
146  for (int i = 0; i < n; i++)
147  {
148    measurements[i] = analogRead(colorSensor);
149    delay(30);
150  }
151  // calc average
152  long avg = 0;
153  for (int i = 0; i < n; i++)
154  {
155    avg += measurements[i];
156  }
157  avg /= n;
158  // calc error
159  long error  = 0;
160  for (int i = 0; i < n; i++)
161  {
162    error += (avg-measurements[i])*(avg-measurements[i]);
163  }
164  Serial.print("Error: ");
165  error /= n;
166  Serial.println(error);
167
168  if (error < 3)
169  {
170    // measurement accurate, return average
171    Serial.println(avg);
172    return avg;
173  }
174  else
175  {
176    // measurement inaccurate, return  1023
177    return 1023;
178  }
179}
180
181void referencing()
182{
183  delay(50);
184  bool ref = false;
185  while (!ref)
186  {
187    ref = mot1.referencing(posM1);
188    ref &= mot2.referencing(posM2);
189    ref &= mot3.referencing(posM3);
190  }
191  delay(50);
192}
193
194// ISR
195void conveyorISR()
196{
197  posConveyor++;
198}
199
200void  setup() {
201  Serial.begin(115200);
202  Serial.println("Delta Robot @ MegaDueShield");
203
204  delta.setUpperLegsLength(75);
205  delta.setLowerLegsLength(180);
206  delta.setBaseSize(41.83);
207  delta.setPlatformSize(26);
208
209  coil31 = *shield.getDCMotor(5);
210  coil32  = *shield.getDCMotor(6);
211
212  conveyor = *shield.getDCMotor(1);
213  gripper  = *shield.getDCMotor(7);
214  compressor = *shield.getDCMotor(8);
215  laser = *shield.getDCMotor(2);
216  separator = *shield.getDCMotor(4);
217
218  mot3.setHalfBridge(coil31, coil32);
219
220  mot1.motorConfig(200, 80, 600, 400);
221  mot2.motorConfig(200, 120, 800, 400);
222  mot3.motorConfig(200, 60, 800, 400);
223
224  mot1.attachLimitSwitch(limitSwitch1);
225  mot2.attachLimitSwitch(limitSwitch2);
226  mot3.attachLimitSwitch(limitSwitch3);
227
228  pinMode(photocell, INPUT_PULLUP);
229  pinMode(stack, INPUT_PULLUP);
230  pinMode(conveyorEncoder,  INPUT_PULLUP);
231  attachInterrupt(digitalPinToInterrupt(conveyorEncoder), conveyorISR,  CHANGE);
232
233  shield.rgb(0, 0, 150);
234  delay(200);
235  shield.rgbOff();
236
237  referencing();
238  delay(500);
239
240  speedProfile(80);
241  laser.on();
242
243  
244  Timer1.initialize(300000);  
245  Timer1.attachInterrupt(pressureControl);  
246}
247
248void loop() {
249  mot1.release();
250  mot2.release();
251  mot3.release();
252
253  while (!digitalRead(stack))
254  {
255    // no item in stock
256    if (!pass)
257    {
258       conveyor.off();
259    }
260    else
261    {
262      if ((millis()  - passTime) > 2000)
263      {
264        // conveyor on until white item has passed
265        pass = false;
266      }
267    } 
268    delay(300);  
269  }
270  pass  = false;
271  conveyor.cw(135);
272  delay(400);
273  
274  int colorVal = 1023;
275  while (colorVal > 1000)
276  {
277    colorVal= colorDetection();
278  }
279  //  separate
280  separator.on();
281  delay(80);
282  separator.off();
283
284  if  (colorVal < whiteUpperThreshold)
285  {
286    // color is white
287    delay(300);
288    pass = true;
289    passTime = millis();
290  }
291  else
292  {
293    // color  is either red or blue, remove item from conveyor
294    delay(20); // wait to prevent  light barrier from triggering falsely
295    while (!digitalRead(photocell))
296    {
297
298    }
299    posConveyor = 0;
300    long t0 = millis();
301
302    while  (posConveyor < 140)
303    {}
304    long t1 = millis();
305
306    double xTrig  = 285.0 - 37940.0 / (t1 - t0);
307
308    while (posConveyor < xTrig)
309    {
310    }
311
312
313    long t = millis();
314    posP2P(-18, 10, -165);
315    long  t2 = millis();
316    //delay(4000);
317    //posCP(-21, 10, -167, -21, 10, -176);
318    posP2P(-18, 10, -176);
319    long t3 = millis();
320
321    gripper.on();
322    delay(200);
323    
324
325    // choose where to place the item based on color
326    if (colorVal < blueLowerThreshold)
327    {
328      // color is red
329      posP2P(0,  0, -160);
330      posP2P(60, 0, -160);
331    }
332    else
333    {
334      //  color is blue
335      posP2P(-20, 30, -160);
336      posP2P(-70, -20, -160);
337    }
338
339    gripper.off();
340    delay(300);    
341    posP2P(0, 0, -140);
342    referencing();
343  }
344}

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

Downloadable files

Arduino Mega Shield

PCB

Arduino Mega Shield

PCB

Arduino Mega Shield

Documentation

Vacuum Gripper

Mouint vaccum gripper

Vacuum Gripper

Mouint vaccum gripper

Vacuum Gripper

Comments

Only logged in users can leave comments

ArduinoFT

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content