Diff: Sources/Pixy.cpp

Revision:

0:15a8480061e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Pixy.cpp	Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,79 @@
+
+#include "Pixy.h"
+#include "Robot.h"
+#include "Declarations.h"
+
+Pixy::Pixy(Serial& _cam) : cam(_cam)
+{
+    //cam.baud( 230400 );
+    cam.baud( 460800 );
+    cam.attach(this, &Pixy::rxCallback, Serial::RxIrq);
+}
+
+
+// This function is called when a character goes into the RX buffer.
+void Pixy::rxCallback()
+{
+    static const int buffersize = 256;
+    this->detects = 0;
+    static int startPoint = 0;
+    static uint8_t buffer[buffersize] = {};
+    static bool startFound = false;
+    static int ii = 1;
+
+    while( cam.readable() ) {
+        buffer[ii] = cam.getc();
+        if( buffer[ii-1] == 85  && (buffer[ii] == 170 )  ) {
+            startPoint = ii-1;
+
+            //check if detection was on the edge of buffer. Skip package if so.
+            if( ii<(buffersize-14))
+                startFound = true;
+            else
+                ii = 1;
+                
+            detects++;
+        }
+        ++ii;
+
+        //reset ii
+        if( ii>=(buffersize-1))
+            ii = 1;
+    }
+
+    //start not found, reset ii to 1
+    if( !startFound && ii >= 3 || ii >= (buffersize-1)) {
+        ii = 1;
+        return;
+    }
+
+    //start is found but not enough bytes received - return
+    if( (ii-startPoint) <= 13 )
+        return;
+
+    //copy memory to pixy struct
+    memcpy( &pixy, buffer + startPoint+2, 12);
+
+    //reset variables
+    startFound = false;
+    ii = 1;
+}
+
+int Pixy::getX()
+{
+    return pixy.x;
+}
+
+int Pixy::getY()
+{
+    return pixy.y;
+}
+
+int Pixy::getSignature()
+{
+    return pixy.signature;
+}
+
+int Pixy::getDetects(){
+    return this->detects;
+}