From 5cb458082a36c142bb7c21d4c3d0fddcfe7d4a8a Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 21:46:56 -0700 Subject: [PATCH] Modify Turning PID - Uses a modified Velocity PID to keep robot still while rotating - Could also use a Position PID to keep robot still but should be looked into further --- src/main/java/frc4388/robot/Constants.java | 1 + src/main/java/frc4388/robot/subsystems/Drive.java | 11 ++++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c96e86..f4f8574 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -53,6 +53,7 @@ public final class Constants { public static final double TICKS_PER_MOTOR_REV = 2048; public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5; public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; /* Ratio Calculation */ public static final double TICK_TIME_TO_SECONDS = 0.1; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 8c9e346..e1dd0ee 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -329,12 +329,13 @@ public class Drive extends SubsystemBase { talon.set(TalonFXControlMode.MotionMagic, targetPos); } + /** + * Runs a Turning PID to rotate a to a target angle + * @param targetAngle target angle in degrees + */ public void runTurningPID(double targetAngle){ - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, 0, DemandType.AuxPID, targetAngle); - //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - //m_rightFrontMotor.set(DemandType.AuxPID, 0); + double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV; + runVelocityPID(0, targetGyro); } public double getGyroYaw() {