From 48f7fa6143c27a12af3ec289ca5739954e92e4a7 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Wed, 27 Mar 2024 22:10:38 -0400 Subject: [PATCH] moved heading pid controller for swerve auto aim into the swerve subsystem, so that auto led status can be added later --- .../java/frc/robot/commands/auto/AutoAim.java | 2 +- .../frc/robot/commands/swerve/SetHeading.java | 25 ++---------- .../subsystems/SwerveDriveSubsystem.java | 40 +++++++++++++++++-- 3 files changed, 41 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java index b3759b6..1d91524 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAim.java +++ b/src/main/java/frc/robot/commands/auto/AutoAim.java @@ -56,7 +56,7 @@ private AutoAim( new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce), new SetShooter(shooter, shooterAngle, runOnce), new SetElevator(elevator, elevatorHeight, runOnce) - // led command here + // some led command here ); } diff --git a/src/main/java/frc/robot/commands/swerve/SetHeading.java b/src/main/java/frc/robot/commands/swerve/SetHeading.java index 23b6ad5..3fb80bc 100644 --- a/src/main/java/frc/robot/commands/swerve/SetHeading.java +++ b/src/main/java/frc/robot/commands/swerve/SetHeading.java @@ -6,12 +6,8 @@ import java.util.function.DoubleSupplier; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.PID; import frc.robot.Constants.Speeds; import frc.robot.subsystems.SwerveDriveSubsystem; @@ -23,8 +19,6 @@ public class SetHeading extends Command { private DoubleSupplier _xSpeed; private DoubleSupplier _ySpeed; - private PIDController _headingController = new PIDController(PID.SWERVE_HEADING_KP, 0, PID.SWERVE_HEADING_KD); - /** * Creates a new SetHeading. * @@ -48,9 +42,6 @@ public SetHeading( _xSpeed = xSpeed; _ySpeed = ySpeed; - _headingController.setTolerance(3); - _headingController.enableContinuousInput(-180, 180); - addRequirements(_swerve); } @@ -71,19 +62,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - SmartDashboard.putData(_headingController); - - double rotationVelocity = MathUtil.clamp( - _headingController.calculate(_swerve.getHeading().getDegrees(), _heading.getAsDouble()), - -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, - Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED - ); - - _swerve.driveChassis(new ChassisSpeeds( + _swerve.setHeading( _xSpeed.getAsDouble() * Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(), _ySpeed.getAsDouble() * Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(), - rotationVelocity - )); + _heading.getAsDouble() + ); } // Called once the command ends or is interrupted. @@ -95,6 +78,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return _runOnce && _headingController.atSetpoint(); + return _runOnce && _swerve.atDesiredHeading(); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 8f49e07..a49956d 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -29,6 +30,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.PID; +import frc.robot.Constants.Speeds; import frc.robot.utils.SwerveModule; import frc.robot.utils.UtilFuncs; @@ -60,7 +63,8 @@ public class SwerveDriveSubsystem extends SubsystemBase { // private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, // BNO055.vector_type_t.VECTOR_EULER); - + private PIDController _headingController = new PIDController(PID.SWERVE_HEADING_KP, 0, PID.SWERVE_HEADING_KD); + private final AHRS _gyro = new AHRS(); private VisionSubsystem _visionSubsystem; @@ -113,9 +117,8 @@ public ChassisSpeeds getRobotRelativeSpeeds() { public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { _visionSubsystem = visionSubsystem; - // resetPose( - // new Pose2d(2.52, 5.25, Rotation2d.fromDegrees(180)) - // ); // for testing + _headingController.setTolerance(3); + _headingController.enableContinuousInput(-180, 180); // setupOrchestra(); @@ -285,6 +288,35 @@ public void driveChassis(ChassisSpeeds chassisSpeeds) { setStates(moduleStates); } + /** + * Return whether the chassis is at the last desired set heading. + */ + public boolean atDesiredHeading() { + return _headingController.atSetpoint(); + } + + /** + * Sets the chassis to a desired heading while driving. + * + * @param xSpeed X robot speed. + * @param ySpeed Y robot speed. + * + * @param heading The heading to set to. + */ + public void setHeading(double xSpeed, double ySpeed, double heading) { + double rotationVelocity = MathUtil.clamp( + _headingController.calculate(getHeading().getDegrees(), heading), + -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, + Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED + ); + + driveChassis(new ChassisSpeeds( + xSpeed, + ySpeed, + rotationVelocity + )); + } + /** * Sets the state of each SwerveModule through an array. *