基于颜色分拣机器人

简介

随着科技的发展，自动化程度也越来越高，人们可以从重复性的工作岗位转向更有意义的职业。

制造工业机器人并不一定意味着在昂贵的硬件上花费大量资金。这款分拣机器人就是由Arduino Mega控制而成，是一款相当简单的机器。尽管如此，它并不缺乏速度或精度来完成它的工作：机器人从传送带上拾取物品，并按照颜色对它们进行分类，类似于工业拾取和放置机器人。

步骤一 材料准备

硬件准备：

Arduino Mega 2560

步进电机

通用3D打印机

软件准备：

Arduino IDE

Arduino web 编辑器

Arduino microsoft visual studio代码扩展

步骤二 原理说明

与3D打印机中经常使用的简单运动学（例如线性运动学）不同，这款机器人的运动过程更复杂且不易理解。为了控制具有笛卡尔坐标的三角洲机器人，需要所谓的反向运动学。根据Robert L. Williams II发表的一篇论文，已经实现了三角机器人的逆运动学。 尽管这似乎是计算上的昂贵，但三角运动学比其他机器人运动学有一些很好的优势。首先，它们非常快速和精确，其次具有相当好的操作范围。

运动跟踪位置由激光光栅捕获。这使机器人知道物品何时通过传送带上的某个位置。此外，通过读取编码器信号来测量传送带速度。这让机器人知道物品移动的速度。鉴于该信息，可以计算机器人的运动，以便在正确的时间选择物品。

真空夹具物品由真空夹具操纵。由于我没有真空泵，我已经找到了另一种解决方案：

有两个气缸彼此相邻安装。第一个气缸连接到一个阀门，要么压入或压出。第二个圆筒连接到真空杯。当压力施加到第一气缸时，它延伸。由于两个活塞彼此机械连接，第一气缸的运动也迫使第二气缸延伸，该运动在第二气缸内产生真空。该真空用于拾取物品。

步骤三 编写程序

#include 

#include 

#include 



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

  }

}

步骤四 验证结果

最终我们出色的完成了我们的作品，在这里我们可以看到机器人可以成功的分拣物体了。