File content as of revision 2:9caf24407d32:

#include "mbed.h"
#include <stdlib.h>
#include "Pixy.h"
#include "MD25.h"
#include "Servoloop.h"
#define min(x, y) (((x) < (y)) ? (x) : (y))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

Pixy pixy(Pixy::SPI, D11, D12, D13);
Serial pc(USBTX, USBRX);
Timer t;
Servoloop panLoop(200, 200); // Servo loop for pan
Servoloop tiltLoop(150, 200); // Servo loop for tilt
MD25 motors(I2C_SDA, I2C_SCL); // declare the motors

int random(int numberone, int numbertwo);
 
 
int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;    
int oldX, oldY, oldSignature;

void init_motors(void){
motors.setMode(0); // MODE 0, 0=marche arriere 128=stop 255=marche arriere vmax
motors.setCommand(32); // 0x20 reset encoders
motors.setCommand(50); // 0X32 Disable time out
//motors.setCommand(32); // 0x20 reset encoders
//motors.setCommand(48); // 0X32 Disable speed regulation
 }
 void setSpeed (int vitMotor1, int vitMotor2){
     motors.setSpeedRegisters(vitMotor1,vitMotor2); 
     }
    

//---------------------------------------
// 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;

pc.printf("blocks =");
pc.printf("%d",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 = 200;
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(200 - (size/256), -100, 200);
// 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, -200, 200);
int rightSpeed = constrain(forwardSpeed - differential, -200, 200);
// And set the motor speeds
motors.setSpeedRegisters(leftSpeed,rightSpeed);
}
//---------------------------------------
// Random search for blocks
//
// This code pans back and forth at random
// until a block is detected
//---------------------------------------

void ScanForBlocks(){
if (t.read_ms() - lastMove > 20){
lastMove = t.read_ms();
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){
setSpeed(0,255);}
else{
setSpeed(5,250);}
wait(random(250, 500));
}
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
}
}



int main (void)
{
   
uint16_t blocks;  
  
t.start();
    pc.format(8,Serial::None,1); 
    pc.baud(9600); 
    pc.printf("started.. \n");
uint32_t lastBlockTime = 0;

while(1){

blocks = pixy.getBlocks();
// If we have blocks in sight, track and follow them
if (blocks){
int trackedBlock = TrackBlock(blocks);
FollowBlock(trackedBlock);
lastBlockTime = t.read();}

else if (t.read() - lastBlockTime > 100){
setSpeed(128,128);
ScanForBlocks();}
}
    }
    
    
    int random(int numberone, int numbertwo) {
    int random = 0;
    if ((numberone < 0) && (numbertwo < 0)) {
        numberone = numberone * -1;
        numbertwo = numbertwo * -1;
        random = -1 * (rand()%(numberone + numbertwo));
    }
    if ((numbertwo < 0) && (numberone >= 0)) {
        numbertwo = numbertwo * -1;
        random = (rand()%(numberone + numbertwo)) - numbertwo;
    }
    if ((numberone < 0) && (numbertwo >= 0)) {
        numberone = numberone * -1;
        random = (rand()%(numberone + numbertwo)) - numberone;
    } else {
        random = (rand()%(numberone + numbertwo)) - min(numberone, numbertwo);
    }
    return random;
    }