03 31 changes

src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java

@@ -49,19 +49,19 @@ public double getDeviceError() {
 
     @Override
     public double getDeviceReference() {
-        return getClosedLoopReference(false).getValueAsDouble();
+        return getClosedLoopReference(true).getValueAsDouble();
     }
 
     @Override
     public double getMotorVelocity() {
-        return getVelocity(false).getValueAsDouble();
+        return getVelocity(true).getValueAsDouble();
     }
 
     @Override
-    public double getMotorPosition() {  return getPosition(false).getValueAsDouble(); }
+    public double getMotorPosition() {  return getPosition(true).getValueAsDouble(); }
 
     @Override
-    public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable(false).getValue(); }
+    public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable(true).getValue(); }
 
     @Override
     public void zeroMotorPosition() {

src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java

@@ -328,7 +328,7 @@ public IPhoenix6 getDeviceById(String subsystemName, int id) {
             if (device.id == id) {
                 // ensure the name is set
                 if (device.name == null) device.name = key;
-                return getDevInst(device, config, true);
+                return getDevInst(device, config, false);
             }
         }
         return null;

src/main/java/com/team1816/season/RobotContainer.java

@@ -35,7 +35,6 @@ public void autonomousInit() {
         superstructure.setTurretFixedAngle(0);
         superstructure.setAutoAimTurret(true);
         superstructure.setSuperstructureWantedGatekeeperState(Superstructure.WantedGatekeeperState.CLOSE);
-        superstructure.setGatekeeperAndFeederReversing(false);
         superstructure.setSuperstructureWantedIntakeState(Superstructure.WantedIntakeState.INTAKE);
         superstructure.forceAllowGatekeeperControl(true);
     }
@@ -45,10 +44,11 @@ public void teleopInit() {
         superstructure.setSuperstructureWantedSwerveState(Superstructure.WantedSwerveState.MANUAL_DRIVING);
         superstructure.setSuperstructureWantedShooterDistanceState(Superstructure.WantedShooterDistanceState.AUTOMATIC);
         superstructure.setInclineDucking(true);
+        //superstructure.setRobotPose();
         superstructure.setTurretFixedAngle(0);
         superstructure.setAutoAimTurret(true);
-        superstructure.setGatekeeperAndFeederReversing(false);
         superstructure.setSuperstructureWantedGatekeeperState(Superstructure.WantedGatekeeperState.CLOSE);
+        superstructure.forceAllowGatekeeperControl(true);
     }
 
     private void configureBindings() {
@@ -73,13 +73,6 @@ private void configureBindings() {
                 superstructure.setInclineDucking(true);
             }));
 
-        driverController.povUp().onTrue(Commands.runOnce(() ->
-                superstructure.setGatekeeperAndFeederReversing(true)
-            ))
-            .onFalse(Commands.runOnce(() ->
-                superstructure.setGatekeeperAndFeederReversing(false)
-            ));
-
         // Shooter
         driverController.a().onTrue(Commands.runOnce(() -> {
             superstructure.setSuperstructureWantedShooterDistanceState(Superstructure.WantedShooterDistanceState.PRESET_CLOSE);

src/main/java/com/team1816/season/subsystems/Feeder.java

@@ -6,6 +6,9 @@
 import com.team1816.lib.util.GreenLogger;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import java.util.Timer;
+import java.util.TimerTask;
+
 import static com.team1816.lib.Singleton.factory;
 
 public class Feeder extends SubsystemBase implements ITestableSubsystem {
@@ -16,11 +19,14 @@ public class Feeder extends SubsystemBase implements ITestableSubsystem {
 
     //MOTORS
     private final IMotor feedMotor = (IMotor)factory.getDevice(NAME, "feedMotor");
+    private final IMotor agitateMotor = (IMotor)factory.getDevice(NAME, "agitateMotor");
 
     private final DutyCycleOut feedMotorDutyCycle = new DutyCycleOut(0);
+    private final DutyCycleOut agitateMotorDutyCycle = new DutyCycleOut(0);
 
     public Feeder() {
         super();
+
         GreenLogger.periodicLog(NAME + "/Wanted State", () -> wantedState);
     }
 
@@ -32,29 +38,33 @@ public void periodic() {
 
     private void applyState() {
         feedMotor.setControl(feedMotorDutyCycle.withOutput(wantedState.getFeedMotorDutyCycle()));
+        agitateMotor.setControl(agitateMotorDutyCycle.withOutput(wantedState.getAgitateMotorDutyCycle()));
     }
 
     public void setWantedState(FeederState state) {
         wantedState = state;
     }
 
     public enum FeederState {
-        FEEDING(factory.getConstant(NAME, "feedingDutyCycle", 0)),
-        STOPPED(factory.getConstant(NAME, "stoppedDutyCycle", 0)),
-        REVERSING(factory.getConstant(NAME, "reversingDutyCycle", 0));
+        FEEDING(factory.getConstant(NAME, "feedingDutyCycle", 0),
+            factory.getConstant(NAME, "agitateForwardDutyCycle", .15)),
+        STOPPED(factory.getConstant(NAME, "stoppedDutyCycle", 0),
+            factory.getConstant(NAME, "agitateStoppedDutyCycle", 0));
 
-        private double feedMotorDutyCycle;
+        private final double feedMotorDutyCycle;
+        private final double agitateMotorDutyCycle;
 
-        FeederState(double feedMotorDutyCycle){
+        FeederState(double feedMotorDutyCycle, double agitateMotorDutyCycle) {
             this.feedMotorDutyCycle = feedMotorDutyCycle;
-        }
-
-        private void adjustFeedMotorValue(double adjustValue) {
-            this.feedMotorDutyCycle += adjustValue;
+            this.agitateMotorDutyCycle = agitateMotorDutyCycle;
         }
 
         public double getFeedMotorDutyCycle() {
             return feedMotorDutyCycle;
         }
+
+        public double getAgitateMotorDutyCycle() {
+            return agitateMotorDutyCycle;
+        }
     }
 }

src/main/java/com/team1816/season/subsystems/Gatekeeper.java

@@ -52,15 +52,13 @@ public GatekeeperState getState() {
     }
 
     public enum GatekeeperState {
-        OPEN(factory.getConstant(NAME, "topOpenVelocity", 0), 0),
-        CLOSED(factory.getConstant(NAME, "topClosedVelocity", 0), 0),
-        REVERSING(factory.getConstant(NAME, "topReversingVelocity", 0), 0);
+        OPEN(factory.getConstant(NAME, "topOpenVelocity", 0)),
+        CLOSED(factory.getConstant(NAME, "topClosedVelocity", 0));
 
-        private double topMotorValue, bottomMotorValue;
+        private double topMotorValue;
 
-        GatekeeperState(double topMotorValue, double bottomMotorValue){
+        GatekeeperState(double topMotorValue){
             this.topMotorValue = topMotorValue;
-            this.bottomMotorValue = bottomMotorValue;
         }
 
         public double getTopMotorValue() {

src/main/java/com/team1816/season/subsystems/Shooter.java

@@ -208,6 +208,10 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem {
 
     private final ShooterTableCalculator shooterTableCalculator = new ShooterTableCalculator();
 
+    private Translation2d turretTarget = Translation2d.kZero;
+    private Pose2d turretPose = Pose2d.kZero;
+    private boolean isBlueAlliance = false;
+
     public Shooter() {
         super();
         // if the turret is ghosted we can say we are calibrated because the motors will not move
@@ -264,9 +268,6 @@ public Shooter() {
         GreenLogger.periodicLog(NAME + "/incline/Ducking", () -> isInclineDucking);
         GreenLogger.periodicLog(NAME + "/incline/Angle Adjustment Degrees", () -> inclineAngleAdjustmentDegrees);
 
-        GreenLogger.periodicLog(NAME + "/turret/Field Pose", this::getCurrentTurretPose2d, Pose2d.struct);
-        GreenLogger.periodicLog(NAME + "/turret/Current Robot Relative Angle Degrees", () -> getCurrentRobotRelativeTurretRotation2d().getDegrees());
-        GreenLogger.periodicLog(NAME + "/turret/Wanted Angle Degrees", () -> wantedTurretAngleDegrees);
         GreenLogger.periodicLog(NAME + "/turret/Aimed", this::isTurretAimed);
         GreenLogger.periodicLog(NAME + "/turret/Calibrated", () -> isTurretCalibrated);
         GreenLogger.periodicLog(NAME + "/turret/Aiming in Dead Zone", () -> isTurretAimingInDeadZone);
@@ -276,6 +277,14 @@ public Shooter() {
         GreenLogger.periodicLog(NAME + "/turret/Fixed Angle Degrees", () -> turretFixedAngleDegrees);
         GreenLogger.periodicLog(NAME + "/turret/Auto Aiming Turret", () -> autoAimTurret);
         GreenLogger.periodicLog(NAME + "/turret/Angle Adjustment Degrees", () -> turretAngleAdjustmentDegrees);
+        GreenLogger.periodicLog(NAME + "/turret/Is Blue Alliance", () -> isBlueAlliance);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Turret Pose", () -> turretPose, Pose2d.struct);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Target Pose", () -> turretTarget, Translation2d.struct);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Robot Pose", () -> BaseRobotState.robotPose, Pose2d.struct);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Shooter Offset", () -> SHOOTER_OFFSET, Translation3d.struct);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Distance to Target", () -> turretPose.getTranslation().getDistance(turretTarget));
+        GreenLogger.periodicLog(NAME + "/turret/calc/Wanted Angle Degrees", () -> wantedTurretAngleDegrees);
+        GreenLogger.periodicLog(NAME + "/turret/calc/Current Angle Degrees", () -> getCurrentRobotRelativeTurretRotation2d().getDegrees());
     }
 
     @Override
@@ -317,17 +326,17 @@ public void readFromHardware() {
     }
 
     private void applyState() {
-        Translation2d target = Translation2d.kZero;
+        turretTarget = Translation2d.kZero;
         if (autoAimTurret || wantedDistanceState == ShooterDistanceState.AUTOMATIC) {
             isAutoAiming = true;
-            target = getTargetTranslation2d();
+            turretTarget = getTargetTranslation2d();
         }
         else {
             isAutoAiming = false;
         }
 
         if (autoAimTurret) {
-            aimTurretAtTarget(target);
+            aimTurretAtTarget(turretTarget);
         }
         else {
             setTurretAngle(turretFixedAngleDegrees);
@@ -338,7 +347,7 @@ private void applyState() {
                 setInclineAngle(wantedDistanceState.getInclineAngleDegrees());
                 setLaunchVelocities(wantedDistanceState.getLaunchVelocityRPS());
             }
-            case AUTOMATIC -> aimInclineAndLaunchersAtTarget(target);
+            case AUTOMATIC -> aimInclineAndLaunchersAtTarget(turretTarget);
         }
     }
 
@@ -457,7 +466,7 @@ private Translation2d getTargetTranslation2d() {
         Pose2d robotPose = BaseRobotState.robotPose;
         double robotXMeters = robotPose.getX();
         double robotYMeters = robotPose.getY();
-        boolean isBlueAlliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue;
+        isBlueAlliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue;
         boolean isInAllianceZone = isBlueAlliance
             ? robotXMeters < ROBOT_STARTING_LINE
             : robotXMeters > FlippingUtil.fieldSizeX - ROBOT_STARTING_LINE;
@@ -719,7 +728,9 @@ private Pose2d getCurrentTurretPose2d() {
 
         Pose2d robotPose2d = BaseRobotState.robotPose;
 
-        return robotPose2d.transformBy(robotToTurretTransform2d);
+        turretPose = robotPose2d.transformBy(robotToTurretTransform2d);
+
+        return turretPose;
     }
 
     /**

src/main/java/com/team1816/season/subsystems/Superstructure.java

@@ -5,6 +5,8 @@
 import com.team1816.lib.subsystems.Vision;
 import com.team1816.lib.subsystems.drivetrain.Swerve;
 import com.team1816.lib.util.GreenLogger;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class Superstructure extends BaseSuperstructure {
     private final Shooter shooter;
@@ -27,13 +29,6 @@ public class Superstructure extends BaseSuperstructure {
      */
     private boolean forceAllowGatekeeperControl = false;
 
-    /**
-     * If the gatekeeper and feeder should reverse to try to unjam. This is separate from the main
-     * state for the gatekeeper because it should take priority over any other conflicting button
-     * presses telling the gatekeeper to do something.
-     */
-    private boolean gatekeeperAndFeederReversing = false;
-
     public Superstructure(Swerve swerve, Vision vision) {
         super(swerve, vision);
         this.shooter = Singleton.CreateSubSystem(Shooter.class);
@@ -44,7 +39,6 @@ public Superstructure(Swerve swerve, Vision vision) {
         GreenLogger.periodicLog("superstructure/Wanted Super State", () -> wantedSuperState);
         GreenLogger.periodicLog("superstructure/Actual Super State", () -> actualSuperState);
         GreenLogger.periodicLog("superstructure/Force Allowing Gatekeeper Control", () -> forceAllowGatekeeperControl);
-        GreenLogger.periodicLog("superstructure/Gatekeeper and Feeder Reversing", () -> gatekeeperAndFeederReversing);
     }
 
     @Override
@@ -72,6 +66,10 @@ private void applyStates() {
         }
     }
 
+//    public void setRobotPose (){
+//        swerve.resetPose(new Pose2d(0.4318, 0.4318, new Rotation2d(0)));
+//    }
+
     public void setWantedSuperState(WantedSuperState superState) {
         this.wantedSuperState = superState;
     }
@@ -175,17 +173,6 @@ public void decreaseTurretAngleAdjustment() {
         shooter.decreaseTurretAngleAdjustment();
     }
 
-    /**
-     * Sets if the gatekeeper and feeder should reverse to try to unjam. This is separate from
-     * setting the main state for the gatekeeper because it should take priority over any other
-     * conflicting button presses telling the gatekeeper to do something.
-     *
-     * @param shouldGatekeeperAndFeederReverse If the gatekeeper and feeder should reverse.
-     */
-    public void setGatekeeperAndFeederReversing(boolean shouldGatekeeperAndFeederReverse) {
-        gatekeeperAndFeederReversing = shouldGatekeeperAndFeederReverse;
-    }
-
     /**
      * Sets the turret back into calibration mode.
      */
@@ -220,24 +207,22 @@ private void defaulting() {
                 case PRESET_AUTO_THING -> Shooter.ShooterDistanceState.PRESET_AUTO_THING;
             }
         );
-        gatekeeper.setWantedState(
-            gatekeeperAndFeederReversing
-                // If we are trying to reverse the gatekeeper to unjam, that request should always
-                // take priority.
-                ? Gatekeeper.GatekeeperState.REVERSING
-                : (
-                    // Only let fuel into the shooter if the shooter is ready, or if we are force
-                    // allowing control.
-                    shooter.isAimed() || forceAllowGatekeeperControl
-                        ? switch (wantedGatekeeperState) {
-                        case OPEN -> Gatekeeper.GatekeeperState.OPEN;
-                        case CLOSE -> Gatekeeper.GatekeeperState.CLOSED;
-                    }
-                        : Gatekeeper.GatekeeperState.CLOSED
-                )
-        );
+
+        if (shooter.isAimed() || forceAllowGatekeeperControl) {
+            // Only let fuel into the shooter if the shooter is ready, or if we are force
+            // allowing control.
+            gatekeeper.setWantedState(
+                switch (wantedGatekeeperState) {
+                    case OPEN -> Gatekeeper.GatekeeperState.OPEN;
+                    case CLOSE -> Gatekeeper.GatekeeperState.CLOSED;
+                }
+            );
+        } else {
+            gatekeeper.setWantedState(Gatekeeper.GatekeeperState.CLOSED);
+        }
+
         // Only spin up the launch motors if we are trying to shoot to save battery.
-        shooter.setSpinUpLaunchMotors(gatekeeper.getState() == Gatekeeper.GatekeeperState.OPEN);
+        shooter.setSpinUpLaunchMotors(wantedGatekeeperState == WantedGatekeeperState.OPEN);
         intake.setWantedState(
             switch (wantedIntakeState) {
                 case INTAKE -> Intake.IntakeState.INTAKE;
@@ -251,7 +236,6 @@ private void defaulting() {
             switch (gatekeeper.getState()) {
                 case OPEN -> Feeder.FeederState.FEEDING;
                 case CLOSED -> Feeder.FeederState.STOPPED;
-                case REVERSING -> Feeder.FeederState.REVERSING;
             }
         );
     }