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.













Tips to save money on airfare

1. Kayak

Use the app Kayak to create a price alert. This will tell you when the price of the flight tickets goes down.

2. Time frame to buy the flight tickets

 Buy the the flight tickets between one month and two weeks prior to your desire date of travel. This works because if the airplane it's empty the airlines will lower the cost of the tickets.

3. Check if your desired flight it's full o empty

I use Orbitz for this. You only have to select your desired flight on Orbitz and it will appear the option to select seats with this feature you can see how the sale of the flight it's going.

4. Buy the flight tickets at night

The best time to buy the flight tickets is between 6 pm to 7 am.

5. The best days to buy the flight tickets are Tuesday, Wednesday, Friday and Saturday.