Majordhome/pixy
Dependencies: MD25 Servoloop mbed pixy
Fork of pixyHelloWorld by
Diff: main.cpp
- Revision:
- 1:cf1bfec4aae0
- Parent:
- 0:63532ae95efe
- Child:
- 2:9caf24407d32
diff -r 63532ae95efe -r cf1bfec4aae0 main.cpp --- a/main.cpp Sun Nov 16 11:54:19 2014 +0000 +++ b/main.cpp Mon Apr 13 12:17:40 2015 +0000 @@ -1,28 +1,161 @@ #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 main() { - pixy.setSerialOutput(&pc); - while (1) { - static int i = 0; - int j; - uint16_t blocks; - - blocks = pixy.getBlocks(); - - if (blocks) { - i++; +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 = 400; +void FollowBlock(int trackedBlock) +{ - if (i % 50 == 0) { - pc.printf("Detected %d:\n\r", blocks); - for (j = 0; j < blocks; j++) { - pc.printf(" block %d: ", j); - pixy.blocks[j].print(pc); - } - } - } - } -} \ No newline at end of file +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.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(-250,250);} +else{ +setSpeed(0,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; + } \ No newline at end of file