public static void Intake(GameController GAMEPAD, StringBuilder stringBuilder)
{
if (GAMEPAD.GetButton(3))
{
//intake.Set(ControlMode.PercentOutput, 1);
//stringBuilder.Append("\t");
//stringBuilder.Append("Tilt Up");
}
if (GAMEPAD.GetButton(4))
{
intake.Set(ControlMode.PercentOutput, -1);
stringBuilder.Append("\t");
stringBuilder.Append("Intake");
}
if (!GAMEPAD.GetButton(3) && !GAMEPAD.GetButton(4))
{
intake.Set(ControlMode.PercentOutput, 0);
}
}
public static void Load(GameController GAMEPAD, StringBuilder stringBuilder)
{
if (GAMEPAD.GetButton(5))
{
loader.Set(ControlMode.PercentOutput, 1);
stringBuilder.Append("\t");
stringBuilder.Append("Load Up");
}
if (GAMEPAD.GetButton(6))
{
loader.Set(ControlMode.PercentOutput, -1);
stringBuilder.Append("\t");
stringBuilder.Append("Load Down");
}
if (!GAMEPAD.GetButton(5) && !GAMEPAD.GetButton(6))
{
loader.Set(ControlMode.PercentOutput, 0);
}
}
public static void Tilt(GameController GAMEPAD, StringBuilder stringBuilder)
{
if (GAMEPAD.GetButton(1))
{
tiltMoter.Set(ControlMode.PercentOutput, 1);
stringBuilder.Append("\t");
stringBuilder.Append("Tilt Up");
}
if (GAMEPAD.GetButton(2))
{
tiltMoter.Set(ControlMode.PercentOutput, -1);
stringBuilder.Append("\t");
stringBuilder.Append("Tilt Down");
}
if (!GAMEPAD.GetButton(1) && !GAMEPAD.GetButton(2))
{
tiltMoter.Set(ControlMode.PercentOutput, 0);
}
}
public static void Shoot(GameController GAMEPAD, StringBuilder stringBuilder)
{
double power = GAMEPAD.GetAxis(4);
Helpers.Deadband(ref power);
double shooterSpeed = System.Math.Pow(power, 2) * System.Math.Sign(power) * .5;
shooterBM.Set(ControlMode.PercentOutput, shooterSpeed);
shooterBS.Set(ControlMode.PercentOutput, -shooterSpeed);
shooterTM.Set(ControlMode.PercentOutput, -shooterSpeed);
shooterTS.Set(ControlMode.PercentOutput, -shooterSpeed);
stringBuilder.Append("\t");
stringBuilder.Append(shooterSpeed);
}
static void Drive()
{
float x = gamepad.GetAxis(1);
float z = gamepad.GetAxis(2);
DeadZone(ref x);
DeadZone(ref z);
z *= turnSpeed;
x *= driveSpeed;
float left = x + z;
float right = x - z;
right *= -1;
left = left * left * left;
right = right * right * right;
victor0.Set(ControlMode.PercentOutput, left);
victor2.Set(ControlMode.PercentOutput, right);
}
public void drivePercentage(float speed, float rotation)
{
double[] speeds = arcadeDrive(speed, rotation);
leftMaster.Set(ControlMode.PercentOutput, speeds[0]);
rightMaster.Set(ControlMode.PercentOutput, speeds[1]);
}
static void Initialize()
{
// Serial port
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
_uart.Open();
// Victor SPX Slaves
// Left Slave
victor1.Set(ControlMode.Follower, 0);
// Right Slave
victor3.Set(ControlMode.Follower, 2);
// Talon SRX Slaves
// Feeder Slave
feederL.Set(ControlMode.Follower, 0);
feederL.SetInverted(true);
// Intake Slave
intakeLft.Set(ControlMode.Follower, 2);
intakeLft.SetInverted(true);
// Hood
hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs);
hood.SetSensorPhase(false);
hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
hood.Config_kI(0, 0.0005f, kTimeoutMs);
hood.Config_kD(0, 0f, kTimeoutMs);
hood.Config_kF(0, 0f, kTimeoutMs);
/* use slot0 for closed-looping */
hood.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
hood.ConfigNominalOutputForward(0.0f, kTimeoutMs);
hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
//***********************
// MAY NEED TUNING
//***********************
// Turret
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs);
int absPos = turret.GetSelectedSensorPosition(1);
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
turret.SetSelectedSensorPosition(0, 0, kTimeoutMs);
turret.SetSensorPhase(true);
turret.Config_IntegralZone(20);
turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay
turret.Config_kI(0, 0f, kTimeoutMs);
turret.Config_kD(0, 0f, kTimeoutMs);
turret.Config_kF(0, 0f, kTimeoutMs);
// use slot0 for closed-looping
turret.SelectProfileSlot(0, 0);
// set the peak and nominal outputs, 1.0 means full
turret.ConfigNominalOutputForward(0.0f, kTimeoutMs);
turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs);
turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs);
turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
// how much error is allowed? This defaults to 0.
turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
// Shooter
shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
shooterSensorTalon.SetSensorPhase(false);
shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs);
shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7);
shooterVESC.Set(0);
pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false);
}