/* 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);
}
/** Singleton instance and entry into while loop that runs the desired program*/
public static void Main()
{
/* Tracking gamepad buttons states for single press captures*/
bool LastBtn1 = false;
bool LastBtn2 = false;
bool LastBtn3 = false;
bool LastBtn5 = false;
bool LastBtn6 = false;
/* Forword/Backward Scalor */
const float kScalarX = 0.50f;
/* Left/Right Scalor */
const float kScalarY = 0.50f;
/* Turning Scalor */
const float kScalarTwist = 0.30f;
/* Ramp Rate */
const float kVoltageRampSec = 0.2f;
/* Gains for PixyDrive (May have to play with this for a better PixyDrive experience)*/
float KpGain = 0.002f; /*P-Gain for Turning*/
float KdGain = 0.001f; /*D-Gain for Turning*/
float kTurnCorrectionRatio = 0.3f; /*Ratio for turning speed*/
float KpGain1 = 0.0003f; /*P-Gain for driving*/
float KdGain1 = 0.0f; /*D-Gain for driving*/
float kForwardCorrectionRatio = 0.6f; /*Ratio for driving speed*/
/* Configure Talons to operate in percentage VBus mode, and Ramp Up Voltage*/
foreach (CTRE.TalonSrx temp in Talons)
{
temp.SetControlMode(TalonSrx.ControlMode.kPercentVbus);
temp.SetVoltageRampRate(12.0f / kVoltageRampSec);
}
/* Initiate the program by starting off in Manaul Drive mode */
CurrentState = States.ManualDrive;
/* Variables to hold target position and distance of object, set when PixyDrive is initialized */
int TargetX = 160;
int TargetArea = 0;
/* Number of values to use when averaging data from Block, Set to 1 to remove average */
int AvgCount = 3;
/* Tracks the Distance between the PixyCamera and the Object using the pervious Area used in PD Loop */
int LastErrorArea = 0;
/* Value used to cycle between 3 colors of the LED on the PixyCamera */
byte RGB = 0;
while (true)
{
/* Keep robot enabled if gamepad is connected and in 'D' mode */
if (GamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* Clear the average of X, Y, and Area */
int AverageX = 0;
int AverageY = 0;
int AverageArea = 0;
/* Adds up the pervious values of X, Y, and Area */
for (int i = 0; i < AvgCount; i++)
{
/* Run Pixy process to check for Data, pull Block if there is data */
Thread.Sleep(5);
Pixy.Process();
if (Pixy.GetBlock(blockToFill))
{
/* Store block from Pixy into Local Block */
CurrentBlock = blockToFill;
/* Uncomment for block data */
Debug.Print("===================================");
Debug.Print(blockToFill.ToString());
}
///* Pull Block data into variables */
AverageX += CurrentBlock.X; /* Pull X from CurrentBlock */
AverageY += CurrentBlock.Y; /* Pull Y from CurrentBlock */
AverageArea += CurrentBlock.Area; /* Pull Area from CurrentBlock */
}
/* Finishes the average process by dividing the sum by the number of values */
AverageX /= AvgCount;
AverageY /= AvgCount;
AverageArea /= AvgCount;
/* Sync Status of Pixy */
bool Synced = Pixy.Status.Synced;
/* Duration since last time Pixy had a good data in ms */
long TimeSinceLastBlock = Pixy.Status.TimeSinceLastBlock;
/* Uncomment for Pixy status information */
//Debug.Print("===================================");
//Debug.Print(Pixy.Status.ToString());
/* Get angular rate of the robot from Pigeon IMU */
float[] XYZ_Dps = new float[3];
Pidgeot.GetRawGyro(XYZ_Dps);
float CurrentAngularRate = XYZ_Dps[2];
/* Allows for increasing or decreasing the distance between PixyCamera and the target object
* Button 5 (Left-bumper) decreases the range
* Button 6 (Right-bumper) increases the range */
bool Btn5 = GamePad.GetButton(5);
bool Btn6 = GamePad.GetButton(6);
if (Btn5 && !LastBtn5)
{
if (TargetArea < (TargetArea + 350)) /* Minimum Distance found through play */
{
TargetArea += 50; /* Step closer to object */
}
/* Else, the object is too close to PixyCam */
}
if (Btn6 & !LastBtn6)
{
if (TargetArea > (TargetArea - 350)) /* Maximum Distance found through play */
{
TargetArea -= 50; /* Step further from object */
}
/* Else, the object is too far from PixyCam */
}
/* Control the LED on the Pixy Camera
* Button 2 (A) Cycles the color of the LED between Red, Green, and Blue */
bool Btn2 = GamePad.GetButton(2);
if (Btn2 && !LastBtn2)
{
/* Cycle between 3 values by incrementing and wrapping around */
if (RGB == 2)
{
RGB = 0;
}
else
{
RGB++;
}
/* Set the states of the color based on RGB value
* Colors can be modified by doing different combinations of 0 and 255 */
if (RGB == 0)
{
Pixy.SetLED(true, false, false);
}
else if (RGB == 1)
{
Pixy.SetLED(false, true, false);
}
else if (RGB == 2)
{
Pixy.SetLED(false, false, true);
}
}
LastBtn2 = Btn2;
/* Always give user the ability to enter Manaul Drive mode
* Button 1 (X) changes the PixyDrive State to Manual Drive State */
bool Btn1 = GamePad.GetButton(1);
if (Btn1 && !LastBtn1)
{
CurrentState = States.ManualDrive;
}
LastBtn1 = Btn1;
/* State machine that determines if Pixy Target needs to be initiated, Pixy object is in target of Pixy Camera parameters, or
* robot is in Manual Drive mode */
switch (CurrentState)
{
case States.Init:
/* Grab Inital area and X-position, and set as target values */
TargetX = AverageX;
TargetArea = AverageArea;
CurrentState = States.PixyDrive;
break;
case States.PixyDrive:
/* Bulk of the PixyDrive, where it uses the current Pixy data to allign with the TargetArea and TargetX */
if (Synced == true && TimeSinceLastBlock < 50) /* Time since last block should be around 50 ms */
{
/* Object is in View of camera and we getting good data */
/* Find the Error between the desired values between the the current values of Area and X */
int ErrorX = TargetX - AverageX;
int ErrorArea = TargetArea - AverageArea;
/* Variable used later on to smooth out PD of Drive */
int ErrorArea_Kd1 = ErrorArea - LastErrorArea;
/* Track the last error value and compared to next error value */
LastErrorArea = ErrorArea;
/* Checks to see if the error is within an allowable margin,
* ErrorX is good in the sense that there is minimal noise on X and Y
* Error Area can be tweeked according to application as larger objects will have more noise and smallear objects have
* less noise and will require more preciseness for better response */
if ((ErrorX < 3 && ErrorX > -3) && (ErrorArea < 20 && ErrorArea > -20))
{
/* Consisered centerd and a good distance away so stop */
TankDrive(0, 0);
}
else
{
/* There is error with either X or Area, so fix that */
bool xOutOfBounds = OutOfBoundsX(AverageX, CurrentBlock.Width);
/* If object is out bounds, kill the forward and backward drive, turn right or left to still follow */
if (xOutOfBounds)
{
/* Set ErrorArea to 0 to prevent forward/backward drive in PixyDrive */
ErrorArea = 0;
ErrorArea_Kd1 = 0;
}
/* PD Loop for PixyDrive until centered
* Turning uses ErrorX for P and CurrentAngularRate for D
* Forward/Backward uses ErrorArea for P and ErrorArea_Kd1 (ErrorArea - LastErrorArea) for D */
float Turn = (ErrorX) * KpGain - (CurrentAngularRate) * KdGain;
float Forward = (ErrorArea) * KpGain1 + (ErrorArea_Kd1) * KdGain1;
/* Cap the return values to turn down the output of the motors */
Turn = Cap(Turn, kTurnCorrectionRatio);
Forward = Cap(Forward, kForwardCorrectionRatio);
/* With the turn and forward values found, feed that to the TankDrive accordingly */
float LeftAuto = ((-Turn + Forward));
float RightAuto = ((Turn + Forward));
TankDrive(LeftAuto, RightAuto);
}
}
else
{
/* We are in PixyDrive Mode, but there is no object found, so stop */
TankDrive(0, 0);
}
break;
case States.ManualDrive:
/* Allow the user to enter PixyDrive mode
* Button 3 (B) Changes Manual Drive Stat to PixyDrive State */
bool Btn3 = GamePad.GetButton(3);
if (Btn3 && !LastBtn3)
{
CurrentState = States.Init;
}
LastBtn3 = Btn3;
/* Regular mecanum drive that is scaled and Gamepad joysticks have been adjusted */
float X = GamePad.GetAxis(0);
/* Invert gamepad so forward is truly forward */
float Y = -1 * GamePad.GetAxis(1);
float Twist = GamePad.GetAxis(2);
MecanumDrive(Y * kScalarY, X * kScalarX, Twist * kScalarTwist);
break;
}
}
}
/**
* 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;
}
}
