Skip to main content

CMUcam5 Pixy object tracking code

The code:

//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#include <SPI.h>  
#include <Pixy.h>

#define X_CENTER    160L
#define Y_CENTER    100L
#define RCS_MIN_POS     0L
#define RCS_MAX_POS     1000L
#define RCS_CENTER_POS  ((RCS_MAX_POS-RCS_MIN_POS)/2)

class ServoLoop
{
public:
  ServoLoop(int32_t pgain, int32_t dgain);

  void update(int32_t error);
   
  int32_t m_pos;
  int32_t m_prevError;
  int32_t m_pgain;
  int32_t m_dgain;
};


ServoLoop panLoop(200, 200);  // Servo loop for pan
ServoLoop tiltLoop(150, 200); // Servo loop for tilt


ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
  m_pos = RCS_CENTER_POS;
  m_pgain = pgain;
  m_dgain = dgain;
  m_prevError = 0x80000000L;
}

void ServoLoop::update(int32_t error)
{
  long int vel;
  char buf[32];
  if (m_prevError!=0x80000000)
  {
    vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
    //sprintf(buf, "%ld\n", vel);
   // Serial.print(buf);
    m_pos += vel;
    if (m_pos>RCS_MAX_POS)
      m_pos = RCS_MAX_POS;
    else if (m_pos<RCS_MIN_POS)
      m_pos = RCS_MIN_POS;
    //cprintf("%d %d %d\n", m_axis, m_pos, vel);
  }
  m_prevError = error;
}

class DCMOTORS{
  private:
    int leftSpeed;
    int rigthSpeed;
    int leftDir;
    int rightDir;
  public:
    DCMOTORS(){
      leftSpeed = 9;
      rigthSpeed = 10;
      leftDir = 4;
      rightDir = 7;
      pinMode(leftDir, OUTPUT);
      pinMode(rightDir, OUTPUT);
      pinMode(leftSpeed, OUTPUT);
      pinMode(rigthSpeed, OUTPUT);
      //Enable the Motor Shield output;  
 pinMode(6, OUTPUT);
 digitalWrite(6, HIGH);
    }//end DCMOTORS
    int setLeftSpeed(int speed){
      if(speed > 0){//forward
          digitalWrite(leftDir, HIGH);
          analogWrite(leftSpeed, speed);
        }else{//backward
          digitalWrite(leftDir, LOW);
          analogWrite(leftSpeed, speed);
        }
    }//end set
    int setRightSpeed(int speed){
       if(speed > 0){
          digitalWrite(rightDir, HIGH);
          analogWrite(rigthSpeed, speed);
        }else{
          digitalWrite(rightDir, LOW);
          analogWrite(rigthSpeed, speed);
        }
    }//end set

    

};


Pixy pixy;

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

  pixy.init();
}

uint32_t lastBlockTime = 0;

DCMOTORS motors;

//---------------------------------------
// Main loop - runs continuously after setup
//---------------------------------------
void loop()
{
  uint16_t blocks;
  blocks = pixy.getBlocks();

  // If we have blocks in sight, track and follow them
  if (blocks)
  {
    int trackedBlock = TrackBlock(blocks);
    FollowBlock(trackedBlock);
    lastBlockTime = millis();
  }  
  else if (millis() - lastBlockTime > 100)
  {
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    ScanForBlocks();
  }
}

int oldX, oldY, oldSignature;

//---------------------------------------
// Track blocks via the Pixy pan/tilt mech
// (based in part on Pixy CMUcam5 pantilt example)
//---------------------------------------
int TrackBlock(int blockCount)
{
  int trackedBlock = 0;
  long maxSize = 0;

 // Serial.print("blocks =");
  //Serial.println(blockCount);

  for (int i = 0; i < blockCount; i++)
  {
    if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature))
    {
      long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
      if (newSize > maxSize)
      {
        trackedBlock = i;
        maxSize = newSize;
      }
    }
  }

  int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
  int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;

  panLoop.update(panError);
  tiltLoop.update(tiltError);

  pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

  oldX = pixy.blocks[trackedBlock].x;
  oldY = pixy.blocks[trackedBlock].y;
  oldSignature = pixy.blocks[trackedBlock].signature;
  return trackedBlock;
}

//---------------------------------------
// Follow blocks via the Zumo robot drive
//
// This code makes the robot base turn
// and move to follow the pan/tilt tracking
// of the head.
//---------------------------------------
int32_t size = 400;
void FollowBlock(int trackedBlock)
{
  int32_t followError = RCS_CENTER_POS - panLoop.m_pos;  // How far off-center are we looking now?

  // Size is the area of the object.
  // We keep a running average of the last 8.
  size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
  size -= size >> 6;

  // Forward speed decreases as we approach the object (size is larger)
  int forwardSpeed = constrain(400 - (size/256), -100, 400);  

  // Steering differential is proportional to the error times the forward speed
  int32_t differential = (followError + (followError * forwardSpeed))>>8;

  // Adjust the left and right speeds by the steering differential.
  int leftSpeed = constrain(forwardSpeed + differential, -400, 400);
  int rightSpeed = constrain(forwardSpeed - differential, -400, 400);

  // And set the motor speeds
  motors.setLeftSpeed(leftSpeed);
  motors.setRightSpeed(rightSpeed);
}

//---------------------------------------
// Random search for blocks
//
// This code pans back and forth at random
// until a block is detected
//---------------------------------------
int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;

void ScanForBlocks()
{
  if (millis() - lastMove > 20)
  {
    lastMove = millis();
    panLoop.m_pos += scanIncrement;
    if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS))
    {
      tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
      scanIncrement = -scanIncrement;
      if (scanIncrement < 0)
      {
        motors.setLeftSpeed(-175);
        motors.setRightSpeed(175);
      }
      else
      {
        motors.setLeftSpeed(+90);
        motors.setRightSpeed(-90);
      }
      delay(random(175, 250));
    }

    pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
  }
}

Comments

Popular posts from this blog

Cave of Wonders ( Cueva de las Maravillas) Dominican Republic

The Cave of Wonders is located on the road between San Pedro de Macoris and La Romana. It has paintings and engravings on the walls made by the Tainos Indian. The tour is given for about 300 meters inside the cave and 25 meters underground. The tour ends visiting a sanctuary of iguanas. The price of the ticket is 300 Dominican pesos or about $6.50 dollars. I recommend you to visit the cave if you are near by because it has beautiful paintings and engravings that are part of the history of the country and you can only see them in person because they don't allow you to take pictures of the paintings and engravings.













TOP Lockscreen rewards

1. SurveyCow

This app is the best to earn money while using your cellphone. You can earn 200 points per day that equals to 20 cents per day for answering questions that appears on your lock screen. Also there are bonus surveys that can add 150 points. To withdraw the money you need 10,000 Beef points that will pay you 10 dollars directly to your Paypal account.

2. S'more- Lockscreen Rewards

S'more is a very good app to earn money. It will pay you 10 cents per day for keeping the app. Everyday will appear some surveys that could pay you up to 32 cents. This app doesn't have Paypal as a method to withdraw money but has Amazon and a lot of others gift cards. You can withdraw the money every time you hit 100 points.

3. Slidejoy

With this app you can earn up to 3 dollars per month for unlocking your phone but you can only redeem it on the 18 of each month. It has Amazon or Paypal as a method for cashout.

4. Adme

This app pays for unlocking your phone but it's slow. It will tak…