//---------------------------------------
// 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);
}
//---------------------------------------
// 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(-250);
motors.setRightSpeed(250);
}
else
{
motors.setLeftSpeed(+180);