void initTurret()
{
/* first choose the sensor */
_turret.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_turret.SetSensorDirection(false);
_turret.ConfigEncoderCodesPerRev(4096); // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder
_turret.SetP(0, TURRET_P); /* tweak this first, a little bit of overshoot is okay */
_turret.SetI(0, TURRET_I);
_turret.SetD(0, TURRET_D);
_turret.SetF(0, TURRET_F);
/* use slot0 for closed-looping */
_turret.SelectProfileSlot(0);
/* set the peak and nominal outputs, 12V means full */
_turret.ConfigNominalOutputVoltage(MINIMUM_TURRET_VOLTAGE, -1 * MINIMUM_TURRET_VOLTAGE); //The minimum voltage that will be applied to the turret.
_turret.ConfigPeakOutputVoltage(+3.0f, -3.0f); //THe maximum voltage that will be applied to the turret.
/* how much error is allowed? This defaults to 0. */
_turret.SetAllowableClosedLoopErr(0, 0);
_turret.SetPosition(0); /* start our position at zero, this example uses relative positions */
_turret.SetVoltageRampRate(0); /* V per sec */
_turret.SetControlMode(ControlMode.kPosition);
_turret.Set(angleSetpoint);
_turret.SetEncPosition(0);
}
