From 64d7b50b1e673b0d4aa572858e77de486bedadfa Mon Sep 17 00:00:00 2001 From: kennyseybold <42521401+kennyseybold@users.noreply.github.com> Date: Sun, 7 Apr 2024 20:56:00 -0400 Subject: [PATCH] final changes --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 10 +++++----- ...justCommand.java => TrapClimbCommand.java} | 6 +++--- .../commands/robotState/ClimbCommand.java | 3 ++- .../robotState/ClimbTrapDecendCommand.java | 3 ++- .../robotState/FullTrapClimbCommand.java | 3 ++- .../frc/robot/constants/AutonConstants.java | 2 +- .../frc/robot/constants/ClimbConstants.java | 4 ++-- .../frc/robot/constants/DriveConstants.java | 4 ++-- .../frc/robot/constants/LedConstants.java | 4 ++-- .../robot/constants/MagazineConstants.java | 2 +- .../frc/robot/constants/RobotConstants.java | 2 +- .../robot/constants/RobotStateConstants.java | 2 +- .../constants/SuperStructureConstants.java | 2 +- .../frc/robot/constants/VisionConstants.java | 6 +++--- .../frc/robot/subsystems/auto/AutoSwitch.java | 16 ++++++++++++++++ .../robotState/RobotStateSubsystem.java | 19 ++++++++++++++----- .../robot/subsystems/shooter/ShooterIOFX.java | 2 +- 18 files changed, 61 insertions(+), 33 deletions(-) rename src/main/java/frc/robot/commands/climb/{TrapClimbAdjustCommand.java => TrapClimbCommand.java} (73%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 558d96a7..edc278fe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -81,13 +81,11 @@ public void robotInit() { m_robotContainer.configureTelemetry(); // m_robotContainer.configurePitDashboard(); } - m_robotContainer.enableDeadeye(); Shuffleboard.getTab("Match") .add(new ToggleAllianceColorCommand(m_robotContainer)) .withSize(1, 1) .withPosition(2, 0); logger.info("robotinit"); - m_robotContainer.enableDeadeye(); } @Override @@ -127,6 +125,7 @@ public void disabledExit() {} @Override public void autonomousInit() { + m_robotContainer.enableDeadeye(); logger.info("Auto Init"); m_robotContainer.setIsAuto(true); // if (!m_robotContainer.hasElbowZeroed()) m_robotContainer.zeroElbow(); @@ -148,6 +147,7 @@ public void autonomousExit() { @Override public void teleopInit() { + m_robotContainer.enableDeadeye(); logger.info("Teleop Init | Match #{}", DriverStation.getMatchNumber()); m_robotContainer.setIsAuto(false); if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c87e6455..cb585607 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,7 @@ import frc.robot.commands.climb.JogClimbClosedLoopCommand; import frc.robot.commands.climb.ToggleRatchetCommand; import frc.robot.commands.climb.ToggleTrapBarPosCommand; -import frc.robot.commands.climb.TrapClimbAdjustCommand; +import frc.robot.commands.climb.TrapClimbCommand; import frc.robot.commands.climb.ZeroClimbCommand; import frc.robot.commands.drive.DriveAutonCommand; import frc.robot.commands.drive.DriveTeleopCommand; @@ -62,7 +62,6 @@ import frc.robot.commands.robotState.FeedCommand; import frc.robot.commands.robotState.FullTrapClimbCommand; import frc.robot.commands.robotState.IntakeCommand; -import frc.robot.commands.robotState.MovingVisionShootCommand; import frc.robot.commands.robotState.OperatorRumbleCommand; import frc.robot.commands.robotState.PodiumCommand; import frc.robot.commands.robotState.PositionShootCommand; @@ -76,6 +75,7 @@ import frc.robot.commands.robotState.TuningOffCommand; import frc.robot.commands.robotState.TuningShootCommand; import frc.robot.commands.robotState.UpdateElbowOffsetCommand; +import frc.robot.commands.robotState.VisionShootCommand; import frc.robot.commands.wrist.ClosedLoopWristCommand; import frc.robot.commands.wrist.OpenLoopWristCommand; import frc.robot.commands.wrist.WriteWristToStowCommand; @@ -392,7 +392,7 @@ public void configurePitDashboard() { .withPosition(0, 0); Shuffleboard.getTab("Pit") - .add("Elbow Pos Zero ", new ClosedLoopElbowCommand(elbowSubsystem, 0.0)) + .add("Elbow Pos Zero ", new ClosedLoopElbowOffsetCommand(elbowSubsystem, 0.0, () -> 0.0)) .withSize(1, 1) .withPosition(1, 0); Shuffleboard.getTab("Pit") @@ -450,7 +450,7 @@ public void configurePitDashboard() { .withSize(1, 1); Shuffleboard.getTab("Pit") - .add("Adjusted Climb Pos", new TrapClimbAdjustCommand(climbSubsystem)) + .add("Adjusted Climb Pos", new TrapClimbCommand(climbSubsystem)) .withPosition(8, 0) .withSize(1, 1); // Shuffleboard.getTab("Pit") @@ -972,7 +972,7 @@ private void configureDriverBindings() { // Vision Shoot new JoystickButton(driveJoystick, Button.M_SWH.id) .onTrue( - new MovingVisionShootCommand( + new VisionShootCommand( robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem)); // Release Game Piece Command diff --git a/src/main/java/frc/robot/commands/climb/TrapClimbAdjustCommand.java b/src/main/java/frc/robot/commands/climb/TrapClimbCommand.java similarity index 73% rename from src/main/java/frc/robot/commands/climb/TrapClimbAdjustCommand.java rename to src/main/java/frc/robot/commands/climb/TrapClimbCommand.java index d5728ddb..ac9cc3db 100644 --- a/src/main/java/frc/robot/commands/climb/TrapClimbAdjustCommand.java +++ b/src/main/java/frc/robot/commands/climb/TrapClimbCommand.java @@ -4,16 +4,16 @@ import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.climb.ClimbSubsystem.ClimbStates; -public class TrapClimbAdjustCommand extends Command { +public class TrapClimbCommand extends Command { private ClimbSubsystem climbSubsystem; - public TrapClimbAdjustCommand(ClimbSubsystem climbSubsystem) { + public TrapClimbCommand(ClimbSubsystem climbSubsystem) { this.climbSubsystem = climbSubsystem; } @Override public void initialize() { - climbSubsystem.trapClimbAdjust(); + climbSubsystem.trapClimb(); } @Override diff --git a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java index 29ed7a55..0729e24a 100644 --- a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java @@ -22,7 +22,8 @@ public ClimbCommand( @Override public void initialize() { - if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) { + if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED + || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) { robotStateSubsystem.climb(false, false); } else { climbAllowed = false; diff --git a/src/main/java/frc/robot/commands/robotState/ClimbTrapDecendCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbTrapDecendCommand.java index 27e38870..c05bbf20 100644 --- a/src/main/java/frc/robot/commands/robotState/ClimbTrapDecendCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ClimbTrapDecendCommand.java @@ -21,7 +21,8 @@ public ClimbTrapDecendCommand( @Override public void initialize() { - if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) { + if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED + || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) { robotStateSubsystem.climb(true, true); } else { climbAllowed = false; diff --git a/src/main/java/frc/robot/commands/robotState/FullTrapClimbCommand.java b/src/main/java/frc/robot/commands/robotState/FullTrapClimbCommand.java index 8e52b0b5..fa7ca025 100644 --- a/src/main/java/frc/robot/commands/robotState/FullTrapClimbCommand.java +++ b/src/main/java/frc/robot/commands/robotState/FullTrapClimbCommand.java @@ -21,7 +21,8 @@ public FullTrapClimbCommand( @Override public void initialize() { - if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) { + if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED + || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) { robotStateSubsystem.climb(true, false); } else { climbAllowed = false; diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index b40d6a36..3e03bb73 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -141,7 +141,7 @@ public final class Setpoints { public static final Pose2d MS1 = new Pose2d(4.3, 5.55, Rotation2d.fromDegrees(0.0)); public static final Pose2d NAS1 = new Pose2d(4.2, 2.8, Rotation2d.fromDegrees(-33.2)); public static final Pose2d NAS2 = - new Pose2d(4.4, 4.6, Rotation2d.fromDegrees(-12.2)); // 4,5.1, -6.7 + new Pose2d(4.55, 4.6, Rotation2d.fromDegrees(-12.2)); // 4,5.1, -6.7 // Path Midpoints public static final Pose2d MP1 = new Pose2d(5, 7.61, Rotation2d.fromDegrees(0.0)); diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index ae01acf3..1f84d25a 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -58,8 +58,8 @@ public final class ClimbConstants { public static final double kRightClimbTrapPos = 12.056; // 5.5 // TRAP ADJUST - public static final double kLeftClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch; - public static final double kRightClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch; + public static final double kLeftClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch * 2; + public static final double kRightClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch * 2; public static final double kLeftStowPos = 2.5; public static final double kRightStowPos = 2.5; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 7d2629d3..5a2f3df2 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -119,8 +119,8 @@ public static TalonFXConfiguration getDriveTalonConfig() { TalonFXConfiguration driveConfig = new TalonFXConfiguration(); CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); - currentConfig.SupplyCurrentLimit = 50; - currentConfig.SupplyCurrentThreshold = 55; + currentConfig.SupplyCurrentLimit = 40; + currentConfig.SupplyCurrentThreshold = 45; currentConfig.SupplyTimeThreshold = 0.0; currentConfig.SupplyCurrentLimitEnable = true; currentConfig.StatorCurrentLimitEnable = false; diff --git a/src/main/java/frc/robot/constants/LedConstants.java b/src/main/java/frc/robot/constants/LedConstants.java index 2c015d14..915de13d 100644 --- a/src/main/java/frc/robot/constants/LedConstants.java +++ b/src/main/java/frc/robot/constants/LedConstants.java @@ -11,8 +11,8 @@ public class LedConstants { public static final Color kGreen = new Color(0, 255, 0); public static final Color kYellowish = new Color(166, 255, 0); - public static final int kBlinkOffCount = 15; - public static final int kBlinkOnCount = 30; // > kBlinkOffCount + public static final int kBlinkOffCount = 7; + public static final int kBlinkOnCount = 14; // > kBlinkOffCount public static final int kLoopCounterCandy = 5; public static final Color[] candy = { diff --git a/src/main/java/frc/robot/constants/MagazineConstants.java b/src/main/java/frc/robot/constants/MagazineConstants.java index 9b64eef5..cf6aab83 100644 --- a/src/main/java/frc/robot/constants/MagazineConstants.java +++ b/src/main/java/frc/robot/constants/MagazineConstants.java @@ -16,7 +16,7 @@ public final class MagazineConstants { public static final int kMinBeamBreaks = 0; // 3 public static final double kIntakingSpeed = -25; // -46 public static final double kFastIntakingSpeed = -25; // -90 - public static final double kEmptyingSpeed = -90; // -72 + public static final double kEmptyingSpeed = -70; // -90 public static final double kReversingSpeed = 4.8; // TODO do testing to determine correct speed public static final double kReleaseTime = 0.75; diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index cae01a57..b35ff290 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -37,7 +37,7 @@ public class RobotConstants { public static final Translation2d kBlueFeedTargetPos = new Translation2d(kBlueSpeakerXPos + 1, kBlueSpeakerYPos + 1); - public static final double kDegreeShootOffset = Units.degreesToRadians(-3.5); + public static final double kDegreeShootOffset = Units.degreesToRadians(-3.5); // -3.5 // Robot Sizes public static final double kShooterOffset = 0.2; // meters diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index b7d8442d..97c9205f 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -8,7 +8,7 @@ public final class RobotStateConstants { public static final double kLookupMaxDistance = 5.4; // 540cm public static final double kDistanceIncrement = 0.01; // 1cm public static final double kDistanceOffset = 0.00; - public static final double kElbowShootOffset = 0.0; // .5 .7 + public static final double kElbowShootOffset = 0.00321; // .5 .7 public static final String kElbowPreferencesKey = "Elbow/Offset"; public static final double kLeftFeedLinearCoeff = 2.5; public static final double kRightFeedLinearCoeff = 0; diff --git a/src/main/java/frc/robot/constants/SuperStructureConstants.java b/src/main/java/frc/robot/constants/SuperStructureConstants.java index 43405087..0f3eb1d5 100644 --- a/src/main/java/frc/robot/constants/SuperStructureConstants.java +++ b/src/main/java/frc/robot/constants/SuperStructureConstants.java @@ -56,7 +56,7 @@ public final class SuperStructureConstants { // SUBWOOFER public static final double kWristSubwooferSetPoint = kWristIntakeSetPoint; public static final double kElbowSubwooferSetPoint = kElbowIntakeSetPoint; - public static final double kShooterSubwooferSetPoint = 40; // 60 + public static final double kShooterSubwooferSetPoint = 50; // 60 // FEEDING public static final double kWristFeedingSetPoint = kWristIntakeSetPoint; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 8211f8f6..9c9c3c3f 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -34,9 +34,9 @@ public final class VisionConstants { public static final double multiTagCoeff = 18.0 / 100.0; public static final double baseNumber = Math.E; public static final double powerNumber = 4.0; - public static final double FOV45MultiTagCoeff = 15.0 / 100.0; - public static final double FOV45powerNumber = 4.0; - public static final double FOV45SinlgeTagCoeff = 19.0 / 100.0; + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; + public static final double FOV45powerNumber = 4.5; + public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0; public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0; public static final double FOV58MJPGPowerNumber = 3.5; public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 1ab1005b..c0796bd2 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -289,6 +289,22 @@ private AutoCommandInterface getAutoCommand(int switchPos) { List.of(5, 4, 3), 4.0, AutonConstants.Setpoints.NAS2); + case 0x25: + return new SmartNonAmpAutoCommand( + driveSubsystem, + robotStateSubsystem, + superStructure, + magazineSubsystem, + intakeSubsystem, + elbowSubsystem, + pathHandler, + deadeye, + ledSubsystem, + "NonAmpInitial1_MiddleNote3", + AutonConstants.kNonAmpPathMatrix, + List.of(3, 5, 4), + 4.0, + AutonConstants.Setpoints.NAS2); case 0x30: return new DoNothingCommand( robotStateSubsystem, driveSubsystem, superStructure, magazineSubsystem, elbowSubsystem); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index eea2b6f1..da302c27 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.CANBus.CANBusStatus; import com.opencsv.CSVReader; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; @@ -306,6 +305,7 @@ public void startShootKnownPos(Pose2d pos) { getShootSolution(driveSubsystem.getDistanceToSpeaker(pos)); + intakeSubsystem.toEjecting(); magazineSubsystem.setSpeed(0.0); superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]); ledSubsystem.setOff(); @@ -340,6 +340,7 @@ public void startShoot() { shootKnownPos = false; movingShoot = false; + intakeSubsystem.toEjecting(); driveSubsystem.setIsAligningShot(true); getShootSolution(driveSubsystem.getDistanceToSpeaker()); @@ -358,6 +359,7 @@ public void startShootDistance(double distance) { getShootSolution(distance); + intakeSubsystem.toEjecting(); magazineSubsystem.setSpeed(0.0); superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]); ledSubsystem.setOff(); @@ -373,9 +375,12 @@ public void startMovingShoot() { driveSubsystem.setIsAligningShot(true); driveSubsystem.setIsMoveAndShoot(true); + intakeSubsystem.toEjecting(); Translation2d virtualT = driveSubsystem.getPoseMeters().getTranslation(); ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed(); - getShootSolution(driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d()))); + getShootSolution( + driveSubsystem.getDistanceToSpeaker( + new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation()))); for (int i = 0; i < RobotStateConstants.kMoveWhileShootIterations; i++) { virtualT = @@ -383,7 +388,9 @@ public void startMovingShoot() { new Translation2d( speeds.vxMetersPerSecond * shootSolution[3], speeds.vyMetersPerSecond * shootSolution[3])); - getShootSolution(driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d()))); + getShootSolution( + driveSubsystem.getDistanceToSpeaker( + new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation()))); } driveSubsystem.setMoveAndShootVirtualPose( new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation())); @@ -751,7 +758,8 @@ public void periodic() { Translation2d virtualT = driveSubsystem.getPoseMeters().getTranslation(); ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed(); getShootSolution( - driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d()))); + driveSubsystem.getDistanceToSpeaker( + new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation()))); for (int i = 0; i < RobotStateConstants.kMoveWhileShootIterations; i++) { virtualT = @@ -760,7 +768,8 @@ public void periodic() { speeds.vxMetersPerSecond * shootSolution[3], speeds.vyMetersPerSecond * shootSolution[3])); getShootSolution( - driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d()))); + driveSubsystem.getDistanceToSpeaker( + new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation()))); } superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java index 71c68ad6..1d8fc6a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java @@ -23,7 +23,7 @@ public class ShooterIOFX implements ShooterIO, Checkable { @HealthCheck @Timed( - percentOutput = {0.2, -0.2, 0.8, -0.8}, + percentOutput = {0.2, -0.2, 0.8, 0.0, -0.8}, duration = 3) private TalonFX shooterLeft;