/* 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); }