small cleanup after testing (unused constants/imports, etc)

redshiftrobotics · Feb 18, 2024 · 1411991 · 1411991
1 parent f9f16d2
commit 1411991
Show file tree

Hide file tree

Showing 5 changed files with 3 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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;