//DriveBase
public static TalonSRX CreateDriveBaseTalon(int CAN_ID, bool inverted = false)
{
TalonSRX talon = new TalonSRX(CAN_ID);
const int MAX_CURRENT = 10;
talon.ConfigContinuousCurrentLimit(MAX_CURRENT);
talon.ConfigPeakCurrentLimit(2 * MAX_CURRENT);
talon.ConfigPeakCurrentDuration(0);
talon.SetInverted(inverted);
talon.SetNeutralMode(NeutralMode.Brake);
return(talon);
}
//Excavation
public static TalonSRX CreateLinearActuator(int CAN_ID)
{
TalonSRX talon = new TalonSRX(CAN_ID);
talon.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
talon.ConfigFeedbackNotContinuous(true, 100);
talon.SetInverted(true);
talon.SetSensorPhase(true);
talon.ConfigPeakCurrentLimit(6);
talon.ConfigPeakCurrentDuration(150);
talon.ConfigContinuousCurrentLimit(3);
// talon.SetStatusFramePeriod(StatusFrame.Status_1_General_, 25); // Sets encoder value feedback rate -TODO
return(talon);
}
public Cannon()
{
angleMotor.SetNeutralMode(NeutralMode.Brake);
angleMotor.ConfigPeakOutputForward(0.5f);
angleMotor.ConfigPeakOutputReverse(-0.5f);
angleMotor.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
angleMotor.ConfigSelectedFeedbackCoefficient(1.0f);
angleMotor.Config_kP(0.001f);
revolverMotor.SetInverted(true);
revolverMotor.SetNeutralMode(NeutralMode.Brake);
revolverMotor.ConfigPeakOutputForward(0.5f);
revolverMotor.ConfigPeakOutputReverse(0.5f);
revolverMotor.ConfigContinuousCurrentLimit(20);
revolverMotor.ConfigPeakCurrentLimit(0);
revolverMotor.ConfigPeakCurrentDuration(0);
revolverMotor.EnableCurrentLimit(true);
_setpoint = Angle;
}
public static void Main()
{
/* Disable drivetrain/motors */
_rightTalon.Set(ControlMode.PercentOutput, 0);
_leftTalon.Set(ControlMode.PercentOutput, 0);
#if (fourWheeled)
_rightFollower.Follow(_rightTalon);
_leftFollower.Follow(_leftTalon);
#endif
/* Configure output and sensor direction */
_rightTalon.SetInverted(true);
_leftTalon.SetInverted(false);
#if (fourWheeled)
_rightFollower.SetInverted(true);
_leftFollower.SetInverted(false);
#endif
/* Mode print */
Debug.Print("This is arcade drive using Arbitrary Feedforward");
bool Btn1 = false;
bool Btn2 = false;
bool Btn3 = false;
bool Btn4 = false;
bool Btn10 = false;
bool VoltageComp = false;
bool CurrentLimit = false;
bool NeutralState = false;
bool RampRate = false;
bool FirstCall = true;
while (true)
{
/* Enable motor controllers if gamepad connected */
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Gamepad Stick Control */
float forward = -1 * _gamepad.GetAxis(1);
float turn = 1 * _gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref forward);
CTRE.Phoenix.Util.Deadband(ref turn);
turn *= 0.5f; //Scaled down for safety
forward *= 0.75f;
bool btn1 = _gamepad.GetButton(1);
bool btn2 = _gamepad.GetButton(2);
bool btn3 = _gamepad.GetButton(3);
bool btn4 = _gamepad.GetButton(4);
bool btn10 = _gamepad.GetButton(10);
if (btn1 && !Btn1)
{
VoltageComp = !VoltageComp;
FirstCall = true;
}
else if (btn2 && !Btn2)
{
CurrentLimit = !CurrentLimit;
FirstCall = true;
}
else if (btn3 && !Btn3)
{
NeutralState = !NeutralState;
FirstCall = true;
}
else if (btn4 && !Btn4)
{
RampRate = !RampRate;
FirstCall = true;
}
else if (btn10 && !Btn10)
{
VoltageComp = false;
CurrentLimit = false;
NeutralState = false;
RampRate = false;
FirstCall = true;
}
Btn1 = btn1;
Btn2 = btn2;
Btn3 = btn3;
Btn4 = btn4;
Btn10 = btn10;
if (VoltageComp)
{
_rightTalon.ConfigVoltageCompSaturation(10, 10);
_rightTalon.ConfigVoltageMeasurementFilter(16, 10);
_rightTalon.EnableVoltageCompensation(true);
_leftTalon.ConfigVoltageCompSaturation(10, 10);
_leftTalon.ConfigVoltageMeasurementFilter(16, 10);
_leftTalon.EnableVoltageCompensation(true);
if (FirstCall)
{
Debug.Print("Voltage Compensation: On");
}
}
else
{
_rightTalon.EnableVoltageCompensation(false);
_leftTalon.EnableVoltageCompensation(false);
if (FirstCall)
{
Debug.Print("Voltage Compensation: Off");
}
}
if (CurrentLimit)
{
_rightTalon.ConfigContinuousCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentDuration(0, 10);
_rightTalon.EnableCurrentLimit(true);
_leftTalon.ConfigContinuousCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentDuration(0, 10);
_leftTalon.EnableCurrentLimit(true);
if (FirstCall)
{
Debug.Print("Current Limit: On");
}
}
else
{
_rightTalon.EnableCurrentLimit(false);
_leftTalon.EnableCurrentLimit(false);
if (FirstCall)
{
Debug.Print("Current Limit: Off");
}
}
if (NeutralState)
{
_rightTalon.SetNeutralMode(NeutralMode.Coast);
_leftTalon.SetNeutralMode(NeutralMode.Coast);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Coast);
_leftFollower.SetNeutralMode(NeutralMode.Coast);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Coast");
}
}
else
{
_rightTalon.SetNeutralMode(NeutralMode.Brake);
_leftTalon.SetNeutralMode(NeutralMode.Brake);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Brake);
_leftFollower.SetNeutralMode(NeutralMode.Brake);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Brake");
}
}
if (RampRate)
{
_rightTalon.ConfigOpenloopRamp(3, 0);
_leftTalon.ConfigOpenloopRamp(3, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: On, 3 Seconds");
}
}
else
{
_rightTalon.ConfigOpenloopRamp(0.0f, 0);
_leftTalon.ConfigOpenloopRamp(0.0f, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: Off, 0 Seconds");
}
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
_rightTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, -turn);
_leftTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, +turn);
if (FirstCall)
{
Debug.Print("");
}
FirstCall = false;
Thread.Sleep(5);
}
}
/**************************************************************************************************************************************************/
public static void Main()
{
while (true)
{
CTRE.Phoenix.Watchdog.Feed();
// From PDF - if controller is connected enable HERO Board
if (Joy.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
/* feed watchdog to keep Talon's enabled */
CTRE.Phoenix.Watchdog.Feed();
}
else
{
stringBuilder.Append("\n");
stringBuilder.Append("WARNING: Watchdog not fed");
}
// Update indicators
//connection_pin.Write(Joy.GetConnectionStatus() == UsbDeviceConnection.Connected);
//Current Limits
backRight.ConfigContinuousCurrentLimit(40, 10);
backRight.ConfigPeakCurrentLimit(60, 10);
backRight.ConfigPeakCurrentDuration(400, 10);
backRight.EnableCurrentLimit(true);
backLeft.ConfigContinuousCurrentLimit(40, 10);
backLeft.ConfigPeakCurrentLimit(60, 10);
backLeft.ConfigPeakCurrentDuration(400, 10);
backLeft.EnableCurrentLimit(true);
frontRight.ConfigContinuousCurrentLimit(40, 10);
frontRight.ConfigPeakCurrentLimit(60, 10);
frontRight.ConfigPeakCurrentDuration(400, 10);
frontRight.EnableCurrentLimit(true);
frontLeft.ConfigContinuousCurrentLimit(40, 10);
frontLeft.ConfigPeakCurrentLimit(60, 10);
frontLeft.ConfigPeakCurrentDuration(400, 10);
frontLeft.EnableCurrentLimit(true);
/*// Low battery
* if (pdp.GetVoltage() < 10) {
* low_battery_pin.Write(true);
* }
*
* else {
* low_battery_pin.Write(false);
* }
*
* // Fix
* power_pin.Write(true);
*
* // Brownout
* if (pdp.GetVoltage() < 8) {
* brownout_pin.Write(true);
* }
*
* else {
* brownout_pin.Write(false);
* }
*
* // Fix
* current_pin.Write(pdp.GetChannelCurrent(1) > 30);*/
Drive();
Debug.Print(stringBuilder.ToString());
stringBuilder.Clear();
// Run this task every 20ms
Thread.Sleep(20);
}
}
