Jonathan Goret / pixyMajordhome

Majordhome/pixy

Dependencies:   MD25 Servoloop mbed pixy

Fork of pixyHelloWorld by Arcadie Cracan

main.cpp@2:9caf24407d32, 2015-04-14 (annotated)

Committer:
johnylafleur
Date:
Tue Apr 14 11:00:56 2015 +0000
Revision:
2:9caf24407d32
Parent:
1:cf1bfec4aae0 
Pixy+ MD25 Motors Tracking;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:63532ae95efe 1 #include "mbed.h"
johnylafleur 1:cf1bfec4aae0 2 #include <stdlib.h>
acracan 0:63532ae95efe 3 #include "Pixy.h"
johnylafleur 1:cf1bfec4aae0 4 #include "MD25.h"
johnylafleur 1:cf1bfec4aae0 5 #include "Servoloop.h"
johnylafleur 1:cf1bfec4aae0 6 #define min(x, y) (((x) < (y)) ? (x) : (y))
johnylafleur 1:cf1bfec4aae0 7 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
acracan 0:63532ae95efe 8
acracan 0:63532ae95efe 9 Pixy pixy(Pixy::SPI, D11, D12, D13);
acracan 0:63532ae95efe 10 Serial pc(USBTX, USBRX);
johnylafleur 1:cf1bfec4aae0 11 Timer t;
johnylafleur 1:cf1bfec4aae0 12 Servoloop panLoop(200, 200); // Servo loop for pan
johnylafleur 1:cf1bfec4aae0 13 Servoloop tiltLoop(150, 200); // Servo loop for tilt
johnylafleur 1:cf1bfec4aae0 14 MD25 motors(I2C_SDA, I2C_SCL); // declare the motors
acracan 0:63532ae95efe 15
johnylafleur 1:cf1bfec4aae0 16 int random(int numberone, int numbertwo);
johnylafleur 1:cf1bfec4aae0 17
johnylafleur 1:cf1bfec4aae0 18
johnylafleur 1:cf1bfec4aae0 19 int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
johnylafleur 1:cf1bfec4aae0 20 uint32_t lastMove = 0;
johnylafleur 1:cf1bfec4aae0 21 int oldX, oldY, oldSignature;
johnylafleur 1:cf1bfec4aae0 22
johnylafleur 1:cf1bfec4aae0 23 void init_motors(void){
johnylafleur 1:cf1bfec4aae0 24 motors.setMode(0); // MODE 0, 0=marche arriere 128=stop 255=marche arriere vmax
johnylafleur 1:cf1bfec4aae0 25 motors.setCommand(32); // 0x20 reset encoders
johnylafleur 1:cf1bfec4aae0 26 motors.setCommand(50); // 0X32 Disable time out
johnylafleur 1:cf1bfec4aae0 27 //motors.setCommand(32); // 0x20 reset encoders
johnylafleur 1:cf1bfec4aae0 28 //motors.setCommand(48); // 0X32 Disable speed regulation
johnylafleur 1:cf1bfec4aae0 29 }
johnylafleur 1:cf1bfec4aae0 30 void setSpeed (int vitMotor1, int vitMotor2){
johnylafleur 1:cf1bfec4aae0 31 motors.setSpeedRegisters(vitMotor1,vitMotor2);
johnylafleur 1:cf1bfec4aae0 32 }
johnylafleur 1:cf1bfec4aae0 33
johnylafleur 1:cf1bfec4aae0 34
johnylafleur 1:cf1bfec4aae0 35 //---------------------------------------
johnylafleur 1:cf1bfec4aae0 36 // Track blocks via the Pixy pan/tilt mech
johnylafleur 1:cf1bfec4aae0 37 // (based in part on Pixy CMUcam5 pantilt example)
johnylafleur 1:cf1bfec4aae0 38 //---------------------------------------
johnylafleur 1:cf1bfec4aae0 39 int TrackBlock(int blockCount){
johnylafleur 1:cf1bfec4aae0 40
johnylafleur 1:cf1bfec4aae0 41 int trackedBlock = 0;
johnylafleur 1:cf1bfec4aae0 42 long maxSize = 0;
johnylafleur 1:cf1bfec4aae0 43
johnylafleur 1:cf1bfec4aae0 44 pc.printf("blocks =");
johnylafleur 1:cf1bfec4aae0 45 pc.printf("%d",blockCount);
johnylafleur 1:cf1bfec4aae0 46
johnylafleur 1:cf1bfec4aae0 47 for (int i = 0; i < blockCount; i++){
johnylafleur 1:cf1bfec4aae0 48 if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature)){
johnylafleur 1:cf1bfec4aae0 49 long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
johnylafleur 1:cf1bfec4aae0 50 if (newSize > maxSize){
johnylafleur 1:cf1bfec4aae0 51 trackedBlock = i;
johnylafleur 1:cf1bfec4aae0 52 maxSize = newSize;}
johnylafleur 1:cf1bfec4aae0 53 }
johnylafleur 1:cf1bfec4aae0 54 }
johnylafleur 1:cf1bfec4aae0 55 int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
johnylafleur 1:cf1bfec4aae0 56 int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;
johnylafleur 1:cf1bfec4aae0 57 panLoop.update(panError);
johnylafleur 1:cf1bfec4aae0 58 tiltLoop.update(tiltError);
johnylafleur 1:cf1bfec4aae0 59 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
johnylafleur 1:cf1bfec4aae0 60 oldX = pixy.blocks[trackedBlock].x;
johnylafleur 1:cf1bfec4aae0 61 oldY = pixy.blocks[trackedBlock].y;
johnylafleur 1:cf1bfec4aae0 62 oldSignature = pixy.blocks[trackedBlock].signature;
johnylafleur 1:cf1bfec4aae0 63 return trackedBlock;
johnylafleur 1:cf1bfec4aae0 64 }
johnylafleur 1:cf1bfec4aae0 65
johnylafleur 1:cf1bfec4aae0 66 //---------------------------------------
johnylafleur 1:cf1bfec4aae0 67 // Follow blocks via the Zumo robot drive
johnylafleur 1:cf1bfec4aae0 68 //
johnylafleur 1:cf1bfec4aae0 69 // This code makes the robot base turn
johnylafleur 1:cf1bfec4aae0 70 // and move to follow the pan/tilt tracking
johnylafleur 1:cf1bfec4aae0 71 // of the head.
johnylafleur 1:cf1bfec4aae0 72 //---------------------------------------
johnylafleur 2:9caf24407d32 73 int32_t size = 200;
johnylafleur 1:cf1bfec4aae0 74 void FollowBlock(int trackedBlock)
johnylafleur 1:cf1bfec4aae0 75 {
acracan 0:63532ae95efe 76
johnylafleur 1:cf1bfec4aae0 77 int32_t followError = RCS_CENTER_POS - panLoop.m_pos; // How far off-center are we looking now?
johnylafleur 1:cf1bfec4aae0 78 // Size is the area of the object.
johnylafleur 1:cf1bfec4aae0 79 // We keep a running average of the last 8.
johnylafleur 1:cf1bfec4aae0 80 size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
johnylafleur 1:cf1bfec4aae0 81 size -= size >> 3;
johnylafleur 1:cf1bfec4aae0 82 // Forward speed decreases as we approach the object (size is larger)
johnylafleur 2:9caf24407d32 83 int forwardSpeed = constrain(200 - (size/256), -100, 200);
johnylafleur 1:cf1bfec4aae0 84 // Steering differential is proportional to the error times the forward speed
johnylafleur 1:cf1bfec4aae0 85 int32_t differential = (followError + (followError * forwardSpeed))>>8;
johnylafleur 1:cf1bfec4aae0 86 // Adjust the left and right speeds by the steering differential.
johnylafleur 2:9caf24407d32 87 int leftSpeed = constrain(forwardSpeed + differential, -200, 200);
johnylafleur 2:9caf24407d32 88 int rightSpeed = constrain(forwardSpeed - differential, -200, 200);
johnylafleur 1:cf1bfec4aae0 89 // And set the motor speeds
johnylafleur 1:cf1bfec4aae0 90 motors.setSpeedRegisters(leftSpeed,rightSpeed);
johnylafleur 1:cf1bfec4aae0 91 }
johnylafleur 1:cf1bfec4aae0 92 //---------------------------------------
johnylafleur 1:cf1bfec4aae0 93 // Random search for blocks
johnylafleur 1:cf1bfec4aae0 94 //
johnylafleur 1:cf1bfec4aae0 95 // This code pans back and forth at random
johnylafleur 1:cf1bfec4aae0 96 // until a block is detected
johnylafleur 1:cf1bfec4aae0 97 //---------------------------------------
johnylafleur 1:cf1bfec4aae0 98
johnylafleur 1:cf1bfec4aae0 99 void ScanForBlocks(){
johnylafleur 1:cf1bfec4aae0 100 if (t.read_ms() - lastMove > 20){
johnylafleur 1:cf1bfec4aae0 101 lastMove = t.read_ms();
johnylafleur 1:cf1bfec4aae0 102 panLoop.m_pos += scanIncrement;
johnylafleur 1:cf1bfec4aae0 103 if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS)){
johnylafleur 1:cf1bfec4aae0 104 tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
johnylafleur 1:cf1bfec4aae0 105 scanIncrement = -scanIncrement;
johnylafleur 1:cf1bfec4aae0 106 if (scanIncrement < 0){
johnylafleur 2:9caf24407d32 107 setSpeed(0,255);}
johnylafleur 1:cf1bfec4aae0 108 else{
johnylafleur 2:9caf24407d32 109 setSpeed(5,250);}
johnylafleur 1:cf1bfec4aae0 110 wait(random(250, 500));
johnylafleur 1:cf1bfec4aae0 111 }
johnylafleur 1:cf1bfec4aae0 112 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
johnylafleur 1:cf1bfec4aae0 113 }
johnylafleur 1:cf1bfec4aae0 114 }
johnylafleur 1:cf1bfec4aae0 115
johnylafleur 1:cf1bfec4aae0 116
johnylafleur 1:cf1bfec4aae0 117
johnylafleur 1:cf1bfec4aae0 118 int main (void)
johnylafleur 1:cf1bfec4aae0 119 {
johnylafleur 2:9caf24407d32 120
johnylafleur 2:9caf24407d32 121 uint16_t blocks;
johnylafleur 2:9caf24407d32 122
johnylafleur 1:cf1bfec4aae0 123 t.start();
johnylafleur 1:cf1bfec4aae0 124 pc.format(8,Serial::None,1);
johnylafleur 1:cf1bfec4aae0 125 pc.baud(9600);
johnylafleur 1:cf1bfec4aae0 126 pc.printf("started.. \n");
johnylafleur 1:cf1bfec4aae0 127 uint32_t lastBlockTime = 0;
johnylafleur 1:cf1bfec4aae0 128
johnylafleur 1:cf1bfec4aae0 129 while(1){
johnylafleur 1:cf1bfec4aae0 130
johnylafleur 1:cf1bfec4aae0 131 blocks = pixy.getBlocks();
johnylafleur 1:cf1bfec4aae0 132 // If we have blocks in sight, track and follow them
johnylafleur 1:cf1bfec4aae0 133 if (blocks){
johnylafleur 1:cf1bfec4aae0 134 int trackedBlock = TrackBlock(blocks);
johnylafleur 1:cf1bfec4aae0 135 FollowBlock(trackedBlock);
johnylafleur 1:cf1bfec4aae0 136 lastBlockTime = t.read();}
johnylafleur 1:cf1bfec4aae0 137
johnylafleur 1:cf1bfec4aae0 138 else if (t.read() - lastBlockTime > 100){
johnylafleur 1:cf1bfec4aae0 139 setSpeed(128,128);
johnylafleur 1:cf1bfec4aae0 140 ScanForBlocks();}
johnylafleur 1:cf1bfec4aae0 141 }
johnylafleur 1:cf1bfec4aae0 142 }
johnylafleur 1:cf1bfec4aae0 143
johnylafleur 1:cf1bfec4aae0 144
johnylafleur 1:cf1bfec4aae0 145 int random(int numberone, int numbertwo) {
johnylafleur 1:cf1bfec4aae0 146 int random = 0;
johnylafleur 1:cf1bfec4aae0 147 if ((numberone < 0) && (numbertwo < 0)) {
johnylafleur 1:cf1bfec4aae0 148 numberone = numberone * -1;
johnylafleur 1:cf1bfec4aae0 149 numbertwo = numbertwo * -1;
johnylafleur 1:cf1bfec4aae0 150 random = -1 * (rand()%(numberone + numbertwo));
johnylafleur 1:cf1bfec4aae0 151 }
johnylafleur 1:cf1bfec4aae0 152 if ((numbertwo < 0) && (numberone >= 0)) {
johnylafleur 1:cf1bfec4aae0 153 numbertwo = numbertwo * -1;
johnylafleur 1:cf1bfec4aae0 154 random = (rand()%(numberone + numbertwo)) - numbertwo;
johnylafleur 1:cf1bfec4aae0 155 }
johnylafleur 1:cf1bfec4aae0 156 if ((numberone < 0) && (numbertwo >= 0)) {
johnylafleur 1:cf1bfec4aae0 157 numberone = numberone * -1;
johnylafleur 1:cf1bfec4aae0 158 random = (rand()%(numberone + numbertwo)) - numberone;
johnylafleur 1:cf1bfec4aae0 159 } else {
johnylafleur 1:cf1bfec4aae0 160 random = (rand()%(numberone + numbertwo)) - min(numberone, numbertwo);
johnylafleur 1:cf1bfec4aae0 161 }
johnylafleur 1:cf1bfec4aae0 162 return random;
johnylafleur 1:cf1bfec4aae0 163 }