ページを更新する

Dobot Magician UART 2020/03/12 ～ 2020/07/27

■Dobot Magician + Pixy2 + Arduino MEGA を使った Pick & Place のサンプルコードです。

・2020/07/27 コード中 "==" と書くべきところが、 "=" になっていたのを修正

※最小半径の判断、MOVL/ARC の切り替えは MOVL を使っているためで、MOVJ の場合そういった処理は不要になります。
　通常の Pick & Place では、始点-終点の座標だけが重要で、通過点の軌跡が直線である必要はないので、MOVJ が最適です。
　移動速度もかなり早くなります。
　・・・そういうことも分からない頃に作ったものなので、ご注意を！

/*
 * Dobot Magician UART Control for Arduino MEGA
 * Dobot + Pixy2 Pick and Place Demo 2019.12.04 f.izawa
 * URL : http://www.izawa-web.com/
 * e-mail : f.izawa@dream.com
 * 
 * 2019/12/06 ピック範囲の制限を追加。
 * 　　　　　　　Dobot の最小半径内を通る時、中間点を通るを追加。
 * 　　　　　　　制限範囲外の時は、ブザーを鳴らすを追加
 * 2020/07/27 コード中 "=="と書くべきところが "=" になっていたのを修正
 */
#include <Pixy2.h>

// **********************
// Dobot IOFunction 
// **********************
enum {
  IOFunctionDummy, // Do not config function
  IOFunctionPWM,   // PWM Output
  IOFunctionDO,    // IO Output
  IOFunctionDI,    // IO Input
  IOFunctionADC    // AD Input
};

// **********************
// ClearAllAlarmsState 
// **********************
int ClearAllAlarmsState(void)
{
  byte buf[6];
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 0; // len= 2 + Payload Len;
  buf[3] = 20;    // id;
  buf[4] = 1;     // ctrl r/w($01) = W, isQueued($02) = false;
  buf[5] = 0xEB;  // checkSum
  for (int i= 0; i<6; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 6;
  int index = 0;
  while (index < 6 && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  return (index == 6 && buf[3] == 20);
}
// **********************
// GetQueuedCmdCurrentIndex
// **********************
int GetQueuedCmdCurrentIndex(uint64_t &queuedCmdCurrentIndex, uint16_t msec)
{
  byte buf[14];
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 0; // len= 2 + Payload Len;
  buf[3] = 246;   // id;=0xF6
  buf[4] = 0;     // ctrl r/w = R, isQueued = False;
  buf[5] = 0x0A;  // checkSum
  for (int i = 0; i < 6; i++) Serial1.write(buf[i]);
  delay(msec);
  
  int len = 14;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 246){
    queuedCmdCurrentIndex = 
      buf[5+0] | buf[5+1] << 8 | buf[5+2] << 16 | buf[5+3] << 24 | buf[5+4] << 32 | buf[5+5] << 40 | buf[5+6] << 48 | buf[5+7] << 56;
    return true;
  }
  else
    return false;
}

// **********************
// GetPose
// **********************
int GetPose(float &x, float &y, float &z, float &r)
{
  union data{
   byte buf[4];
   float value;
  }f;

  byte buf[38];
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 0; // len= 2 + Payload Len;
  buf[3] = 10;    // id;
  buf[4] = 0;     // ctrl r/w = R, isQueued = False;
  buf[5] = 0xF6;  // checkSum
  for (int i = 0; i < 6; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 38;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 10){
    for (int i = 0; i < 4; i++) f.buf[i] = buf[5 + i];
    x = f.value;
    for (int i = 0; i < 4; i++) f.buf[i] = buf[9 + i];
    y  = f.value;
    for (int i = 0; i < 4; i++) f.buf[i] = buf[13 + i];
    z  = f.value;
    for (int i = 0; i < 4; i++) f.buf[i] = buf[17 + i];
    r  = float(f.value);
    // 以降アーム角度が続くが、省略
    // a0 : buf[21 + i];
    // a1 : buf[25 + i];
    // a2 : buf[29 + i];
    // a3 : buf[33 + i];
    return true;
  }
  else
    return false;  
}

// **********************
// SetEndEffectorSuctionCup
// **********************
int SetEndEffectorSuctionCup(byte ctrl, byte suck)
{
  byte buf[14];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 2; // len= 2 + Payload Len;
  buf[3] = 62;    // id;
  buf[4] = 3;     // ctrl r/w($01) = W, isQueued($02) = false;
  buf[5] = ctrl;  //isCtrlEnabled
  buf[6] = suck;  //issucked
  for (int i=3; i < 7; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[7] = checkSum;
  for (int i = 0; i < 8; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 14;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  return (index == len && buf[3] == 62);
}

// **********************
//  GetEndEffectorSuctionCup
// **********************
int GetEndEffectorSuctionCup(byte &isCtrlEnable, byte &isSucked)
{
  byte buf[8];
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 0; // len= 2 + Payload Len;
  buf[3] = 62;    // id;
  buf[4] = 0;     // ctrl r/w(0x01) = R, isQueued(0x02) = false;
  buf[5] = 0xC2;  // checkSum;
  for (int i = 0; i < 6; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 8;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 62){
    isCtrlEnable = buf[5];
    isSucked = buf[6];
    return true;    
  }
  else
    return false;
}

// **********************
// HomeCmd
// **********************
int HomeCmd(void)
{ 
  byte buf[14];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 4; // len= 2 + Payload Len;
  buf[3] = 31;    // id;
  buf[4] = 3;     // ctrl r/w = W, isQueued = True;
  // ダミー(4バイト)
  for (int i = 0; i < 4; i++) buf[5 + i] = 0;
  // チェックサム
  for (int i = 3; i<9; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[9] = checkSum;
  for (int i = 0; i < 10; i++) Serial1.write(buf[i]);
  delay(500); //最小 300 msec;
  
  int len = 14;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    Serial.print(buf[index], HEX);
    Serial.print(":");
    index++;
  }
  if (index == len && buf[3] == 31){
    uint64_t queuedCmdIndex =
      buf[5+0] | buf[5+1] << 8 | buf[5+2] << 16 | buf[5+3] << 24 | buf[5+4] << 32 | buf[5+5] << 40 | buf[5+6] << 48 | buf[5+7] << 56;
    Serial.print("\nqueuedCmdIndex=");
    char sbuf[50];
    sprintf(sbuf, "%ld", queuedCmdIndex);
    Serial.print(sbuf);
    // ホーミング完了まで待つ
    uint64_t executedCmdIndex = 0;
    while (executedCmdIndex < queuedCmdIndex){
      // 最小 1200 msec
      GetQueuedCmdCurrentIndex(executedCmdIndex, 1500);
      Serial.print("\nexecutedCmdIndex="); 
      sprintf(sbuf, "%ld", executedCmdIndex);
      Serial.print(sbuf);
    }   
    return true;
  }
  else{
    return false;
  }
}

// **********************
// PTPCmd
// **********************
int PTPCmd(float x, float y, float z, float r)
{
  union data{
    byte buf[4];
    float value;
  }f;

  byte buf[23];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 17; // len= 2 + Payload Len;
  buf[3] = 84;     // id;
  buf[4] = 3;      // ctrl r/w = W, isQueued = True;
  buf[5] = 2;      // PTPMode 2= MOVL_XYZ
  f.value = x;
  for (int i= 0 ; i < 4; i++) buf[6 + i] = f.buf[i];
  f.value = y;
  for (int i= 0 ; i < 4; i++) buf[10 + i] = f.buf[i];
  f.value = z;
  for (int i= 0 ; i < 4; i++) buf[14 + i] = f.buf[i];
  f.value = r;
  for (int i= 0 ; i < 4; i++) buf[18 + i] = f.buf[i];
  for (int i = 3; i < 22; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[22] = checkSum;
  for (int i = 0; i < 23; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 14;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 84){
    uint64_t queuedCmdIndex =
      buf[5+0] | buf[5+1] << 8 | buf[5+2] << 16 | buf[5+3] << 24 | buf[5+4] << 32 | buf[5+5] << 40 | buf[5+6] << 48 | buf[5+7] << 56;
    Serial.print("\nqueuedCmdIndex=");
    char sbuf[50];
    sprintf(sbuf, "%ld", queuedCmdIndex);
    Serial.print(sbuf);
    // 移動完了まで待つ         
    uint64_t executedCmdIndex = 0;
    while (executedCmdIndex < queuedCmdIndex){
      // 500msec は長いかも・・・
      GetQueuedCmdCurrentIndex(executedCmdIndex, 500);  
      Serial.print("\nexecutedCmdIndex=");
      sprintf(sbuf, "%ld", executedCmdIndex);
      Serial.print(sbuf);
    }   
    return true;
  }
  else
    return false;  
}
// **********************
// ArcCmd
// **********************
int ArcCmd(float cirx, float ciry, float cirz, float cirr, float tox, float toy, float toz, float tor)
{
  union data{
    byte buf[4];
    float value;
  }f;

  byte buf[38];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 32; // len= 2 + Payload Len;
  buf[3] = 101;     // id;
  buf[4] = 3;      // ctrl r/w = W, isQueued = True;
  f.value = cirx;
  for (int i= 0 ; i < 4; i++) buf[5 + i] = f.buf[i];
  f.value = ciry;
  for (int i= 0 ; i < 4; i++) buf[9 + i] = f.buf[i];
  f.value = cirz;
  for (int i= 0 ; i < 4; i++) buf[13 + i] = f.buf[i];
  f.value = cirr;
  for (int i= 0 ; i < 4; i++) buf[17 + i] = f.buf[i];

  f.value = tox;
  for (int i= 0 ; i < 4; i++) buf[21 + i] = f.buf[i];
  f.value = toy;
  for (int i= 0 ; i < 4; i++) buf[25 + i] = f.buf[i];
  f.value = toz;
  for (int i= 0 ; i < 4; i++) buf[29 + i] = f.buf[i];
  f.value = tor;
  for (int i= 0 ; i < 4; i++) buf[33 + i] = f.buf[i];
  
  for (int i = 3; i < 37; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[37] = checkSum;
  for (int i = 0; i < 38; i++) Serial1.write(buf[i]);
  delay(100);
  
  int len = 14;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 101){
    uint64_t queuedCmdIndex =
      buf[5+0] | buf[5+1] << 8 | buf[5+2] << 16 | buf[5+3] << 24 | buf[5+4] << 32 | buf[5+5] << 40 | buf[5+6] << 48 | buf[5+7] << 56;
    Serial.print("\nqueuedCmdIndex=");
    char sbuf[50];
    sprintf(sbuf, "%ld", queuedCmdIndex);
    Serial.print(sbuf);
    // 移動完了まで待つ         
    uint64_t executedCmdIndex = 0;
    while (executedCmdIndex < queuedCmdIndex){
      // 500msec は長いかも・・・
      GetQueuedCmdCurrentIndex(executedCmdIndex, 500);  
      Serial.print("\nexecutedCmdIndex=");
      sprintf(sbuf, "%ld", executedCmdIndex);
      Serial.print(sbuf);
    }   
    return true;
  }
  else
    return false;  
}
// **********************
// SetIOMultiplexing
// **********************
int SetIOMultiplexing(byte ioAddress, byte ioFunction)
{
  byte buf[8];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;
  buf[2] = 2 + 2; // len= 2 + Payload Len;
  buf[3] = 130;   // id;
  buf[4] = 1;     // ctrl r/w = W, isQueued = False;
  buf[5] = ioAddress;  // EIO Num(1..20)
  buf[6] = ioFunction; // IOFunction,
  for (int i = 3; i < 7; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[7] = checkSum;
  for (int i = 0; i < 8; i++) Serial1.write(buf[i]);
  delay(100); // 10 msec でも大丈夫
  
  int len = 8;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  return (index == len && buf[3] == 130); 
}

// **********************
// SetIODO
// **********************
int SetIODO(byte ioAddress, byte ioLevel)
{
  byte buf[8];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;  
  buf[2] = 2 + 2; // len= 2 + Payload Len;
  buf[3] = 131;   // id;
  buf[4] = 1;     // ctrl r/w = W, isQueued = False;
  buf[5] = ioAddress;  // EIO Num(1..20)
  buf[6] = ioLevel;    // Level output 0-Low level 1-High level
  for (int i = 3; i < 7; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[7] = checkSum;
  for (int i = 0; i < 8; i++) Serial1.write(buf[i]);
  delay(100); // 10 msec でも大丈夫

  int len = 8;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  return (index == len && buf[3] == 131);
}

// **********************
// GetIODO
// **********************
int GetIODO(byte ioAddress, byte &ioLevel)
{
  byte buf[8];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;  
  buf[2] = 2 + 2; // len= 2 + Payload Len;
  buf[3] = 131;   // id;
  buf[4] = 0;     // ctrl r/w = r, isQueued = False;
  buf[5] = ioAddress;
  buf[6] = 0;
  for (int i = 3; i < 7; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[7] = checkSum;
  for (int i = 0; i < 8; i++) Serial1.write(buf[i]);
  delay(100); // 10 msec でも大丈夫
  
  int len = 8;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 131 && buf[5] == ioAddress){
    ioLevel = buf[6];
    return true;
  }
  else
    return false;  
}

// **********************
// GetIODI
// **********************
int GetIODI(byte ioAddress, byte &ioValue)
{
  byte buf[8];
  byte checkSum = 0;
  buf[0] = 0xAA;
  buf[1] = 0xAA;  
  buf[2] = 2 + 2; // len= 2 + Payload Len;
  buf[3] = 133;   // id;
  buf[4] = 0;     // ctrl r/w = r, isQueued = False;
  buf[5] = ioAddress;
  buf[6] = 0;
  for (int i = 3; i < 7; i++) checkSum = checkSum + buf[i];
  checkSum = 0x100 - checkSum;
  buf[7] = checkSum;
  for (int i = 0; i < 8; i++) Serial1.write(buf[i]);
  delay(100); // 10 msec でも大丈夫

  int len = 8;
  int index = 0;
  while (index < len && Serial1.available() > 0) {
    buf[index] = Serial1.read();
    index++;
  }
  if (index == len && buf[3] == 133 && buf[5] == ioAddress){
    ioValue = buf[6];
    return true;
  }
  else
    return false;  
}

Pixy2 pixy;

const int pixyW = 316;    // Pixy2 水平解像度
const int pixyH = 208;    // Pixy2 垂直解像度

// プログラム開始位置
float startPosX = 200.0;
float startPosY = 0.0;
float startPosZ = 0.0;
float startPosR = 0.0;

// キャプチャ位置（カメラの中心線が Dobot の中心を通ること）
// Pixy2 の取付角度によってある程度制限される
float captureX = 220.0;
float captureY = -48.7;
float captureZ = 70.0;
// 吸引チューブの影ができるので、回転
float captureR = 90.0;

// プレース位置（７点）
// Arduino 版では、通過点の処理をしていないため
// Dobot の最小半径にかかる位置は指定しないほうが良い
float placingX[] = { 100.0,   50.0,    0.0,  100.0,   50.0,    0.0,  -50.0};
float placingY[] = {-200.0, -200.0, -200.0, -250.0, -250.0, -250.0, -250.0};
float placingZ[] = {   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0};

// 吸引時のZ座標
float suctionZ = -60.0;

// 移動中のZ値
float movingZ = 0.0;

// Doobot の床レベル（重要）
float dobotFL = -130.0;

// ピッキングの Z 値が Dobot フロアーレベルの時、エラーにならない半径
float radMin = 220.0;
float radMax = 320.0; 

// ピッキング範囲の制限 Y 値
float pickAreaYMin = -110.0;
float pickAreaYMax = 110.0;
 
// Z = 0 の時、エラーにならない最小半径
// Dobot PTPCmd は 2 点間を直線移動するため、この半径内を通る時は、中間点を追加する
float radLimMin = 140.0;

// カメラ位置
// アームを X 軸方向(Y=0)にした状態で、基準座標からの相対距離
float pixyOffX = 30.0;
float pixyOffY = 53.0;

// ピクセルからミリへの変換係数の初期値（キャプチャ時の Z により再計算される）
// キャプチャ高さが固定であれば、固定値で良い
float pixToMMScale = 0.7308;

// 外部スイッチ入力
const int PICKPLACE_PIN = 31;
const int HOMING_PIN = 33;
const int DOTEST_PIN = 35;
const int ARCTEST_PIN = 37;
const int LED_PIN = 41;
const int BUZZER_PIN = 11; // PWM OUT

// ******************************
// 座標変換
// ******************************
float degToRad(float deg){
  return (PI * deg) / 180.0;
}
float radToDeg(float rad){
  return (rad * 180.0) / PI;
}
float getAngle(float x, float y){
  float ang = 0.0;
  if (x != 0.0){
    ang = abs(atan(y / x));
    if ((x < 0.0) && (y >= 0.0)) ang = PI - ang;
    else if ((x >= 0.0) && (y < 0.0)) ang = PI * 2.0 - ang;
    else if ((x < 0.0) && (y < 0.0)) ang = ang + PI;
  }
  return ang;  
}
float getDistance(float x, float y){
  return sqrt(sq(x) + sq(y));  
}
void getPolar(float x, float y, float rad, float dist, float &resX, float &resY){
  resX = dist * cos(rad) + x;
  resY = dist * sin(rad) + y;
}

// Z軸廻りに左回転
void rotateZ(float x, float y, float rad, float &resX, float &resY){
  resX = x * cos(rad) + y * sin(rad);
  resY = x * -sin(rad) + y * cos(rad);
}
void rotatePoint(float genX, float genY, float x, float y, float rad, float &resX, float &resY){
  rotateZ(x - genX, y - genY, rad, resX, resY);
  resX = resX + genX;
  resY = resY + genY;
}
// 垂線
void getPerPoint(float ptX, float ptY, float x1, float y1, float x2, float y2, float &resX, float &resY){
  float faz = 0.00000001;
  if (abs(x2 - x1) <= faz){
    resX = x1;
    resY = ptY;  
  }
  else if (abs(y2 - y1) <= faz){
    resX = ptX;
    resY = y1;  
  }
  else {
    float ang = atan((y2-y1) / (x2-x1));
    float xx, yy;
    rotatePoint(x1, y1, ptX, ptY, ang, xx, yy);
    rotatePoint(x1, y1, xx, y1, -ang, resX, resY);
  }
}

// Pixy 座標をDobot座標に
void pixyToDobotXY(int pixX, int pixY, float poseX, float poseY, float poseZ, float &resX, float &resY){
  // Z = 0 の時のカメラ位置から Dobot の床レベルまでの距離を基準にする
  float baseZ = poseZ - dobotFL;
  // キャプチャ画像の座標を左下原点の座標に
  int pxx = pixX;
  int pxy = pixyH - pixY;
  // Dobot アームの回転角度
  float ang0 = getAngle(poseX, poseY);
  // Dobot のヘッド座標からのカメラの方向と距離
  float pxdist = getDistance(pixyOffX, pixyOffY);
  float pxang = getAngle(pixyOffX, pixyOffY);
  // カメラ位置
  float camx = pxdist * cos(pxang + ang0) + poseX;
  float camy = pxdist * sin(pxang + ang0) + poseY; 
  // キャプチャ高さが一定であれば、再計算の必要ない
  // Pixy画面一杯の横幅（これが基準）カメラの水平レンズ視野角度60°の 1 / 2
  float pcw = baseZ * tan(degToRad(30.0)) * 2.0;
  pixToMMScale = pcw / pixyW; //ピクセル基準
  // Pixy画面の中心を 0, 0 にした座標に（ピクセル単位）それをミリメートルに
  float cadx = (pxx - pixyW / 2.0) * pixToMMScale;
  float cady = (pxy - pixyH / 2.0) * pixToMMScale;
  // 中心からの角度と距離（ミリメートル単位）
  float dist = getDistance(cadx, cady);
  float rad = getAngle(cadx, cady);
  // カメラの座標から同じ距離、角度の座標を取得（角度はカメラ角度分を足す）
  resX = dist * cos(rad - PI / 2.0) + camx;
  resY = dist * sin(rad - PI / 2.0) + camy;
  //dobotPartX = dist * cos(rad - PI / 2.0) + camx;
  //dobotPartY = dist * sin(rad - PI / 2.0) + camy;
}

// **********************
// setup
// **********************
boolean homingFlag = false;
boolean pickingFlag   = false;
boolean dotestFlag = false;
boolean arctestFlag = false;

void setup() {
  // GPIO 
  pinMode(PICKPLACE_PIN, INPUT_PULLUP);
  pinMode(HOMING_PIN, INPUT_PULLUP);
  pinMode(DOTEST_PIN, INPUT_PULLUP);
  pinMode(ARCTEST_PIN, INPUT_PULLUP);
  
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT); // PWM buzzer
    
  // シリアルモニタ
  Serial.begin(115200);
  // Dobot UART
  Serial1.begin(115200);

  // Pixy2 初期化
  Serial.print("\npixyInit ");
  pixy.init(); 
  pixy.setLamp(0, 0);
  
  //SetIOMultiplexing(18, IOFunctionDO);
  //SetIOMultiplexing(20, IOFunctionDI);  
  
  ClearAllAlarmsState();
  digitalWrite(LED_PIN, HIGH);
  PTPCmd(startPosX, startPosY, startPosZ, startPosR);
  digitalWrite(LED_PIN, LOW);
  
  homingFlag = false;
  pickingFlag   = false;
  dotestFlag = false;
}

void loop() {
  // ピック＆プレース
  if (!pickingFlag  && !digitalRead(PICKPLACE_PIN)){
    pickingFlag = true;
    delay(500);
    Serial.print("\nPICK&PLACE ... ");
    // キャプチャ位置へ移動
    //digitalWrite(LED_PIN, HIGH);
    PTPCmd(captureX, captureY, captureZ, captureR);
    //digitalWrite(LED_PIN, LOW);
    delay(50);
    // Pixy2 の照明を ON
    pixy.setLamp(1, 1);
    delay(400);
    // キャプチャ中 ... のLED
    digitalWrite(LED_PIN, HIGH);
    delay(50);
    // Pixy2 でブロックを検出
    static int activeSignature;
    pixy.ccc.getBlocks();
    Serial.print("\nBlocks = "); Serial.print(pixy.ccc.numBlocks);
    delay(100);
    // キャプチャ中 ... の LED を消す
    digitalWrite(LED_PIN, LOW);
    if (pixy.ccc.numBlocks > 0){
      float dobotPartX = 0.0;
      float dobotPartY = 0.0;
     
      int loopFlag = true;
      int index = 0;
      int activeIndex = -1;
      while (loopFlag && index < pixy.ccc.numBlocks){
        pixy.ccc.blocks[index].print();
        // 毎回、最初に見つかった 1 個を採用
        activeSignature = pixy.ccc.blocks[0].m_signature;
        // Pixy2 画像を Dobot 座標に変換
        pixyToDobotXY(
          pixy.ccc.blocks[index].m_x, pixy.ccc.blocks[index].m_y, captureX, captureY, captureZ, dobotPartX, dobotPartY);
        float dist = getDistance(dobotPartX, dobotPartY);
        Serial.print("\ncadDist= "); Serial.print(dist);
        Serial.print("\npartX= "); Serial.print(dobotPartX);
        Serial.print("\npartY= "); Serial.print(dobotPartY);
        Serial.print("\npartAge= "); Serial.print(pixy.ccc.blocks[index].m_age);
        
        
        // ピッキング範囲と Pixy2 検出の Age によって制限(ちらちらしているものは、Age が若い、安定しているものは　Age =255）
        if (dist >= radMin && dist <= radMax && dobotPartY >= pickAreaYMin && dobotPartY <= pickAreaYMax && pixy.ccc.blocks[index].m_age >= 100 && activeSignature < 7){
          activeIndex = index;
          loopFlag = false;
          Serial.print("\ndobotPartX="); Serial.print(dobotPartX);
          Serial.print("\ndobotPartY="); Serial.print(dobotPartY);
        }
        index++;
      }
      // 制限範囲外、またはちらつき      
      if (activeIndex == -1){
        // ブザーを鳴らす
        tone(BUZZER_PIN, 800, 250) ; // 800Hz, 250msec
        //analogWrite(BUZER_PIN, 128);
        //delay(500);
        //analogWrite(BUZZER_PIN, 0);   
      }
      else {
        // Pixy2 画像を Dobot 座標に変換
        delay(100);
        // Pixy2 の照明を消す
        pixy.setLamp(0, 0);
        digitalWrite(LED_PIN, HIGH);
        // 検出位置に移動
        PTPCmd(dobotPartX, dobotPartY, movingZ, 0.0);
        // 下げる
        PTPCmd(dobotPartX, dobotPartY, suctionZ, 0.0);
        // 吸引
        SetEndEffectorSuctionCup(1,1);
        // 上げる
        PTPCmd(dobotPartX, dobotPartY, movingZ, 0.0);
        // 最小半径にかかる時は、中間点を追加
        float resX = 0.0;
        float resY = 0.0;
        getPerPoint(0, 0, dobotPartX, dobotPartY, placingX[activeSignature], placingY[activeSignature], resX, resY);
        float dist = getDistance(resX, resY);
        Serial.print("\ndist = "); Serial.print(dist);
        float ang = getAngle(resX, resY);
        if (dist < radLimMin) {
          tone(BUZZER_PIN, 1500, 1000) ;
          Serial.print(" MidPoint ... ");
          getPolar(0, 0, ang, radLimMin, resX, resY);
          // 中間点を通る折れ線移動でプレース位置へ
          //PTPCmd(resX, resY, placingZ[activeSignature], 0.0);
          //PTPCmd(placingX[activeSignature], placingY[activeSignature], placingZ[activeSignature], 0.0);
          // 中間点を通る円弧移動でプレース位置へ
          ArcCmd(resX, resY, placingZ[activeSignature], 0.0, placingX[activeSignature], placingY[activeSignature], placingZ[activeSignature], 0.0); 
        }
        else{
          // プレース位置へ移動
          PTPCmd(placingX[activeSignature], placingY[activeSignature], placingZ[activeSignature], 0.0);
        }
        // 離す
        SetEndEffectorSuctionCup(1, 0);
        // Z 座標 0 でキャプチャ位置に戻す
        PTPCmd(captureX, captureY, movingZ, 0.0);
        digitalWrite(LED_PIN, LOW);
      }
    }
    else{
      // 見つからない（ブザーは鳴らさない）
      delay(490);
    }
    pickingFlag = false;
  }
  // ホーミング
  else if (!homingFlag && !digitalRead(HOMING_PIN)){
    homingFlag = true;
    delay(500); 
    Serial.print("\nHOMING ... ");
    // ホーミング中 ...
    digitalWrite(LED_PIN, HIGH);
    //SetIOMultiplexing(18, IOFunctionDO);
    //SetIODO(18, 1);  
    HomeCmd();
    digitalWrite(LED_PIN, LOW);
    //SetIODO(18, 0);
    homingFlag = false;
  }
  else if (!dotestFlag && digitalRead(DOTEST_PIN) == LOW){
    dotestFlag = true;
    delay(500);
    Serial.print("\nDOTEST ... ");
    digitalWrite(LED_PIN, HIGH);
    //SetIOMultiplexing(18, IOFunctionDO);
    //byte ioLevel;
    //GetIODO(18, ioLevel);
    //SetIODO(18, 1);//!ioLevel);
    dotestFlag = false;
  }
  // ArcCmd のテスト
  else if (!arctestFlag && !digitalRead(ARCTEST_PIN)){
    arctestFlag = true;
    delay(500);
    Serial.print("\nARCTEST ... ");
    digitalWrite(LED_PIN, HIGH);
    PTPCmd(200.0, 150.0, 0.0, 0.0);
    ArcCmd(250.0, 0.0, 0.0, 0.0, 200.0, -150.0, 0.0, 0.0);
    digitalWrite(LED_PIN, LOW);        
    arctestFlag = false;        
  }
  delay(10);
}