/* Initialize Motors */
static void InitMotors()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_Talon.ConfigFactoryDefault();
_victor.ConfigFactoryDefault();
/* Select Sensor */
_Talon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.QuadEncoder, 0, kTimeoutMs);
_victor.ConfigRemoteFeedbackFilter(_Talon.GetDeviceID(), CTRE.Phoenix.MotorControl.RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, 0, kTimeoutMs);
_victor.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.RemoteFeedbackDevice.RemoteFeedbackDevice_RemoteSensor0, 0, kTimeoutMs);
_Talon.SetInverted(true);
_victor.SetInverted(true);
_Talon.SetSensorPhase(true);
_victor.SetSensorPhase(true);
/* Select Neutral Mode */
_Talon.SetNeutralMode(CTRE.Phoenix.MotorControl.NeutralMode.Brake);
_victor.SetNeutralMode(CTRE.Phoenix.MotorControl.NeutralMode.Brake);
/* Closed loop / Motion Magic Parameters */
_Talon.Config_kF(kSlotIdx, kF, kTimeoutMs);
_Talon.Config_kP(kSlotIdx, kP, kTimeoutMs);
_Talon.Config_kI(kSlotIdx, kI, kTimeoutMs);
_Talon.Config_kD(kSlotIdx, kD, kTimeoutMs);
_Talon.Config_IntegralZone(kSlotIdx, kIZone, kTimeoutMs);
_Talon.SelectProfileSlot(kSlotIdx, 0);
_Talon.ConfigNominalOutputForward(kNominalOuput, kTimeoutMs);
_Talon.ConfigNominalOutputReverse(-(kNominalOuput), kTimeoutMs);
_Talon.ConfigPeakOutputForward(kPeakOutput, kTimeoutMs);
_Talon.ConfigPeakOutputReverse(-(kPeakOutput), kTimeoutMs);
_Talon.ConfigMotionCruiseVelocity(kMotionVelocity, kTimeoutMs);
_Talon.ConfigMotionAcceleration(kMotionAcceleration, kTimeoutMs);
_Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_10_Targets, 10, kTimeoutMs);
_Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 10, kTimeoutMs);
/* Closed loop / Motion Magic Parameters */
_victor.Config_kF(kSlotIdx, kF, kTimeoutMs);
_victor.Config_kP(kSlotIdx, kP, kTimeoutMs);
_victor.Config_kI(kSlotIdx, kI, kTimeoutMs);
_victor.Config_kD(kSlotIdx, kD / 2, kTimeoutMs);
_victor.Config_IntegralZone(kSlotIdx, kIZone, kTimeoutMs);
_victor.SelectProfileSlot(kSlotIdx, 0);
_victor.ConfigNominalOutputForward(kNominalOuput, kTimeoutMs);
_victor.ConfigNominalOutputReverse(-(kNominalOuput), kTimeoutMs);
_victor.ConfigPeakOutputForward(kPeakOutput, kTimeoutMs);
_victor.ConfigPeakOutputReverse(-(kPeakOutput), kTimeoutMs);
_victor.ConfigMotionCruiseVelocity(kMotionVelocity, kTimeoutMs);
_victor.ConfigMotionAcceleration(kMotionAcceleration, kTimeoutMs);
_victor.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_10_Targets, 10, kTimeoutMs);
/* Set current position to 0, can be changed with button 4 */
_victor.SetSelectedSensorPosition(0, 0, kTimeoutMs);
_Talon.SetSelectedSensorPosition(0, 0, kTimeoutMs);
}
