Majordhome/pixy
Dependencies: MD25 Servoloop mbed pixy
Fork of pixyHelloWorld by
main.cpp@1:cf1bfec4aae0, 2015-04-13 (annotated)
- Committer:
- johnylafleur
- Date:
- Mon Apr 13 12:17:40 2015 +0000
- Revision:
- 1:cf1bfec4aae0
- Parent:
- 0:63532ae95efe
- Child:
- 2:9caf24407d32
Majordhome/pixy;
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:cf1bfec4aae0 | 73 | int32_t size = 400; |
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 | 1:cf1bfec4aae0 | 83 | int forwardSpeed = constrain(400 - (size/256), -100, 400); |
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 | 1:cf1bfec4aae0 | 87 | int leftSpeed = constrain(forwardSpeed + differential, -400, 400); |
johnylafleur | 1:cf1bfec4aae0 | 88 | int rightSpeed = constrain(forwardSpeed - differential, -400, 400); |
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 | 1:cf1bfec4aae0 | 107 | setSpeed(-250,250);} |
johnylafleur | 1:cf1bfec4aae0 | 108 | else{ |
johnylafleur | 1:cf1bfec4aae0 | 109 | setSpeed(0,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 | 1:cf1bfec4aae0 | 120 | uint16_t blocks; |
johnylafleur | 1:cf1bfec4aae0 | 121 | t.start(); |
johnylafleur | 1:cf1bfec4aae0 | 122 | pc.format(8,Serial::None,1); |
johnylafleur | 1:cf1bfec4aae0 | 123 | pc.baud(9600); |
johnylafleur | 1:cf1bfec4aae0 | 124 | pc.printf("started.. \n"); |
johnylafleur | 1:cf1bfec4aae0 | 125 | uint32_t lastBlockTime = 0; |
johnylafleur | 1:cf1bfec4aae0 | 126 | |
johnylafleur | 1:cf1bfec4aae0 | 127 | while(1){ |
johnylafleur | 1:cf1bfec4aae0 | 128 | |
johnylafleur | 1:cf1bfec4aae0 | 129 | blocks = pixy.getBlocks(); |
johnylafleur | 1:cf1bfec4aae0 | 130 | // If we have blocks in sight, track and follow them |
johnylafleur | 1:cf1bfec4aae0 | 131 | if (blocks){ |
johnylafleur | 1:cf1bfec4aae0 | 132 | int trackedBlock = TrackBlock(blocks); |
johnylafleur | 1:cf1bfec4aae0 | 133 | FollowBlock(trackedBlock); |
johnylafleur | 1:cf1bfec4aae0 | 134 | lastBlockTime = t.read();} |
johnylafleur | 1:cf1bfec4aae0 | 135 | |
johnylafleur | 1:cf1bfec4aae0 | 136 | else if (t.read() - lastBlockTime > 100){ |
johnylafleur | 1:cf1bfec4aae0 | 137 | setSpeed(128,128); |
johnylafleur | 1:cf1bfec4aae0 | 138 | ScanForBlocks();} |
johnylafleur | 1:cf1bfec4aae0 | 139 | } |
johnylafleur | 1:cf1bfec4aae0 | 140 | } |
johnylafleur | 1:cf1bfec4aae0 | 141 | |
johnylafleur | 1:cf1bfec4aae0 | 142 | |
johnylafleur | 1:cf1bfec4aae0 | 143 | int random(int numberone, int numbertwo) { |
johnylafleur | 1:cf1bfec4aae0 | 144 | int random = 0; |
johnylafleur | 1:cf1bfec4aae0 | 145 | if ((numberone < 0) && (numbertwo < 0)) { |
johnylafleur | 1:cf1bfec4aae0 | 146 | numberone = numberone * -1; |
johnylafleur | 1:cf1bfec4aae0 | 147 | numbertwo = numbertwo * -1; |
johnylafleur | 1:cf1bfec4aae0 | 148 | random = -1 * (rand()%(numberone + numbertwo)); |
johnylafleur | 1:cf1bfec4aae0 | 149 | } |
johnylafleur | 1:cf1bfec4aae0 | 150 | if ((numbertwo < 0) && (numberone >= 0)) { |
johnylafleur | 1:cf1bfec4aae0 | 151 | numbertwo = numbertwo * -1; |
johnylafleur | 1:cf1bfec4aae0 | 152 | random = (rand()%(numberone + numbertwo)) - numbertwo; |
johnylafleur | 1:cf1bfec4aae0 | 153 | } |
johnylafleur | 1:cf1bfec4aae0 | 154 | if ((numberone < 0) && (numbertwo >= 0)) { |
johnylafleur | 1:cf1bfec4aae0 | 155 | numberone = numberone * -1; |
johnylafleur | 1:cf1bfec4aae0 | 156 | random = (rand()%(numberone + numbertwo)) - numberone; |
johnylafleur | 1:cf1bfec4aae0 | 157 | } else { |
johnylafleur | 1:cf1bfec4aae0 | 158 | random = (rand()%(numberone + numbertwo)) - min(numberone, numbertwo); |
johnylafleur | 1:cf1bfec4aae0 | 159 | } |
johnylafleur | 1:cf1bfec4aae0 | 160 | return random; |
johnylafleur | 1:cf1bfec4aae0 | 161 | } |