private void GoStraight(float Y, float targetHeading)
{
if (_enableCompensating == false)
{
/* calling app has turned this off */
_isCompensating = false;
}
else if (_pidgey.GetState() != PigeonImu.PigeonState.Ready)
{
/* pigeon is not present on bus */
_isCompensating = false;
}
else
{
/* feature enabled and pigeon is present, we are good to go */
_isCompensating = true;
}
/* if we can compensate do it, otherwise just apply same output on both sides */
float x_correction;
if (_isCompensating == false)
{
/* apply same to both sides */
x_correction = 0;
}
else
{
/* let user know if they have more work to do */
if (_servoParams.P == 0 && _servoParams.I == 0 && _servoParams.D == 0)
{
Debug.Print("CTR: Servo Go Straight With Imu has no PID values, cannot go straight");
}
/* Grab current heading */
float currentHeading = GetImuHeading();
/* Grab angular rate from the pigeon */
_pidgey.GetRawGyro(XYZ_Dps);
float currentAngularRate = XYZ_Dps[2];
/* Heading PID */
float headingError = targetHeading - currentHeading;
float X = (headingError) * _servoParams.P - (currentAngularRate) * _servoParams.D;
X = Util.Cap(X, _maxOutput);
x_correction = -X;
}
/* Select control mode based on selected style */
_driveTrain.Set(_selectedStyle, Y, x_correction);
}
/** 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;
}
}
}