Majordhome/pixy
Dependencies: MD25 Servoloop mbed pixy
Fork of pixyHelloWorld by
main.cpp
- Committer:
- johnylafleur
- Date:
- 2015-04-13
- Revision:
- 1:cf1bfec4aae0
- Parent:
- 0:63532ae95efe
- Child:
- 2:9caf24407d32
File content as of revision 1:cf1bfec4aae0:
#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 = 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.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; }