Diff: PixyData/PixyData.cpp

Revision:

0:fc7c5285fa79

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixyData/PixyData.cpp	Thu Oct 25 03:57:08 2018 +0000
@@ -0,0 +1,47 @@
+#include "mbed.h"
+#include "PixyData.h"
+#include "GlobalVariables.h"
+#include "PinAssign.h"
+
+void PixyParse(void)
+{
+    SigNum      = PixyBuf[4]|(PixyBuf[5]<<8);
+    XObs        = PixyBuf[6]|(PixyBuf[7]<<8);
+    YObs        = PixyBuf[8]|(PixyBuf[9]<<8);
+    WidObs      = PixyBuf[10]|(PixyBuf[11]<<8);
+    HeiObs      = PixyBuf[12]|(PixyBuf[13]<<8);
+    AngObs      = PixyBuf[14]|(PixyBuf[15]<<8);
+}
+
+void PixyData(void)
+{
+    uint8_t buf;
+    buf = pixy.getc();
+    pc.putc(buf);
+    
+    if(PixyCnt==0&&buf==0x56||PixyCnt==1&&buf==0xaa||PixyCnt>=2&&PixyCnt<=15)
+    {
+        PixyBuf[PixyCnt]=buf;
+        PixyCnt++;
+        
+        //pc.printf("proc\n");
+    }
+    else if(PixyCnt==16)
+    {
+        //pc.printf("chks\n");
+        
+        PixyChks = PixyBuf[2]|(PixyBuf[3]<<8);
+        int chks=0;
+        for(uint8_t i=4;i<=15;i++)
+        {
+            chks+=PixyBuf[i];
+        }
+        if(chks==PixyChks)
+        {
+            PixyParse();
+            //pc.printf("pars\n");
+        }
+        PixyCnt=0;
+    }
+    else PixyCnt=0;
+}
\ No newline at end of file