/* Copies block data to another block */
public void CopyTo(PixyBlock block)
{
block.Signature = Signature;
block.X = X;
block.Y = Y;
block.Width = Width;
block.Height = Height;
block.Angle = Angle;
block.Area = Area;
block.IsColorCoded = IsColorCoded;
}
/**
* Grabs a PixyBlock
*
* @param blockToFill Block to fill good with
* @return bool If Block data is available or not
*/
public bool GetBlock(PixyBlock blockToFill)
{
if (_Blocks.Count > 0)
{
PixyBlock pb = (PixyBlock)_Blocks[0]; /* TODO:replace circulator buffer */
pb.CopyTo(blockToFill);
_Blocks.RemoveAt(0); /* TODO: replace circulator buffer */
return(true);
}
return(false);
}
/**
* Processing that searches for the beginning of the frame and syncs up to grab data
*/
public void Process()
{
switch (PixyState)
{
case States.FirstWord:
/* Find the start of the frame */
Word = GetWord(); /* Grab Word */
if (Word == Pixy_FRAME_START_WORD)
{
PixyState = States.SecondWord; /* First word good, look for second word */
}
else if (Word == Pixy_FRAME_WRONG_ORDER)
{
GetByte(0); /* Immediately clock 8 bits to resync */
++_pixyStatus.SyncErrorCount; /* Track error */
_pixyStatus.Synced = false; /* Lost sync */
}
else if (Word == 0)
{
/* Do nothing because Pixy is synced */
}
else
{
++_pixyStatus.SyncErrorCount; /* Track error */
_pixyStatus.Synced = false; /* Lost sync */
}
break;
case States.SecondWord:
//Find the second start frame and determie if Single color or Color coded
Word = GetWord(); /* Grab Word */
if (Word == Pixy_FRAME_START_WORD)
{
/* Frame Start found and single color block */
IsColorCoded = false;
PixyState = States.Block;
}
else if (Word == Pixy_FRAME_START_WORD_CC)
{
/* Frame Start found and Colorcoded block */
IsColorCoded = true;
PixyState = States.Block;
}
else
{
/* Frame Start not found so restart entire process, not synced */
PixyState = States.FirstWord; /* Restart the StartFrame Search */
++_pixyStatus.SyncErrorCount; /* Track error */
_pixyStatus.Synced = false; /* Lost sync */
}
break;
case States.Block:
/* Synced, now grab Block from Pixy */
_pixyStatus.Synced = true; /* Synced */
Start = DateTime.Now; /* Initiate TimeSinceLastBlock */
Block = new PixyBlock(); /* Block for holding data */
Sum = 0; /* Reset sum */
Checksum = GetWord(); /* Grab checksum */
/* Grab Signature */
Block.Signature = GetWord();
Sum += Block.Signature;
/* Grab X */
Block.X = GetWord();
Sum += Block.X;
/* Grab Y */
Block.Y = GetWord();
Sum += Block.Y;
/* Grab Width */
Block.Width = GetWord();
Sum += Block.Width;
/* Grab Height */
Block.Height = GetWord();
Sum += Block.Height;
/* Check to see if we can get angle */
if (!IsColorCoded)
{
/*Angle only availabe in CC mode*/
Block.Angle = 0;
}
else
{
/* Colorcoded block so get angle */
Block.Angle = (Int16)GetWord();
Sum += (UInt16)Block.Angle;
}
/* Calculate area */
Block.Area = (int)(Block.Height * Block.Width);
/* ColorCode store into Block */
Block.IsColorCoded = IsColorCoded;
/* Send CAN Frames of the current data */
byte[] Frame1 = new byte[8];
Frame1[0] = (byte)(Block.X >> 8);
Frame1[1] = (byte)(Block.X & 0xFF);
Frame1[2] = (byte)(Block.Y >> 8);
Frame1[3] = (byte)(Block.Y & 0xFF);
Frame1[4] = (byte)(Block.Width >> 8);
Frame1[5] = (byte)(Block.Width & 0xFF);
Frame1[6] = (byte)(Block.Height >> 8);
Frame1[7] = (byte)(Block.Height & 0xFF);
ulong data1 = (ulong)BitConverter.ToInt64(Frame1, 0);
CTRE.Native.CAN.Send(9, data1, 8, 0);
/* CAN Frames part 2 */
byte[] Frame2 = new byte[8];
Frame2[0] = (byte)(Block.Signature >> 8);
Frame2[1] = (byte)(Block.Signature & 0xFF);
Frame2[2] = (byte)(Block.Angle >> 8);
Frame2[3] = (byte)(Block.Angle & 0xFF);
Frame2[4] = (byte)(Block.Area >> 8);
Frame2[5] = (byte)(Block.Area & 0xFF);
ulong data2 = (ulong)BitConverter.ToInt64(Frame2, 0);
CTRE.Native.CAN.Send(25, data2, 8, 0);
if (Checksum == Sum)
{
/* Checksum is valid so store the block */
_BlockCount++;
_Blocks.Add(Block);
}
else
{
/* Checksum is invalid, throwaway block and keep track */
++_pixyStatus.ChecksumErrorCount;
}
/* Finished grabbing block, reset processing */
PixyState = States.FirstWord;
break;
}
}
