/// <summary> /// Configure the desired talon for closed-loop control. /// </summary> /// <param name="talon">The talon to configure.</param> /// <param name="type">Configure the talon for velocity or positional control.</param> /// <param name="parameters">Set the settings for the talon. Some</param> public static void ConfigureTalon(CANTalon talon, ConfigureType type, EncoderParameters parameters) { // Grab the 360 degree of the magnetic encoder's absolute position. int absolutePosition = talon.GetPulseWidthPosition() & 0xFFF; // Throw out the bits dealing with wrap-arounds. // Set the encoder's signal. talon.SetEncoderPostition(absolutePosition); // Set the sensor and direction. talon.FeedBackDevice = parameters.Device; talon.ReverseSensor(parameters.ReverseSensor); // Set the peak and nominal outputs. +12V is full throttle forward, -12V is full throttle backwards. talon.ConfigNominalOutputVoltage(+parameters.NominalVoltage, -parameters.NominalVoltage); talon.ConfigPeakOutputVoltage(+parameters.PeakVoltage, -parameters.PeakVoltage); // Set the allowed closed-loop error. talon.SetAllowableClosedLoopErr(parameters.AllowedError); // Set the PIDF gains. talon.Profile = parameters.PIDProfile; // Sets the current profile for saving the variables. talon.P = parameters.PIDFValues.kP; // Sets the proportional gain. talon.I = parameters.PIDFValues.kI; // Sets the integral gain. talon.D = parameters.PIDFValues.kD; // Sets the derivative gain. talon.F = parameters.PIDFValues.kF; // Sets the filter gain. // Switches how to configure the talon based on the 'type' parameter. switch (type) { // Run the positional config. case ConfigureType.Position: // Set the talon to be in positional control mode. talon.MotorControlMode = WPILib.Interfaces.ControlMode.Position; break; // Run the velocity config. case ConfigureType.Velocity: // Set the talon to be in speed control mode. talon.MotorControlMode = WPILib.Interfaces.ControlMode.Speed; break; } }