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() {