From eb38a86415a03056edd60e5364cb3adce1ff3e44 Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Wed, 6 Apr 2022 20:26:03 -0700 Subject: [PATCH] Temporarily stop using PID for robot heading --- .../java/frc/robot/drive/DriveSubsystem.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index 4838f7f..adfee5b 100644 --- a/src/main/java/frc/robot/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/drive/DriveSubsystem.java @@ -70,10 +70,21 @@ public class DriveSubsystem extends SubsystemBase { MAX_VELOCITY = Math.abs(maxChassisSpeedsForward.vxMetersPerSecond); MAX_ANGULAR_VELOCITY = Math.abs(maxChassisSpeedsSpinning.omegaRadiansPerSecond); + System.out.println("maxChassisSpeedsSpinning:"); + System.out.println(maxChassisSpeedsSpinning); + MAX_ACCELERATION = Wheel.MAX_ACCELERATION; // TODO: This is the wheel's angular acceleration, do we need a different value for the robot // itself? MAX_ANGULAR_ACCELERATION = Wheel.MAX_ANGULAR_ACCELERATION; + System.out.println("MAX_VELOCITY"); + System.out.println(MAX_VELOCITY); + System.out.println("MAX_ANGULAR_VELOCITY"); + System.out.println(MAX_ANGULAR_VELOCITY); + System.out.println("MAX_ACCELERATION"); + System.out.println(MAX_ACCELERATION); + System.out.println("MAX_ANGULAR_ACCELERATION"); + System.out.println(MAX_ANGULAR_ACCELERATION); } private static final TrapezoidProfile.Constraints MAX_ROTATION = @@ -85,7 +96,7 @@ public class DriveSubsystem extends SubsystemBase { public final TrajectoryConfig trajectoryConfig; private final ProfiledPIDController thetaController = - new ProfiledPIDController(1, 0, 0, MAX_ROTATION, Constants.PERIOD_SECONDS); + new ProfiledPIDController(2, 0.3, 0.2, MAX_ROTATION, Constants.PERIOD_SECONDS); // Used for following trajectories public final HolonomicDriveController driveController = @@ -151,7 +162,8 @@ private void driveTeleop( ChassisSpeeds.fromFieldRelativeSpeeds( yPercentage * MAX_VELOCITY, -xPercentage * MAX_VELOCITY, - thetaControllerVelocity, + // TODO: This ignores the PID output + thetaPercentage * MAX_ANGULAR_VELOCITY, robotHeading); // TODO: Stop logging this after debugging is finished