From 14119912f32c816e39ad131c3b5fb1656a2c72c9 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Sat, 17 Feb 2024 17:28:22 -0800 Subject: [PATCH] small cleanup after testing (unused constants/imports, etc) --- src/main/java/frc/robot/Constants.java | 3 --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/AutoDriveTo.java | 13 +++---------- src/main/java/frc/robot/commands/AutoRotateTo.java | 7 ------- src/main/java/frc/robot/commands/Autos.java | 1 - 5 files changed, 3 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index de37f72..f43ed3b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,9 +66,6 @@ public static class DriverConstants { } public static class RobotMovementConstants { - public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 0; - public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 0; - public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001); public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f6ed3f0..cdf8fbe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,6 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.GenericHID.HIDType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java index 90f92dc..fd07c4c 100644 --- a/src/main/java/frc/robot/commands/AutoDriveTo.java +++ b/src/main/java/frc/robot/commands/AutoDriveTo.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.RobotMovementConstants; @@ -17,8 +16,6 @@ public class AutoDriveTo extends Command { private final PIDController xMovePID, yMovePID; private double initX, initY, goalX, goalY; - private double atSetpointCounter = 0; - private boolean xOnlyMode; /*** @@ -38,11 +35,13 @@ public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) { RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D); + xMovePID.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS); yMovePID = new PIDController( RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D); + yMovePID.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS); addRequirements(this.drivetrain); } @@ -69,12 +68,6 @@ public void execute() { SmartDashboard.putNumber("PoseX", position.getX()); SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees()); - if (Math.abs(x - goalX) < RobotMovementConstants.POSITION_TOLERANCE_METERS - && Math.abs(y - goalY) < RobotMovementConstants.POSITION_TOLERANCE_METERS) - atSetpointCounter += TimedRobot.kDefaultPeriod; - else - atSetpointCounter = 0; - double xSpeed = xMovePID.calculate(x, goalX); double ySpeed = yMovePID.calculate(y, goalY); @@ -99,7 +92,7 @@ public void execute() { @Override public boolean isFinished() { - return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS; + return xMovePID.atSetpoint() && yMovePID.atSetpoint(); } @Override diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java index ba7bd2d..eeef2ba 100644 --- a/src/main/java/frc/robot/commands/AutoRotateTo.java +++ b/src/main/java/frc/robot/commands/AutoRotateTo.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.RobotMovementConstants; @@ -18,7 +17,6 @@ public class AutoRotateTo extends Command { private final boolean relative; private double currentAngleGoal; - private double atSetpointCounter = 0; /*** * Command to autonomously rotate some direction @@ -63,11 +61,6 @@ public void execute() { drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed)); - if (Math.abs(currentAngle - this.currentAngleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) - atSetpointCounter += TimedRobot.kDefaultPeriod; - else - atSetpointCounter = 0; - drivetrain.updateSmartDashboard(); } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 2dd872b..2aee1e2 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import frc.robot.Constants.SwerveDrivetrainConstants; import frc.robot.subsystems.Arm;