Diff: main.cpp

Revision:

1:cf1bfec4aae0

Parent:

0:63532ae95efe

Child:

2:9caf24407d32

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