added boolean for absolute angle reseting on shooter

Team334 · Mar 23, 2024 · ec5ac1d · ec5ac1d
1 parent b90c185
commit ec5ac1d
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -116,7 +116,6 @@ public RobotContainer() {
     _autonChooser = AutoBuilder.buildAutoChooser();
 
     SmartDashboard.putData("AUTON CHOOSER", _autonChooser);
-    SmartDashboard.putBoolean("AUTOAIM REACHED", false);
   }
 
   // to configure button bindings

diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java
@@ -7,6 +7,7 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.elevator.SetElevator;
@@ -52,12 +53,9 @@ private AutoAim(
     DoubleSupplier elevatorHeight,
     boolean runOnce
   ) {
-    // handleInterrupt(() -> SmartDashboard.putBoolean("AUTOAIM REACHED", true));
-
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      Commands.runOnce(() -> SmartDashboard.putBoolean("AUTOAIM REACHED", false)),
       new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
       new SetShooter(shooter, shooterAngle, runOnce),
       new SetElevator(elevator, elevatorHeight, runOnce)

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -52,7 +52,6 @@ public void periodic() {
     SmartDashboard.putNumber("ELEVATOR SETPOINT", _heightController.getSetpoint());
     SmartDashboard.putNumber("ELEVATOR HEIGHT METERS", getHeight());
     SmartDashboard.putNumber("ELEVATOR PERCENT OUTPUT", _leftMotor.get());
-    SmartDashboard.putNumber("ELEVATOR VELOCITY", getVelocity());
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -54,6 +54,8 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private boolean _holdNote = false;
 
+  private final boolean ABSOLUTE_RESET = false;
+
   /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */
   public enum ShooterState {
     SHOOT,
@@ -100,15 +102,21 @@ public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("SHOOTER SETPOINT", _angleController.getSetpoint());
     SmartDashboard.putNumber("SHOOTER ANGLE", getAngle());
-    SmartDashboard.putNumber("SHOOTER ANGLE ENCODER", _angleEncoderAbsolute.getDistance());
+    SmartDashboard.putNumber("SHOOTER ABSOLUTE ENCODER", _angleEncoderAbsolute.getDistance() - 80);
     SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
-    SmartDashboard.putNumber("SHOOTER ANGULAR VELOCITY", getAngularVelocity());
   }
 
   // for resetting the shooter's angle
   private void resetAngle() {
-    double absolutePosition = _angleEncoderAbsolute.getDistance() - 80;
-    _angleMotor.setPosition(absolutePosition * Physical.SHOOTER_ANGLE_GEAR_RATIO / 360);
+    double resetAngle;
+
+    if (ABSOLUTE_RESET) {
+      resetAngle = _angleEncoderAbsolute.getDistance() - 80;
+    } else {
+      resetAngle = 0;
+    }
+
+    _angleMotor.setPosition(resetAngle * Physical.SHOOTER_ANGLE_GEAR_RATIO / 360);
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -101,7 +101,7 @@ public Optional<double[]> getValidNTEntry() {
     double[] botpose_array = botpose_entry.getDoubleArray(new double[11]);
 
     double distance = botpose_array[9];
-    SmartDashboard.putNumber("DISTANCEZ", distance);
+    SmartDashboard.putNumber("TAG DISTANCE", distance);
     if (distance > FieldConstants.TAG_DISTANCE_THRESHOLD) return Optional.empty();
 
     return Optional.of(botpose_array);