public static void DriveFast(double desiredDistance) { AutoFunctions.ConfigureTalon(Right1, ConfigureType.Position, new EncoderParameters { AllowedError = 0, Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative, NominalVoltage = 0.0f, PeakVoltage = 12f * .7f, PIDFValues = new PIDF { kP = 0.151, kI = 0, kD = 0, kF = 0 }, ReverseSensor = false }); AutoFunctions.ConfigureTalon(Left1, ConfigureType.Position, new EncoderParameters { AllowedError = 0, Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative, NominalVoltage = 0.0f, PeakVoltage = 12f * .9f, PIDFValues = new PIDF { kP = 0.151, kI = 0, kD = 0, kF = 0 }, ReverseSensor = true }); Right1.SetEncoderPostition(0); // Reset the encoder position to 0. Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0. // Converts meters to inches. double desiredInches = 39.3701 * desiredDistance; // Converts inches to rotations. double rotations = desiredInches / 3.29; // 3.29 inches per rotation. // Gets the displacement of the motors to set. double left = /*(RobotMap.Left1 .GetEncoderPosition() / 4096 * 39.3701 / 3.29) + */ rotations; double right = /*(RobotMap.Right1.GetEncoderPosition() / 4096 * 39.3701 / 3.29) + */ rotations; // Drive with the specified distance. DriveRotations(left); SmartDashboard.PutNumber("Encoder Distance", rotations); }
/// <summary> /// Initializes the AutoHandler. /// </summary> protected override void Init() { // Instantiates AutoHandler. AutoHandler = new Handler(); //Sets the CAN Talons to rely off of the encoders for movement. AutoFunctions.ConfigureTalon(Right1, ConfigureType.Position, new EncoderParameters { AllowedError = 0, Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative, NominalVoltage = 0.0f, PeakVoltage = 12.0f, PIDFValues = new PIDF { kP = 0.151, kI = 0, kD = 0, kF = 0 }, ReverseSensor = false }); AutoFunctions.ConfigureTalon(Left1, ConfigureType.Position, new EncoderParameters { AllowedError = 0, Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative, NominalVoltage = 0.0f, PeakVoltage = 12.0f, //3.4f PIDFValues = new PIDF { kP = 0.151, kI = 0, kD = 0, kF = 0 }, ReverseSensor = true }); Right1.SetEncoderPostition(0); // Reset the encoder position to 0. Right1.ReverseOutput(true); // Reverse the direction of the motor. Right2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower; Right2.Set(Right1.DeviceId); // Configure the second right talon to be a follower of the other right talon. Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0. Left1.ReverseOutput(false); // Reverse the direction of the motor. Left1.ReverseSensor(true); Left2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower; Left2.Set(Left1.DeviceId); // Configure the second left talon to be a follower of the other left talon. NavX.Reset(); // Reset the NavX's yaw value to zero. }