public static void Main()
{
/* Initialize Display */
CTRE.Gadgeteer.Module.BalanceDisplayModule.LabelSprite titleDisplay, pitchDisplay, outputDisplay, PIDBalanceDisplay, PIDVelocityDisplay,
PIDDriveDisplay, PIDScalerDisplay, batteryDisplay, encoderDisplayLeft, encoderDisplayRight;
/* State and battery display */
titleDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red, 1, 1, 80, 15);
batteryDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Green, 80, 1, 80, 15);
/* Pitch and output display on top right */
pitchDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 1, 21, 80, 15);
outputDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 80, 21, 80, 15);
/* Gain Display at the bottom */
PIDScalerDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Yellow, 1, 41, 80, 15);
PIDBalanceDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 61, 90, 15);
PIDDriveDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 81, 90, 15);
PIDVelocityDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 101, 90, 15);
encoderDisplayRight = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 50, 10, 15);
encoderDisplayLeft = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 90, 10, 15);
/* Initialize drivetrain */
Hardware.leftTalon.ConfigPeakCurrentLimit(15);
Hardware.rightTalon.ConfigPeakCurrentLimit(15);
Hardware.leftTalon.ConfigContinuousCurrentLimit(15);
Hardware.rightTalon.ConfigContinuousCurrentLimit(15);
Hardware.leftTalon.ConfigPeakCurrentDuration(0);
Hardware.rightTalon.ConfigPeakCurrentDuration(0);
Hardware.leftTalon.EnableCurrentLimit(true);
Hardware.rightTalon.EnableCurrentLimit(true);
Hardware.leftGearbox.SetSensorPhase(false);
Hardware.rightTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_10Ms);
Hardware.rightTalon.ConfigVelocityMeasurementWindow(32);
Hardware.leftTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_10Ms);
Hardware.leftTalon.ConfigVelocityMeasurementWindow(32);
Hardware.leftTalon.ConfigNeutralDeadband(0.001f);
Hardware.rightTalon.ConfigNeutralDeadband(0.001f);
/* Speed up Pigeon frames */
Hardware.leftTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrame.Status_2_Feedback0_, 10, 10);
Hardware.rightTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrame.Status_2_Feedback0_, 10, 10);
Hardware.leftTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_3_Quadrature, 2, 10);
Hardware.rightTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_3_Quadrature, 2, 10);
Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.BiasedStatus_2_Gyro, 5, 10);
Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, 10);
CTRE.Phoenix.MotorControl.SensorCollection _leftSensors = Hardware.leftTalon.GetSensorCollection();
CTRE.Phoenix.MotorControl.SensorCollection _rightSensors = Hardware.rightTalon.GetSensorCollection();
float[] XYZ_Dps = new float[3];
float tempP = 0;
float tempI = 0;
float tempD = 0;
byte CurrentPID = 0;
float lastVelocity = 0;
CTRE.MP.MotionProfiler velocityRamp = new CTRE.MP.MotionProfiler(1000000, 6000, 0, 50); //Essentially infinite jerk, with a control on acceleration
CTRE.Phoenix.Stopwatch loopTimer = new CTRE.Phoenix.Stopwatch();
loopTimer.Start();
uint lastOuterLoopTime = loopTimer.DurationMs;
uint lastInnerLoopTime = loopTimer.DurationMs;
float angleSetpoint = 0;
uint loopCounter = 0;
bool trackingMode = false;
bool tippedOver = false;
uint tipStart = 0;
uint tipEnd = 0;
bool pointTurnMode = false;
while (true)
{
if (loopTimer.DurationMs < lastOuterLoopTime + 50)
{
continue;
}
if (loopTimer.DurationMs - lastOuterLoopTime > 1000)
{
//Just reset the timer, it's way too far away now
lastOuterLoopTime = loopTimer.DurationMs;
}
lastOuterLoopTime += 50;
if (Hardware.Gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
float stick = Hardware.Gamepad.GetAxis(1);
float turn = Hardware.Gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref stick);
CTRE.Phoenix.Util.Deadband(ref turn);
/* Grab values from gamepad */
turn *= 0.5f;
int xOffset = 0;
int targetHeight = 0;
bool disableForward = true;
CTRE.Phoenix.Pixy2CCC.Block b = Hardware.pixy.ccc.OffsetBlock(ref disableForward, ref xOffset, ref targetHeight, 50, 2);
if (b != null && trackingMode && !manualMode)
{
if (!disableForward)
{
stick = (targetHeight - 25) * 0.015f;
}
turn = xOffset * 0.002f;
}
bool button10 = Hardware.Gamepad.GetButton(10);
if (button10 && !lastButton10)
{
trackingMode = !trackingMode;
Hardware.Display.Clear();
if (trackingMode)
{
Hardware.Display.AddRectSprite(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Blue, 0, 0, 80, 130);
Hardware.Display.AddRectSprite(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Blue, 80, 0, 80, 130);
}
else
{
titleDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red, 1, 1, 80, 15);
batteryDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Green, 80, 1, 80, 15);
/* Pitch and output display on top right */
pitchDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 1, 21, 80, 15);
outputDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 80, 21, 80, 15);
/* Gain Display at the bottom */
PIDScalerDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Yellow, 1, 41, 80, 15);
PIDBalanceDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 61, 90, 15);
PIDDriveDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 81, 90, 15);
PIDVelocityDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 101, 90, 15);
encoderDisplayRight = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 50, 10, 15);
encoderDisplayLeft = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 90, 10, 15);
}
}
lastButton10 = button10;
/* Pitch offsetter */
bool button2 = Hardware.Gamepad.GetButton(2);
if (button2 && !lastButton2)
{
pitchoffset = 0;
pitchoffset = GetPitch();
}
lastButton2 = button2;
/* State control */
bool button1 = Hardware.Gamepad.GetButton(1);
if (button1 && !lastButton1)
{
OperateState = !OperateState;
Iaccum = 0;
Iaccum_velocity = 0;
}
lastButton1 = button1;
bool button4 = Hardware.Gamepad.GetButton(4);
if (button4 && !lastButton4)
{
CurrentPID++;
if (CurrentPID > 2)
{
CurrentPID = 0;
}
}
lastButton4 = button4;
if (!trackingMode)
{
if (CurrentPID == 0)
{
PIDcontrol_test(BalancePID);
PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White);
PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
}
else if (CurrentPID == 1)
{
PIDcontrol_test(ArbitraryGains);
PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White);
PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
}
else if (CurrentPID == 2)
{
PIDcontrol_test(VelocityPID);
PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange);
PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White);
}
if (PIDcycle == 0)
{
PIDBalanceDisplay.SetText("B P: " + BalancePID.P);
PIDDriveDisplay.SetText("D P: " + ArbitraryGains.P);
PIDVelocityDisplay.SetText("V P: " + VelocityPID.P);
}
else if (PIDcycle == 1)
{
PIDBalanceDisplay.SetText("B I: " + BalancePID.I);
PIDDriveDisplay.SetText("D I: " + ArbitraryGains.I);
PIDVelocityDisplay.SetText("V I: " + VelocityPID.I);
}
else if (PIDcycle == 2)
{
PIDBalanceDisplay.SetText("B D: " + BalancePID.D);
PIDDriveDisplay.SetText("D D: " + ArbitraryGains.D);
PIDVelocityDisplay.SetText("V D: " + VelocityPID.D);
}
PIDScalerDisplay.SetText("" + inc_dec);
batteryDisplay.SetText("Bat: " + Hardware.leftTalon.GetBusVoltage());
}
/* If Pigeon is connected and operation state lko09 */
if (Hardware.pidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Ready && OperateState == true)
{
manualMode = false;
}
else
{
manualMode = true;
}
/* Velocity setpoint pulled from gamepad throttle joystick */
float velocitySetpoint = velocityRamp.Set(-stick * 1000.00f);
/* Get pitch angular rate */
Hardware.pidgey.GetRawGyro(XYZ_Dps);
float pitchRate = -XYZ_Dps[1];
int leftVel = 0;
_leftSensors.GetQuadratureVelocity(out leftVel);
int rightVel = 0;
_rightSensors.GetQuadratureVelocity(out rightVel);
float tempVelocity = -(-leftVel + rightVel) * 10 * 180 / 1920; // u/100ms converted into DPS
/* Compensate for pitch angular rate when finding velocity */
float robotVelocity = ((tempVelocity) + pitchRate); //Keep in dps for easy math
if ((turn > 0.4 || turn < -0.4) && ((velocitySetpoint < 100 && velocitySetpoint > -100) || pointTurnMode))
{
//I'm doing a point turn, lock until I let go of turn
velocitySetpoint *= 0.1f;
pointTurnMode = true;
}
else
{
pointTurnMode = false;
}
turn *= (float)Math.Abs(linearlyInterpolate(robotVelocity, 0, 2000, 1, 0f));
turn *= (float)Math.Max(linearlyInterpolate((float)Math.Abs(GetPitch()), 0, 15, 1, 0f), 0);
float velocityError = velocitySetpoint - robotVelocity;
if (!trackingMode)
{
if (leftVel > 0)
{
encoderDisplayLeft.SetText("+");
}
else if (leftVel < 0)
{
encoderDisplayLeft.SetText("|");
}
else
{
encoderDisplayLeft.SetText("o");
}
if (rightVel < 0)
{
encoderDisplayRight.SetText("+");
}
else if (rightVel > 0)
{
encoderDisplayRight.SetText("|");
}
else
{
encoderDisplayRight.SetText("o");
}
}
//=============================================================================================================================================//
//=============================================================================================================================================//
Iaccum_velocity += (velocityError);
Iaccum_velocity = CTRE.Phoenix.Util.Cap(Iaccum_velocity, accummax_velocity);
float velocityDerivative = robotVelocity - lastVelocity;
lastVelocity = robotVelocity;
float pValue_vel = velocityError * VelocityPID.P;
pValue_vel = CTRE.Phoenix.Util.Cap(pValue_vel, 20);
float iValue_vel = Iaccum_velocity * VelocityPID.I / 10;
iValue_vel = CTRE.Phoenix.Util.Cap(iValue_vel, 20); //Cap I value to 20 degrees
float dValue_vel = velocityDerivative * VelocityPID.D;
angleSetpoint = -(pValue_vel + iValue_vel + dValue_vel);
angleSetpoint = CTRE.Phoenix.Util.Cap(angleSetpoint, 15);
byte[] frame2 = new byte[8];
frame2[0] = (byte)((int)(robotVelocity * 10) >> 8);
frame2[1] = (byte)((int)(robotVelocity * 10) & 0xFF);
frame2[2] = (byte)((int)(velocitySetpoint / 10));
frame2[3] = (byte)((int)(angleSetpoint * 100) >> 8);
frame2[4] = (byte)((int)(angleSetpoint * 100) & 0xFF);
frame2[5] = (byte)((int)(pValue_vel * 10));
frame2[6] = (byte)((int)(iValue_vel * 10));
frame2[7] = (byte)((int)(dValue_vel * 10));
ulong data2 = (ulong)BitConverter.ToUInt64(frame2, 0);
CTRE.Native.CAN.Send(8, data2, 8, 0);
/* Balance PID, call 5 times per outer call */
//=============================================================================================================================================//
//=============================================================================================================================================//
float pitchError = 0;
lastInnerLoopTime = loopTimer.DurationMs;
loopCounter = 0;
while (loopCounter < 5)
{
if (loopTimer.DurationMs < lastInnerLoopTime)
{
continue;
}
lastInnerLoopTime += 5;
loopCounter++;
Hardware.pidgey.GetRawGyro(XYZ_Dps);
float currentAngularRate = XYZ_Dps[1] * 0.001f; //Scaled down for easier gain control
float currentPitch = GetPitch();
float targetPitch = angleSetpoint;
pitchError = targetPitch - currentPitch;
Iaccum += pitchError;
Iaccum = CTRE.Phoenix.Util.Cap(Iaccum, accummax);
tempP = BalancePID.P * getGainTerm(robotVelocity); //Scale P term with velocity
tempI = BalancePID.I / 10;
tempD = BalancePID.D;
if (pointTurnMode)
{
//We are point turning so zero I and Scalar P
tempP = BalancePID.P;
tempI = 0;
}
float pValue = (pitchError) * tempP;
float iValue = (Iaccum) * tempI;
float dValue = (currentAngularRate) * tempD;
iValue = CTRE.Phoenix.Util.Cap(iValue, 0.3f); //Cap I to 30% of max motor output
float Output = pValue - dValue + iValue;
float outputVelocity = robotVelocity / 2000;
float eventHorizonForward;
float eventHorizonReverse;
if (outputVelocity > 0)
{
eventHorizonForward = linearlyInterpolate(outputVelocity, 0, 1, 60, 30);
eventHorizonReverse = linearlyInterpolate(outputVelocity, 0, 1, -60, -120);
}
else
{
eventHorizonForward = linearlyInterpolate(outputVelocity, 0, -1, 60, 120);
eventHorizonReverse = linearlyInterpolate(outputVelocity, 0, -1, -60, -30);
}
if (-currentPitch > eventHorizonForward || -currentPitch < eventHorizonReverse)
{
tipEnd = loopTimer.DurationMs;
}
if (loopTimer.DurationMs - tipStart > 200)
{
tippedOver = true;
}
if (loopTimer.DurationMs - tipEnd > 1000)
{
tippedOver = false;
}
if (tippedOver)
{
Output = 0;
Iaccum_velocity = 0;
Iaccum = 0;
manualMode = true;
}
/* Process output */
//=============================================================================================================================================//
//=============================================================================================================================================//
Output = CTRE.Phoenix.Util.Cap(Output, maxOutput); //cap value from [-1, 1]
if (Hardware.battery.IsLow())
{
/* Scale all drivetrain inputs to 25% if battery is low */
batteryDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red);
Output *= 0.25f;
stick *= 0.25f;
turn *= 0.25f;
}
if (manualMode == false)
{
/* In balance mode, use PI -> PID -> Output */
DrivetrainSet(Output, turn, false);
if (!trackingMode)
{
titleDisplay.SetText("Enabled");
}
if (!trackingMode)
{
outputDisplay.SetText("Out: " + Output);
}
}
else
{
/* In maual mode/disabled, use joystick -> Output */
DrivetrainSet(stick, turn, true);
if (!trackingMode)
{
titleDisplay.SetText("Disabled");
}
if (!trackingMode)
{
outputDisplay.SetText("Out: " + stick);
}
}
/* Balance CAN Frame */
byte[] frame = new byte[8];
frame[0] = (byte)((int)(pValue * 1000) >> 8);
frame[1] = (byte)((int)(pValue * 1000) & 0xFF);
frame[2] = (byte)((int)(-dValue * 100000) >> 8);
frame[3] = (byte)((int)(-dValue * 100000) & 0xFF);
frame[4] = (byte)((int)(iValue * 1000) >> 8);
frame[5] = (byte)((int)(iValue * 1000) & 0xFF);
frame[6] = (byte)((int)(Output * 1000) >> 8);
frame[7] = (byte)((int)(Output * 1000) & 0xFF);
ulong data = (ulong)BitConverter.ToUInt64(frame, 0);
CTRE.Native.CAN.Send(9, data, 8, 0);
}
if (!trackingMode)
{
pitchDisplay.SetText("P: " + GetPitch());
}
//Thread.Sleep(5);
}
}
public static void Main()
{
/* Initialize Display */
CTRE.Gadgeteer.Module.DisplayModule.LabelSprite titleDisplay, pitchDisplay, outputDisplay, PID_PDisplay, PID_IDisplay,
PID_DDisplay, PIDScalerDisplay, PIDSelectDisplay, batteryDisplay, trimDisplay;
/* State and battery display in the 1st row */
titleDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Red, 1, 1, 80, 15);
batteryDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Green, 81, 1, 79, 15);
/* Pitch and output display in the 2nd row */
pitchDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Cyan, 1, 21, 80, 15);
outputDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Cyan, 81, 21, 79, 15);
/* PID Scalar and angle Trim display in the 3rd row */
PIDScalerDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Yellow, 1, 41, 80, 15);
trimDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Blue, 81, 41, 79, 15);
/* Gain Display at the bottom */
PID_PDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 1, 61, 90, 15);
PID_IDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 1, 81, 90, 15);
PID_DDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.White, 1, 101, 90, 15);
PIDSelectDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 91, 81, 60, 15);
foreach (CTRE.Phoenix.MotorControl.CAN.TalonSRX Talon in Hardware.allTalons)
{
/* Voltage Compensation on both Talons */
Talon.ConfigVoltageCompSaturation(10.0f, kTimeout);
Talon.EnableVoltageCompensation(true);
Talon.ConfigVoltageMeasurementFilter(32, kTimeout);
Talon.ConfigNominalOutputForward(0, kTimeout);
Talon.ConfigNominalOutputReverse(0, kTimeout);
Talon.ConfigPeakOutputForward(1, kTimeout);
Talon.ConfigPeakOutputReverse(-1, kTimeout);
/* Current limiting on both Talons */
Talon.ConfigContinuousCurrentLimit(15, kTimeout); // Configured to desired amperage of current draw
Talon.ConfigPeakCurrentLimit(15, kTimeout); // Peak current limit set to 0, current limit when current has excedded continout current limit value
Talon.ConfigPeakCurrentDuration(0, kTimeout); // Current limit the moment peak current limit has been met by current limit
Talon.EnableCurrentLimit(true); // Enable current limiting
/* Change Velocity measurement paramters */
Talon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_10Ms, kTimeout);
Talon.ConfigVelocityMeasurementWindow(32, kTimeout);
/* Speed up Feedback status frame of both Talons */
Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrame.Status_2_Feedback0_, 10, kTimeout);
/* Speed up Status Frame 4, which provides information about battery */
Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_4_AinTempVbat, 10, kTimeout);
}
/* Speed up Pigeon CAN Frames that are important for the cascade PID loop to operate properly */
Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.BiasedStatus_2_Gyro, 5, kTimeout);
Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, kTimeout);
/* Locals used when Gain Scheduling within Balance loop (Inner Loop) */
float tempP = 0;
float tempI = 0;
float tempD = 0;
ServoParameters currentPID = new ServoParameters();
float[] XYZ_Dps = new float[3];
Boolean lowBattery = false;
CTRE.Phoenix.Controller.GameControllerValues gamepadValues = new CTRE.Phoenix.Controller.GameControllerValues();
int lastGamepadPOV = 0;
float angleTrim = 0;
while (true)
{
/* Check to see if gamepad is connected to enable watchdog (Motor Safety) */
if (Hardware.Gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Pull values from gamepad */
float stick = -1 * Hardware.Gamepad.GetAxis(1);
float turn = Hardware.Gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref stick); //Deadband
CTRE.Phoenix.Util.Deadband(ref turn); //Deadband
turn *= 0.50f; //Scale turn speed
Hardware.Gamepad.GetAllValues(ref gamepadValues);
if (gamepadValues.pov == 2 && lastGamepadPOV != 2)
{
angleTrim++;
}
else if (gamepadValues.pov == 6 && lastGamepadPOV != 6)
{
angleTrim--;
}
lastGamepadPOV = gamepadValues.pov;
trimDisplay.SetText("Trm: " + angleTrim);
/* Change operation state when Button 1 (X-Button) is pressed */
bool button1 = Hardware.Gamepad.GetButton(1);
if (button1 && !lastButton1)
{
/* Toggle between operation state and clear accumulated values */
OperateState = !OperateState;
Iaccum = 0;
Iaccum_velocity = 0;
}
lastButton1 = button1;
/* Offset pitch when Button 2 (A-Button) is pressed */
bool button2 = Hardware.Gamepad.GetButton(2);
if (button2 && !lastButton2)
{
/* Update current pitchoffset with new offset */
pitchoffset = 0;
pitchoffset = GetPitch();
angleTrim = 0;
}
lastButton2 = button2;
/* Cycle through current PID values [P, I, D] when Button 3 (B-Button) is pressed */
bool button3 = Hardware.Gamepad.GetButton(3);
if (button3 && !lastButton3)
{
/* Select gain to control */
PIDValue++;
if (PIDValue > 2)
{
PIDValue = 0;
}
}
lastButton3 = button3;
/* Cycle through PID sets [Soft, Hard, Velocity] when Button 4 (Y-Button) is pressed */
bool button4 = Hardware.Gamepad.GetButton(4);
if (button4 && !lastButton4)
{
PIDCycle++;
if (PIDCycle > 2)
{
PIDCycle = 0;
}
}
lastButton4 = button4;
/* Increase the DEC/INC Value for PID Gains by 10x, max of 10.000... when Button 10 (Start-Button) is pressed */
bool button10 = Hardware.Gamepad.GetButton(10);
if (button10 && !lastButton10)
{
/* Increase the increment/decrement value by x10 */
inc_dec *= 10;
if (inc_dec >= 100)
{
inc_dec = 0.001f;
}
}
lastButton10 = button10;
PIDScalerDisplay.SetText("" + inc_dec);
/* Change the highlighted PID value to inform user which PID is in control */
if (PIDValue == 0)
{
PID_PDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.White);
PID_IDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
PID_DDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
}
else if (PIDValue == 1)
{
PID_PDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
PID_IDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.White);
PID_DDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
}
else if (PIDValue == 2)
{
PID_PDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
PID_IDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Orange);
PID_DDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.White);
}
/* Selects PID set to be in control */
if (PIDCycle == 0)
{
currentPID = BalancePID;
PIDSelectDisplay.SetText("Soft");
}
else if (PIDCycle == 1)
{
currentPID = DrivePID;
PIDSelectDisplay.SetText("Hard");
}
else if (PIDCycle == 2)
{
currentPID = VelocityPID;
PIDSelectDisplay.SetText("Velocity");
}
PIDControl(currentPID);
PID_PDisplay.SetText("P: " + currentPID.P);
PID_IDisplay.SetText("I: " + currentPID.I);
PID_DDisplay.SetText("D: " + currentPID.D);
///* Output battery to Display Module */
float vBat = 0;
vBat = Hardware.leftTalon.GetBusVoltage();
batteryDisplay.SetText("Bat: " + vBat);
if (Hardware.battery.IsLow())
{
lowBattery = true;
}
else
{
lowBattery = false;
}
/* If Pigeon is connected and operation state is true, enable balance mode */
if (Hardware.pidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Ready && OperateState == true)
{
manualMode = false;
}
else
{
manualMode = true;
}
/* Velocity PI */
//=============================================================================================================================================//
//=============================================================================================================================================//
/* Get pitch angular rate */
Hardware.pidgey.GetRawGyro(XYZ_Dps);
float pitchRate = XYZ_Dps[0];
float velocityRPM = -(Hardware.leftTalon.GetSelectedSensorVelocity(0) + Hardware.rightTalon.GetSelectedSensorVelocity(0)) * 0.5f * 600 / 4096;
float velocityDPS = (velocityRPM) * 6; //RPM converted into DPS
/* Velocity setpoint pulled from gamepad throttle joystick */
float velocitySetpoint = -stick * VelocityPID.D;
/* Compensate for pitch angular rate when finding velocity */
float wheelVelocity = ((velocityDPS) + pitchRate) / 6 * (float)(System.Math.PI * 6.25f) / 12.00f / 60; //DPS converted into FPS
float velocityError = velocitySetpoint - wheelVelocity;
Iaccum_velocity += (velocityError * VelocityPID.I);
Iaccum_velocity = CTRE.Phoenix.Util.Cap(Iaccum_velocity, accummax_velocity);
float pValue_vel = velocityError * VelocityPID.P;
float iValue_vel = Iaccum_velocity;
float angleSetpoint = pValue_vel + iValue_vel;
/* Balance PID, call 4 times per outer call (Cascade PID control) */
//=============================================================================================================================================//
//=============================================================================================================================================//
for (int i = 0; i < 4; i++)
{
Hardware.pidgey.GetRawGyro(XYZ_Dps); //Get Angular rate for pitch
float currentAngularRate = XYZ_Dps[0] * 0.001f; //Scaled down for easier gain control
float currentPitch = GetPitch(); //Get Pitch
pitchDisplay.SetText("p: " + currentPitch); //Update Display
float targetPitch = angleSetpoint + angleTrim;
float pitchError = targetPitch - currentPitch;
float deadband = 5.0f;
if (currentPitch > (angleTrim - deadband) && currentPitch < (angleTrim + deadband))
{
/* Gain schedule when within 5 degrees of current angleTrim */
tempP = BalancePID.P;
tempI = BalancePID.I;
tempD = BalancePID.D;
}
else
{
tempP = DrivePID.P;
tempI = DrivePID.I;
tempD = DrivePID.D;
Iaccum = 0;
}
Iaccum += pitchError * tempI;
Iaccum = CTRE.Phoenix.Util.Cap(Iaccum, accummax);
/* Clear accumulator when within zone */
if (currentPitch > -0.5 && currentPitch < 0.5)
{
Iaccum = 0;
}
float pValue = (pitchError) * tempP;
float iValue = (Iaccum);
float dValue = (currentAngularRate) * tempD;
float Output = pValue - dValue + iValue;
/* Process output */
//=============================================================================================================================================//
//=============================================================================================================================================//
Output = CTRE.Phoenix.Util.Cap(Output, maxOutput); //cap value to [-1, 1]
if (lowBattery)
{
/* Scale all drivetrain inputs to 25% if battery is low */
batteryDisplay.SetColor(CTRE.Gadgeteer.Module.DisplayModule.Color.Red);
Output *= 0.25f;
stick *= 0.25f;
turn *= 0.25f;
}
if (manualMode == false)
{
/* In balance mode, use PI -> PID -> Output */
DrivetrainSet(Output, turn);
titleDisplay.SetText("Enabled");
outputDisplay.SetText("Out: " + Output);
}
else
{
/* In maual mode/disabled, use joystick -> Output */
DrivetrainSet(stick, turn);
titleDisplay.SetText("Disabled");
outputDisplay.SetText("Out: " + stick);
}
/* Balance CAN Frame */
byte[] frame = new byte[8];
frame[0] = (byte)((int)(pValue * 1000) >> 8);
frame[1] = (byte)((int)(pValue * 1000) & 0xFF);
frame[2] = (byte)((int)(-dValue * 100000) >> 8);
frame[3] = (byte)((int)(-dValue * 100000) & 0xFF);
frame[4] = (byte)((int)(iValue * 1000) >> 8);
frame[5] = (byte)((int)(iValue * 1000) & 0xFF);
frame[6] = (byte)((int)(Output * 1000) >> 8);
frame[7] = (byte)((int)(Output * 1000) & 0xFF);
ulong data = (ulong)BitConverter.ToUInt64(frame, 0);
CTRE.Native.CAN.Send(9, data, 8, 0);
}
/* Velocity CAN Frame */
byte[] frame2 = new byte[8];
frame2[0] = (byte)((int)(wheelVelocity * 1000) >> 8);
frame2[1] = (byte)((int)(wheelVelocity * 1000) & 0xFF);
frame2[2] = (byte)((int)(angleSetpoint * 1000) >> 8);
frame2[3] = (byte)((int)(angleSetpoint * 1000) & 0xFF);
frame2[4] = (byte)((int)(pitchRate * 100) >> 8);
frame2[5] = (byte)((int)(pitchRate * 100) & 0xFF);
frame2[6] = (byte)((int)(velocityDPS * 100) >> 8);
frame2[7] = (byte)((int)(velocityDPS * 100) & 0xFF);
ulong data2 = (ulong)BitConverter.ToUInt64(frame2, 0);
CTRE.Native.CAN.Send(8, data2, 8, 0);
Thread.Sleep(5);
}
}