โปรเจครถติดตามวัตถุ (Car Tracking) พัฒนาโดยโมดูลกล้อง Pixy2 CMUcam5 และบอร์ด Arduino Mega 2560 ตอนที่ 2

#include <Pixy2.h>
#include <PIDLoop.h>
#include <ZumoMotors.h>

#define MAX_TRANSLATE_VELOCITY  250    // this limits how fast Zumo travels forward (400 is max possible for Zumo)
#define Close_block_size 190   // too Close block size
#define Far_block_size 160     // too Far block size
#define straight_grap 150     // Straight Grap Value for go straight

Pixy2 pixy;
ZumoMotors motors;

PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");

  // initialize motor objects
  motors.setLeftSpeed(0);
  motors.setRightSpeed(0);

  // need to initialize pixy object
  pixy.init();
  // user color connected components program
  pixy.changeProg("color_connected_components");
}

int16_t acquireBlock()
{
  if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age > 30)
    return pixy.ccc.blocks[0].m_index;

  return -1;
}

int Width;
Block *trackBlock(uint8_t index)
{
  uint8_t i;

  for (i = 0; i < pixy.ccc.numBlocks; i++)
  {
    Serial.print("Width: ");
    Serial.print(pixy.ccc.blocks[i].m_width);
    Width = pixy.ccc.blocks[i].m_width;
    if (index == pixy.ccc.blocks[i].m_index)
      return &pixy.ccc.blocks[i];
  }

  return NULL;
}

void loop()
{
  static int16_t index = -1;
  int32_t panOffset, tiltOffset, headingOffset, left, right;
  Block *block = NULL;

  pixy.ccc.getBlocks();

  if (index == -1) // search....
  {
    Serial.println("Searching for block...");
    index = acquireBlock();
    if (index >= 0)
      Serial.println("Found block!");
  }
  // If we've found a block, find it, track it
  if (index >= 0)
    block = trackBlock(index);

  // If we're able to track it, move motors
  if (block)
  {
    // calculate pan and tilt errors
    panOffset = (int32_t)pixy.frameWidth / 2 - (int32_t)block->m_x;
    tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight / 2;

    // calculate how to move pan and tilt servos
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);

    // move servos
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

    // calculate translate and rotate errors
    panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
    tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS / 2 + PIXY_RCS_CENTER_POS / 8;

    rotateLoop.update(panOffset);
    translateLoop.update(-tiltOffset);

    // keep translation velocity below maximum
    if (translateLoop.m_command > MAX_TRANSLATE_VELOCITY)
      translateLoop.m_command = MAX_TRANSLATE_VELOCITY; // for calculate Vertical angle

    // set wheel velocities

    if (Width < Close_block_size && Width > Far_block_size ) {
      motors.setLeftSpeed(0);
      motors.setRightSpeed(0);
      rotateLoop.reset();
      translateLoop.reset();
    }
    else if (Width >= Close_block_size  ) { // if Width value more than 90 too close
      if (rotateLoop.m_command >= -straight_grap && rotateLoop.m_command <= straight_grap) {
        motors.setLeftSpeed(-MAX_TRANSLATE_VELOCITY);
        motors.setRightSpeed(-MAX_TRANSLATE_VELOCITY);
      }
      else if (rotateLoop.m_command <= -straight_grap ) {
        motors.setLeftSpeed(0);
        motors.setRightSpeed(-MAX_TRANSLATE_VELOCITY);
      }
      else if (rotateLoop.m_command >= straight_grap ) {
        motors.setLeftSpeed(-MAX_TRANSLATE_VELOCITY);
        motors.setRightSpeed(0);
      }

    }

    else if (Width <= Far_block_size) { // if Width value less than 60 too far
      if (rotateLoop.m_command >= -straight_grap && rotateLoop.m_command <= straight_grap) { 
        motors.setLeftSpeed(MAX_TRANSLATE_VELOCITY);
        motors.setRightSpeed(MAX_TRANSLATE_VELOCITY);
      }

      else if (rotateLoop.m_command <= -straight_grap ) {
        motors.setLeftSpeed(MAX_TRANSLATE_VELOCITY);
        motors.setRightSpeed(0);
      }
      else if (rotateLoop.m_command >= straight_grap ) {
        motors.setLeftSpeed(0);
        motors.setRightSpeed(MAX_TRANSLATE_VELOCITY);
      }
    }

    Serial.print("   rotateLoop : ");
    Serial.print(rotateLoop.m_command); // Horizontal value
    Serial.print("    translateLoop : ");
    Serial.println(translateLoop.m_command); // Vertical value

  }
  else // no object detected, stop motors, go into search state
  {
    rotateLoop.reset();
    translateLoop.reset();
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    index = -1; // set search state
  }
}