minor final cleanup

redshiftrobotics · Mar 10, 2024 · 47fe8b0 · 47fe8b0
1 parent ea99282
commit 47fe8b0
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 17 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,7 +13,6 @@
 import frc.robot.commands.HangControl;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.SetLightstripColorFor;
-import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.LightStrip;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;

diff --git a/src/main/java/frc/robot/commands/ArmRotateTo.java b/src/main/java/frc/robot/commands/ArmRotateTo.java
@@ -32,7 +32,7 @@ public ArmRotateTo(Arm arm, double degrees, double toleranceAngle) {
 
 	@Override
 	public void initialize() {
-		arm.setSetpoint(setpoint);
+		arm.setSetpoint(setpoint, toleranceAngle);
 	}
 
 	@Override

diff --git a/src/main/java/frc/robot/commands/HangControl.java b/src/main/java/frc/robot/commands/HangControl.java
@@ -3,7 +3,6 @@
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.hang.Hang;
 
@@ -17,7 +16,6 @@ public class HangControl extends Command {
 	public HangControl(Hang hanger, Supplier<Double> speedSupplier) {
 		this.hanger = hanger;
 		this.speedSupplier = speedSupplier;
-
 		addRequirements(hanger);
 	}
 
@@ -29,20 +27,23 @@ public void initialize() {
 	@Override
 	public void execute() {
 
-		double speed = speedSupplier.get();
-		// SmartDashboard.putNumber(hanger.getName()+"hs", speed);
-
-		// double currentTime = System.currentTimeMillis();
+		boolean useMagneticLimitSwitches = false;
 
-		// if (!hanger.isAtBottom()) {
-		// 	timeToBeginBlock = null;
-		// } else if (timeToBeginBlock == null) {
-		// 	timeToBeginBlock = currentTime + MILLIS_TILL_BLOCK;
-		// }
+		double speed = speedSupplier.get();
 
-		// if (timeToBeginBlock != null && currentTime >= timeToBeginBlock) {
-		// 	speed = 0;
-		// }
+		if (useMagneticLimitSwitches) {
+			double currentTime = System.currentTimeMillis();
+
+			if (!hanger.isAtBottom()) {
+				timeToBeginBlock = null;
+			} else if (timeToBeginBlock == null) {
+				timeToBeginBlock = currentTime + MILLIS_TILL_BLOCK;
+			}
+
+			if (timeToBeginBlock != null && currentTime >= timeToBeginBlock) {
+				speed = 0;
+			}
+		}
 
 		hanger.setSpeed(speed);
 	}

diff --git a/src/main/java/frc/robot/subsystems/hang/RealHang.java b/src/main/java/frc/robot/subsystems/hang/RealHang.java
@@ -31,7 +31,6 @@ public RealHang(int motorID, boolean motorIsInverted, int rightLimitSwitchId) {
 	@Override
 	public void periodic() {
 		SmartDashboard.putString(name + "Hang", motor.get() + "p" + (isAtBottom() ? " - Down" : ""));
-		// SmartDashboard.putBoolean(name + "Switch", isAtBottom());
 	}
 
 	@Override