/** PID gains setter */
public static void PIDControl(ServoParameters PIDtoControl)
{
bool button5 = Hardware.Gamepad.GetButton(5);
bool button6 = Hardware.Gamepad.GetButton(6);
if (button6 && !lastButton6)
{
/* Increment PID value */
if (PIDValue == 0)
{
PIDtoControl.P += inc_dec;
}
else if (PIDValue == 1)
{
PIDtoControl.I += inc_dec;
}
else if (PIDValue == 2)
{
PIDtoControl.D += inc_dec;
}
}
else if (button5 && !lastButton5)
{
/* Decrement PID value */
if (PIDValue == 0)
{
PIDtoControl.P -= inc_dec;
}
else if (PIDValue == 1)
{
PIDtoControl.I -= inc_dec;
}
else if (PIDValue == 2)
{
PIDtoControl.D -= inc_dec;
}
}
lastButton5 = button5;
lastButton6 = button6;
/* Force PID gains to be positive */
if (PIDtoControl.P <= 0)
{
PIDtoControl.P = 0;
}
if (PIDtoControl.I <= 0)
{
PIDtoControl.I = 0;
}
if (PIDtoControl.D <= 0)
{
PIDtoControl.D = 0;
}
}
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);
}
}