Hi Chaps
Ok So im using code from adafruit The Code | Pixy Pet Robot - Color vision follower | Adafruit Learning System with a Pixy cam, I added my motor control etc and everything works great but I would like to add a Ultrasonic sensor to stop the robot as the pixy cam is not that accurate.
I got it working with the code below but now my motors are somewhat jerky, is there a better way to implement the ultrasonic so that theres no timing issues ?
Thanks
#include <DualVNH5019MotorShield.h>
DualVNH5019MotorShield md;
#include <SPI.h>
#include <Pixy.h>
#include <NewPing.h>
#define TRIGGER_PIN 15 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 14 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
//#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
//---------------------------------------
// Setup - runs once at startup
//---------------------------------------
void setup()
{
Serial.begin(115200);
//Serial.begin(9600);
//Serial.print("Starting...\n");
pixy.init();
md.init();
}
uint32_t lastBlockTime = 0;
//---------------------------------------
// Main loop - runs continuously after setup
//---------------------------------------
void loop()
{
//delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
//Serial.print("Ping: ");
//Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
//Serial.println("cm");
uint16_t blocks;
blocks = pixy.getBlocks();
if (uS > 300 && uS < 1000)
{
md.setM1Speed(0);
md.setM2Speed(0);
}
// If we have blocks in sight, track and follow them
else if (blocks)
{
int trackedBlock = TrackBlock(blocks);
FollowBlock(trackedBlock);
lastBlockTime = millis();
}
else if (millis() - lastBlockTime > 65)
{
md.setM1Speed(0);
md.setM2Speed(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 >> 5;//was 3// 6 work best the last try
// Forward speed decreases as we approach the object (size is larger)
int forwardSpeed = constrain(400 - (size/256), -100, 400);//256
// Steering differential is proportional to the error times the forward speed
int32_t differential = (followError + (followError * forwardSpeed))>>8;//was 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
md.setM1Speed(rightSpeed + 100);
md.setM2Speed(leftSpeed + 100);
}
//---------------------------------------
// Random search for blocks
//
// This code pans back and forth at random
// until a block is detected
//---------------------------------------
int scanIncrement = 20;
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)
{
md.setM1Speed(-250);
md.setM2Speed(250);
}
else
{
md.setM1Speed(+180);
md.setM2Speed(-180);
}
delay(random(250, 500));
}
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
}
}