From 0bcef472910f6bdd489cfa06c1984a12761884b5 Mon Sep 17 00:00:00 2001 From: Anirudh S Date: Thu, 10 Oct 2024 21:19:39 -0700 Subject: [PATCH] gg pre batbcode --- .../frc2024/commands/IntakeCommands.java | 2 +- .../frc2024/constants/PivotConstants.java | 35 ++++++----- .../frc2024/robot/RobotContainer.java | 15 +++-- .../team3647/frc2024/subsystems/Pivot.java | 5 +- .../frc2024/subsystems/Superstructure.java | 2 +- .../team3647/frc2024/util/RobotTracker.java | 63 ++++++++++++------- .../lib/team9442/AllianceChecker.java | 5 +- 7 files changed, 83 insertions(+), 44 deletions(-) diff --git a/src/main/java/team3647/frc2024/commands/IntakeCommands.java b/src/main/java/team3647/frc2024/commands/IntakeCommands.java index 14822ab..3ccc922 100644 --- a/src/main/java/team3647/frc2024/commands/IntakeCommands.java +++ b/src/main/java/team3647/frc2024/commands/IntakeCommands.java @@ -9,7 +9,7 @@ public class IntakeCommands { public Command intake() { - return Commands.run(() -> intake.openLoop(0.8), intake); + return Commands.run(() -> intake.openLoop(1), intake); } public Command outtake() { diff --git a/src/main/java/team3647/frc2024/constants/PivotConstants.java b/src/main/java/team3647/frc2024/constants/PivotConstants.java index d6d521d..14ff067 100644 --- a/src/main/java/team3647/frc2024/constants/PivotConstants.java +++ b/src/main/java/team3647/frc2024/constants/PivotConstants.java @@ -1,10 +1,17 @@ package team3647.frc2024.constants; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.playingwithfusion.TimeOfFlight; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; @@ -91,18 +98,18 @@ public class PivotConstants { // kMasterSpeakerMap.put(5.75, 27.7); // kMasterSpeakerMap.put(6.0, 27.4); // kMasterSpeakerMap.put(20.0, 26.5); - kMasterSpeakerMap.put(0.0, 60.0); - kMasterSpeakerMap.put(1.0, 60.0); - kMasterSpeakerMap.put(1.5, 60.0); // - kMasterSpeakerMap.put(2.0, 49.0); // + kMasterSpeakerMap.put(0.0, 60.0+1.4); + kMasterSpeakerMap.put(1.0, 60.0+1.4); + kMasterSpeakerMap.put(1.5, 60.0+1.4); // + kMasterSpeakerMap.put(2.0, 49.0+1.4); // kMasterSpeakerMap.put(2.5, 45.0); // - kMasterSpeakerMap.put(3.0, 35.0); // - kMasterSpeakerMap.put(3.5, 31.7); // - kMasterSpeakerMap.put(4.0, 30.0); // - kMasterSpeakerMap.put(4.5, 27.9); // - kMasterSpeakerMap.put(5.0, 26.5); // - kMasterSpeakerMap.put(5.5, 26.0); // - kMasterSpeakerMap.put(6.0, 23.4); // + kMasterSpeakerMap.put(3.0, 35.0+1.4); // + kMasterSpeakerMap.put(3.5, 31.7+2.2); // + kMasterSpeakerMap.put(4.0, 30.0+0.6); // + kMasterSpeakerMap.put(4.5, 27.9-1.4); // + kMasterSpeakerMap.put(5.0, 26.5+0.6); // + kMasterSpeakerMap.put(5.5, 26.0-2.2); // + kMasterSpeakerMap.put(6.0, 23.4+0.6); // kMasterSpeakerMap.put(6.52, 22.35); // kMasterSpeakerMap.put(7.04, 22.0); // kMasterSpeakerMap.put(7.4961, 20.7); // it stops working here diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java index 9380d09..b1a912e 100644 --- a/src/main/java/team3647/frc2024/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java @@ -95,7 +95,7 @@ public RobotContainer() { } private void configAllianceChecker() { - allianceChecker.registerObservers(autoCommands, autoChooser); + allianceChecker.registerObservers(autoCommands, autoChooser, tracker); } private void configureButtonBindings() { @@ -400,7 +400,12 @@ public Command getAutonomousCommand() { public final Intake intake = new Intake(IntakeConstants.kMaster, 1, 1, IntakeConstants.kNominalVoltage, 0.02); + private final Trigger isIntaking = + new Trigger(() -> mainController.leftBumper.getAsBoolean() && !DriverStation.isAutonomous()); + private final Trigger isNotIntaking = + new Trigger(() -> !isIntaking.getAsBoolean()).debounce(0.1); + public final Pivot pivot = new Pivot( PivotConstants.kMaster, @@ -415,7 +420,8 @@ public Command getAutonomousCommand() { PivotConstants.maxKG, 0.02, PivotConstants.tofBack, - PivotConstants.tofFront); + PivotConstants.tofFront, + isIntaking); private final ClimbLeft climbLeft = new ClimbLeft( @@ -472,6 +478,8 @@ public Command getAutonomousCommand() { new AprilTagPhotonVision(VisionConstants.zoom, VisionConstants.robotToZoom) .withPriority(true); + + public AllianceChecker allianceChecker = new AllianceChecker(); private final VisionController visionController = @@ -570,6 +578,5 @@ public Command getAutonomousCommand() { .debounce(0.06) .or(mainController.buttonB); - private final BooleanSupplier isIntaking = - () -> mainController.leftBumper.getAsBoolean() && !DriverStation.isAutonomous(); + } diff --git a/src/main/java/team3647/frc2024/subsystems/Pivot.java b/src/main/java/team3647/frc2024/subsystems/Pivot.java index db3f4e2..2fc1eac 100644 --- a/src/main/java/team3647/frc2024/subsystems/Pivot.java +++ b/src/main/java/team3647/frc2024/subsystems/Pivot.java @@ -18,6 +18,7 @@ public class Pivot extends TalonFXSubsystem { private final double maxAngleUnderStage; BooleanSupplier underStage; + BooleanSupplier isIntaking; private final double maxKG; @@ -39,7 +40,8 @@ public Pivot( double maxKG, double kDt, TimeOfFlight tofBack, - TimeOfFlight tofFront) { + TimeOfFlight tofFront, + BooleanSupplier isIntaking) { super(master, ticksToMetersPerSec, ticksToMeters, nominalVoltage, kDt); super.addFollower(slave, false); this.minAngle = minAngle; @@ -50,6 +52,7 @@ public Pivot( this.maxKG = maxKG; this.tofBack = tofBack; this.tofFront = tofFront; + this.isIntaking = isIntaking; master.getPosition().setUpdateFrequency(250); } diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java index e405c0c..84b8563 100644 --- a/src/main/java/team3647/frc2024/subsystems/Superstructure.java +++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java @@ -138,7 +138,7 @@ public boolean isClimbing() { public Command spinUp() { return shooterCommands.setVelocityIndep( - () -> shooterSpeedSupplierRight.getAsDouble() + 4, + () -> shooterSpeedSupplierRight.getAsDouble() + 1, () -> shooterSpeedSupplierLeft.getAsDouble()); } diff --git a/src/main/java/team3647/frc2024/util/RobotTracker.java b/src/main/java/team3647/frc2024/util/RobotTracker.java index 74d9dea..b0819d8 100644 --- a/src/main/java/team3647/frc2024/util/RobotTracker.java +++ b/src/main/java/team3647/frc2024/util/RobotTracker.java @@ -5,10 +5,14 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; + +import java.sql.Driver; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import team3647.lib.team6328.VirtualSubsystem; +import team3647.lib.team9442.AllianceChecker; import team3647.lib.team9442.AllianceObserver; public class RobotTracker extends VirtualSubsystem implements AllianceObserver { @@ -41,7 +45,34 @@ public RobotTracker( Supplier drivePose, Supplier driveSpeeds, InterpolatingDoubleTreeMap shootSpeedMap) { - this.color = Alliance.Red; + this.color = Alliance.Blue; + + + + + DriverStation.getAlliance() + .ifPresent( + (alliance) -> + this.speakerPose = + alliance == Alliance.Red + ? AllianceFlip.flipForPP(speakerPose) + : speakerPose); + + DriverStation.getAlliance() + .ifPresent( + (alliance) -> + this.ampPose = + alliance == Alliance.Red + ? AllianceFlip.flipForPP(ampPose) + : ampPose); + DriverStation.getAlliance() + .ifPresent( + (alliance) -> + this.ampPose = + alliance == Alliance.Red + ? AllianceFlip.flipForPP(feedPose) + : feedPose); + this.speakerPose = AllianceFlip.flipForPP(speakerPose, this.color == Alliance.Red); this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose; this.robotToShooter = robotToShooter; @@ -49,23 +80,7 @@ public RobotTracker( this.drivePose = drivePose; this.driveSpeeds = driveSpeeds; this.shootSpeedMap = shootSpeedMap; - - - // DriverStation.getAlliance() - // .ifPresent( - // (alliance) -> - // this.speakerPose = - // alliance == Alliance.Red - // ? AllianceFlip.flipForPP(speakerPose) - // : speakerPose); - // DriverStation.getAlliance() - // .ifPresent( - // (alliance) -> - // this.ampPose = - // alliance == Alliance.Red - // ? AllianceFlip.flipForPP(ampPose) - // : ampPose); } @Override @@ -161,10 +176,14 @@ public Pose2d getFeed() { @Override public void onAllianceFound(Alliance color) { - this.color = color; - this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose; - this.speakerPose = AllianceFlip.flipForPP(speakerPose, color == Alliance.Red); - this.feedPose = AllianceFlip.flipForPP(feedPose, color == Alliance.Red); - + // for(int i = 0; i <5; i++){ + // DriverStation.reportError("METHODRUN" + DriverStation.getAlliance().isPresent(), false); + // } + // this.color = color; + // DriverStation.reportError(color.name(), false); + // this.ampPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(ampPose) : ampPose; + // this.speakerPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(speakerPose) : speakerPose; + // DriverStation.reportError(String.valueOf(speakerPose.getX()), false); + // this.feedPose = color.equals(Alliance.Red) ? AllianceFlip.flipForPP(feedPose) : feedPose; } } diff --git a/src/main/java/team3647/lib/team9442/AllianceChecker.java b/src/main/java/team3647/lib/team9442/AllianceChecker.java index 4384932..f69be8e 100644 --- a/src/main/java/team3647/lib/team9442/AllianceChecker.java +++ b/src/main/java/team3647/lib/team9442/AllianceChecker.java @@ -6,10 +6,13 @@ import java.util.List; import java.util.Optional; +import org.littletonrobotics.junction.Logger; + public class AllianceChecker { private final List observers = new ArrayList<>(); private Optional alliance = DriverStation.getAlliance(); + public void registerObserver(AllianceObserver observer) { observers.add(observer); } @@ -22,7 +25,7 @@ public void registerObservers(AllianceObserver... addObservers) { public void periodic() { alliance = DriverStation.getAlliance(); - + alliance.ifPresent(color -> observers.forEach(observer -> observer.onAllianceFound(color))); } }