From 22cddc84444bc669883179bd92b27d587dc1a5ec Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Fri, 2 Feb 2024 19:49:15 -0500 Subject: [PATCH] add updated drive state and fix relevant commands + fix 0 pid --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 11 ++++ .../java/frc/robot/commands/AimAtTarget.java | 14 ++++- .../java/frc/robot/commands/ArcadeDrive.java | 9 ++-- .../frc/robot/subsystems/drive/Drive.java | 51 ++++++++++++++----- 5 files changed, 66 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6918da7..3c8ea63 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,7 @@ */ public final class Constants { - public static enum driveTrainState { + public static enum DriveTrainState { /*Manual control */ MANUAL, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4771a28..b91e00a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,6 +88,17 @@ public RobotContainer() { gyroSupplier, simulatedPoseSupplier); + // add suppliers for drive + drive.setCurrentHeadingSupplier( + () -> { + return nav.getCurrentHeading(); + }); + + drive.setTargetHeadingSupplier( + () -> { + return nav.getTargetHeading(); + }); + // Set up auto routines // Set up feedforward characterization diff --git a/src/main/java/frc/robot/commands/AimAtTarget.java b/src/main/java/frc/robot/commands/AimAtTarget.java index e26dcb5..5c13e52 100644 --- a/src/main/java/frc/robot/commands/AimAtTarget.java +++ b/src/main/java/frc/robot/commands/AimAtTarget.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveTrainState; import frc.robot.subsystems.Navigation; import frc.robot.subsystems.drive.Drive; @@ -20,10 +21,19 @@ public AimAtTarget(Drive driveSubsystem, Navigation navSubsystem, int targetId) @Override public void initialize() { navSubsystem.setDesiredTarget(targetId); + driveSubsystem.setDriveState(DriveTrainState.AIM); } @Override - public void execute() { - driveSubsystem.aim(navSubsystem.getCurrentHeading(), navSubsystem.getTargetHeading()); + public void execute() {} + + @Override + public void end(boolean interrupted) { + driveSubsystem.setDriveState(DriveTrainState.MANUAL); + } + + @Override + public boolean isFinished() { + return Math.abs(navSubsystem.getCurrentHeading() - navSubsystem.getTargetHeading()) < 1e-5; } } diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index d0b0207..9a4f911 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -1,7 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.driveTrainState; +import frc.robot.Constants.DriveTrainState; import frc.robot.subsystems.drive.Drive; import java.util.function.DoubleSupplier; @@ -20,12 +20,13 @@ public ArcadeDrive(Drive d, DoubleSupplier xPcent, DoubleSupplier yPcent) { } @Override - public void initialize() {} + public void initialize() { + drivesub.setDriveState(DriveTrainState.MANUAL); + } @Override public void execute() { - drivesub.setArcadeDrive( - xPercent.getAsDouble(), yPercent.getAsDouble(), driveTrainState.MANUAL); + drivesub.setArcadeDrive(xPercent.getAsDouble(), yPercent.getAsDouble()); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 81696e1..d478dcc 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -6,24 +6,35 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Constants.driveTrainState; +import frc.robot.Constants.DriveTrainState; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { private final DriveIO io; - private driveTrainState mode; + private DriveTrainState mode; + private DoubleSupplier targetHeading; + private DoubleSupplier currentHeading; private final DriveIOInputsAutoLogged inputs = new DriveIOInputsAutoLogged(); - private PIDController thetaController = new PIDController(0, 0, 0); // placeholders + private PIDController thetaController = new PIDController(0.05, 0.05, 0.05); // placeholders private double forward; private double rotation; public Drive(DriveIO io) { this.io = io; - this.mode = driveTrainState.MANUAL; + this.targetHeading = + () -> { + return 0.0; + }; + this.currentHeading = + () -> { + return 0.0; + }; + this.mode = DriveTrainState.MANUAL; } @Override @@ -33,10 +44,22 @@ public void periodic() { controlDriveTrain(); } - public void setArcadeDrive(double forward, double rotation, driveTrainState mode) { + public void setDriveState(DriveTrainState state) { + mode = state; + } + + public void setCurrentHeadingSupplier(DoubleSupplier currentHeading) { + this.currentHeading = currentHeading; + } + + public void setTargetHeadingSupplier(DoubleSupplier targetHeading) { + this.targetHeading = targetHeading; + } + + public void setArcadeDrive(double forward, double rotation) { this.forward = forward; this.rotation = rotation; - this.mode = mode; + mode = DriveTrainState.MANUAL; } /** Run open loop based on stick positions. */ @@ -50,10 +73,14 @@ public void stop() { io.setVoltage(0.0, 0.0); } - public void aim(double targetHeading, double currentHeading) { + public void aim() { this.forward = 0; - this.rotation = thetaController.calculate(currentHeading, targetHeading); - this.mode = driveTrainState.AIM; + this.rotation = + thetaController.calculate( + currentHeading.getAsDouble(), targetHeading.getAsDouble()); + if (Math.abs(currentHeading.getAsDouble() - targetHeading.getAsDouble()) < 1e-5) { + mode = DriveTrainState.MANUAL; + } } public Rotation2d getGyroRotation2d() { @@ -88,17 +115,13 @@ public Pose2d getSimulatedPose() { return inputs.simulatedPose; } - public void updateMode(driveTrainState mode) { - this.mode = mode; - } - public void controlDriveTrain() { switch (mode) { case MANUAL: this.driveArcade(forward, rotation); break; case AIM: - this.driveArcade(forward, rotation); + this.aim(); break; default: this.stop();