Project help - Object following/tracking robot

[yoitsK]

I'm trying to compile the code for my robot which follows/tracks a object. I'm using the Motor Drive Shield V2 and the Pixy camera. I discovered a website, https://learn.adafruit.com/pixy-pet-rob ... m/the-code. The Pixy Pet Robot has a Pixy Camera and it is connected to the servo motor. Also, I'm not using the ZumoMotors and the servo motor. I would like to try the code provided on the website. But how can I exclude the Pan and Tilt functions (Servo Motor) and still have the identical functions from the code which creates the robot to track/follow the object?

Code: Select all

#include <SPI.h>  
#include <Pixy.h>
#include <ZumoMotors.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)
//---------------------------------------
// Servo Loop Class
// A Proportional/Derivative feedback
// loop for pan/tilt servo tracking of
// blocks.
// (Based on Pixy CMUcam5 example code)
//---------------------------------------
class ServoLoop
{
public:
	ServoLoop(int32_t proportionalGain, int32_t derivativeGain);
	void update(int32_t error);
	int32_t m_pos;
	int32_t m_prevError;
	int32_t m_proportionalGain;
	int32_t m_derivativeGain;
};
// ServoLoop Constructor
ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)
{
	m_pos = RCS_CENTER_POS;
	m_proportionalGain = proportionalGain;
	m_derivativeGain = derivativeGain;
	m_prevError = 0x80000000L;
}
// ServoLoop Update 
// Calculates new output based on the measured
// error and the current state.
void ServoLoop::update(int32_t error)
{
	long int velocity;
	char buf[32];
	if (m_prevError!=0x80000000)
	{	
		velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
		m_pos += velocity;
		if (m_pos>RCS_MAX_POS) 
		{
			m_pos = RCS_MAX_POS; 
		}
		else if (m_pos<RCS_MIN_POS) 
		{
			m_pos = RCS_MIN_POS;
		}
	}
	m_prevError = error;
}
// End Servo Loop Class
//---------------------------------------
Pixy pixy;  // Declare the camera object
ServoLoop panLoop(200, 200);  // Servo loop for pan
ServoLoop tiltLoop(150, 200); // Servo loop for tilt
ZumoMotors motors;  // declare the motors on the zumo
//---------------------------------------
// Setup - runs once at startup
//---------------------------------------
void setup()
{
	Serial.begin(9600);
	Serial.print("Starting...\n");
	pixy.init();
}
uint32_t lastBlockTime = 0;
//---------------------------------------
// 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);
	}
}
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 >> 3;
	// 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);
} 

[Franklin97355]

What mechanism will you be using to move the camera up/down and left/fight?

[yoitsK]

My plan is mount the Pixy camera on the robot without any additional components, or motions. I'm interested in the code provided from the website but I would like to know if there's a method to exclude the Pan and Tilt functions on the code, but still be able to create the robot to follow/track the object.

[Franklin97355]

There is not a method other than rewriting the code to work without the ability to move the camera and that would be a large undertaking. It might be better to start from scratch and build up from there.

[yoitsK]

That's why I posted a forum on here and ask for help.

[Franklin97355]

I'll move this over to General Project help and you can see if there are any takers.

[yoitsK]

Thank you!

I'll move this over to General Project help and you can see if there are any takers.

[adafruit_support_bill]

Have a look at this thread. There is another forum member attempting the same thing. viewtopic.php?f=25&t=96880&p=640350

[yoitsK]

I compiled new code for my object following robot. The issue on this new code is that I cannot make my robot to stop all motions when the camera does not detect the object. What am I missing in the code and how can I edit the code to make the robot stop motions?

Code: Select all

#include <PixyI2C.h>
#include <Pixy.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *leftMotor = AFMS.getMotor(2);
Adafruit_DCMotor *rightMotor = AFMS.getMotor(3);
Pixy pixy;

int signature = 0;
int x = 0;
int y = 0;
unsigned int width = 0;
unsigned int height = 0;
unsigned int area = 0;
unsigned int newarea = 0;
int Xmin = 70;
int Xmax = 200;
int maxArea = 0;
int minArea = 0;
static int i = 0;

void setup()
{
  Serial.begin(9600);
  AFMS.begin();
  pixy.init();
leftMotor->setSpeed(0);
rightMotor->setSpeed(0);
}

void loop()
{ 
  while(millis()<5000)
  {
    scan();
    area = width * height;  //calculate the object location
    maxArea = area + 1000;
    minArea = area - 1000;
  }
    scan(); 
  if(signature == 1)
  {
    newarea = width * height;
      if (x < Xmin)//turn left if x position < max x position
      {     
       left();
      }
      else if (x > Xmax) //turn right if x position > max x position
      {
       right();
      }
      else if(newarea < minArea)//go forward if object too small
      {
       forward(); 
      }
      else if(newarea > maxArea)//go backward if object too big
      {
       backward(); 
      }
      else
      {
        Stop(); 
      } 
   else
   {
    Stop();
   }
   }
}

void backward()
  {
 leftMotor->setSpeed(180);
 leftMotor->run(FORWARD);
 rightMotor->setSpeed(180);
 rightMotor->run(BACKWARD);
  }

void Stop()
  {
// leftMotor->setSpeed(0);
 leftMotor->run(RELEASE);
// rightMotor->setSpeed(0);
 rightMotor->run(RELEASE);
  }

void forward()
  {
 leftMotor->setSpeed(255);
 leftMotor->run(BACKWARD);
 rightMotor->setSpeed(255);
 rightMotor->run(FORWARD);
  }

void right()
  {
 leftMotor->setSpeed(255);
 leftMotor->run(BACKWARD);
 rightMotor->setSpeed(180);
 rightMotor->run(FORWARD);
  }

void left()
  {
 leftMotor->setSpeed(180);
 leftMotor->run(BACKWARD);
 rightMotor->setSpeed(255);
 rightMotor->run(FORWARD);
  }
  
void scan()
  {
  uint16_t blocks;
  blocks = pixy.getBlocks();               //receive data from camera 
  signature = pixy.blocks[i].signature;    
  x = pixy.blocks[i].x;                    //determine x position
  y = pixy.blocks[i].y;                    //determine y position
  width = pixy.blocks[i].width;            //determine width
  height = pixy.blocks[i].height;          //determine height
  }  

[adafruit_support_bill]

When you do the scan, check for blocks == 0. If there are no blocks detected, then stop your robot.

[yoitsK]

I still having issue on how to make the robot stop when it does not detect object. I couldn't figure it out how to compile that specific code.

[adafruit_support_bill]

Fix your scan() function to return a true only if there are blocks detected. Otherwise return false.

Then check the value returned by scan() before moving your robot.

Code: Select all

bool scan()
{
    uint16_t blocks;
    blocks = pixy.getBlocks();               //receive data from camera 
    if (blocks == 0)
    {
        return false;  // no blocks detected
    }
    signature = pixy.blocks[0].signature;    
    x = pixy.blocks[0].x;                    //determine x position
    y = pixy.blocks[0].y;                    //determine y position
    width = pixy.blocks[0].width;            //determine width
    height = pixy.blocks[0].height;          //determine height
    return true;
} 

[yoitsK]

It remains the same. The robot continue to move.

[adafruit_support_bill]

Please post your code.