// TODO: Try changing type from Int32 to Int16. Two bytes = Int16. private PixyBlock BytesToBlock(byte[] bytes) { PixyBlock block = new PixyBlock(); block.Sync = BytesToInt(bytes[1], bytes[0]); block.Checksum = BytesToInt(bytes[3], bytes[2]); block.Signature = OrBytes(bytes[5], bytes[4]); block.CenterX = ((((int)bytes[7] & 0xff) << 8) | ((int)bytes[6] & 0xff)); block.CenterY = ((((int)bytes[9] & 0xff) << 8) | ((int)bytes[8] & 0xff)); block.Width = ((((int)bytes[11] & 0xff) << 8) | ((int)bytes[10] & 0xff)); block.Height = ((((int)bytes[13] & 0xff) << 8) | ((int)bytes[12] & 0xff)); return(block); }
private void Read() { _Blocks.Clear(); // Clear the list of all previous blocks. byte[] bytes = new byte[64]; // Create a new array to hold the byte data from the Pixy via I2C. Port.Read(Address, 64, bytes); // Read the bytes from the Pixy into the array buffer. int i = 0; for (; i < bytes.Length; i++) { // Set the bytes to be greater than 0. int byte1 = bytes[i]; if (byte1 < 0) { byte1 += 256; } int byte2 = bytes[i]; if (byte2 < 0) { byte2 += 256; } // Check if the end bytes have been received. if (byte1 == 0x55 && byte2 == 0xaa) { break; } } if (i == 63) { return; } else if (i == 0) { i += 2; } for (int byteOffset = i; byteOffset < bytes.Length - BlockSize - 1;) { int byte1 = bytes[byteOffset]; if (byte1 < 0) { byte1 += 256; } int byte2 = bytes[byteOffset++]; if (byte2 < 0) { byte2 += 256; } // Check if the end bytes have been received. if (byte1 == 0x55 && byte2 == 0xaa) { byte[] temp = new byte[BlockSize]; for (int tempOffset = 0; tempOffset < BlockSize; tempOffset++) { temp[tempOffset] = bytes[byteOffset + tempOffset]; } PixyBlock block = BytesToBlock(temp); if (block.Signature == 1) { Blocks.Add(block); byteOffset += BlockSize - 1; } else { byteOffset++; } } else { byteOffset++; } } }