void ResetTargetPosition()
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
_talon.SetSelectedSensorPosition(_targetPosition, 0); //Sets current and desired positions to be equal so we don't move unexpectedly at startup
Thread.Sleep(100); //wait to make sure SetPosition takes effect
}
/**
* Setup all of the configuration parameters.
*/
public void SetupConfig()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* specify sensor characteristics */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
_talon.SetSensorPhase(false); /* make sure positive motor output means sensor moves in position direction */
/* brake or coast during neutral */
_talon.SetNeutralMode(NeutralMode.Brake);
/* closed-loop and motion-magic parameters */
_talon.Config_kF(kSlotIdx, 0.1153f, kTimeoutMs); // 8874 native sensor units per 100ms at full motor output (+1023)
_talon.Config_kP(kSlotIdx, 2.00f, kTimeoutMs);
_talon.Config_kI(kSlotIdx, 0f, kTimeoutMs);
_talon.Config_kD(kSlotIdx, 20f, kTimeoutMs);
_talon.Config_IntegralZone(kSlotIdx, 0, kTimeoutMs);
_talon.SelectProfileSlot(kSlotIdx, 0); /* select this slot */
_talon.ConfigNominalOutputForward(0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
_talon.ConfigMotionCruiseVelocity(8000, kTimeoutMs); // 8000 native units
_talon.ConfigMotionAcceleration(16000, kTimeoutMs); // 16000 native units per sec, (0.5s to reach cruise velocity).
/* Home the relative sensor,
* alternatively you can throttle until limit switch,
* use an absolute signal like CtreMagEncoder_Absolute or analog sensor.
*/
_talon.SetSelectedSensorPosition(0, kTimeoutMs);
}
/**
* Zero the sensor and zero the throttle.
*/
void ZeroSensorAndThrottle()
{
_talon.SetSelectedSensorPosition(0, kTimeoutMs); /* start our position at zero, this example uses relative positions */
_targetPosition = 0;
/* zero throttle */
_talon.Set(ControlMode.PercentOutput, 0);
Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */
}
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);
}
