Diff: Pixy.cpp

Revision:

5:23e98079fb75

Parent:

0:ed8dc4531ac1

--- a/Pixy.cpp	Fri Feb 26 19:10:09 2016 +0000
+++ b/Pixy.cpp	Fri Mar 04 17:20:22 2016 +0000
@@ -29,45 +29,67 @@
 uint16_t Pixy::getBlocks(uint16_t maxBlocks)
 {
     uint8_t i;
-    uint16_t w, checksum, sum;
-    Block *block;
-
+    uint16_t w, checksum, sum, type;
+    Block *block;;
     if (!skipStart) {
-        if (getStart() == false)
+        if (getStart(&type) == false) {
+            //pc->printf("fuck1 \n\r");
             return 0;
-    } else
+        }
+    } else {
         skipStart = false;
+    }
 
     for (blockCount = 0; blockCount < maxBlocks && blockCount < PIXY_MAXIMUM_ARRAYSIZE;) {
+        //pc->printf("tick \n\r");
+        
         checksum = m_link->getWord();
-        if (checksum == PIXY_START_WORD) { // we've reached the beginning of the next frame
+        if ((checksum == PIXY_START_WORD)||(checksum == PIXY_START_WORD_CC)) {
             skipStart = true;
-            //if (pc)
-            //  pc->printf("skip\n\r");
+            //pc->printf("fuck2 \n\r");
             return blockCount;
-        } else if (checksum == 0)
+        } else if (checksum == 0) {
+            //pc->printf("fuck3 \n\r");
             return blockCount;
+        }
 
-        if (blockCount > blockArraySize)
-            resize();
+        if (blockCount > blockArraySize) resize();
 
         block = blocks + blockCount;
-
-        for (i = 0, sum = 0; i < sizeof(Block) / sizeof(uint16_t); i++) {
+        
+        *((uint16_t *)block) = type;
+        
+        for (i = 1, sum = 0; i < (sizeof(Block)/sizeof(uint16_t))-1; i++) {
+            w = m_link->getWord();
+            sum += w;
+            *((int16_t *)block + i) = w;
+        }
+        
+        // Read in the extra word if its a color code
+        if (block->type  == PIXY_START_WORD_CC) {
             w = m_link->getWord();
             sum += w;
             *((uint16_t *)block + i) = w;
         }
 
-        if (checksum == sum)
-            blockCount++;
-        else if (pc)
-            pc->printf("cs error\n\r");
-
+        if (checksum == sum) blockCount++;
+        else if (pc) {
+            //pc->printf("cs error: csum: %d, sum: %d \n\r", checksum, sum);
+            //pc->printf("ERROR: ");
+            block->print(*pc);
+        }
+        
+        // Read in the next block type
         w = m_link->getWord();
-        if (w != PIXY_START_WORD)
+        if (w == PIXY_START_WORD) {
+            type = PIXY_START_WORD;
+        } else if (w == PIXY_START_WORD_CC) {
+            type = PIXY_START_WORD_CC;
+        } else {
             return blockCount;
+        }
     }
+    //pc->printf("fuck4 \n\r");
     return blockCount;
 }
 
@@ -93,21 +115,22 @@
     this->pc = pc;
 }
 
-bool Pixy::getStart()
+bool Pixy::getStart(uint16_t *type)
 {
     uint16_t w, lastw;
 
     lastw = 0xffff;
     while (true) {
         w = m_link->getWord();
+        //pc->printf("word: %04x \r\n", w);
         if (w == 0 && lastw == 0) {
             wait_ms(10);
             return false;
-        } else if (w == PIXY_START_WORD && lastw == PIXY_START_WORD)
+        } else if ((w == PIXY_START_WORD || w == PIXY_START_WORD_CC) && lastw == PIXY_START_WORD) {
+            *type = w;
             return true;
-        else if (w == PIXY_START_WORDX) {
-            if (pc)
-                pc->printf("reorder\n\r");
+        } else if (w == PIXY_START_WORDX) {
+            if (pc) pc->printf("reorder\n\r");
             m_link->getByte(); // resync
         }
         lastw = w;