diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 804abaf5..eafe28de 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2023", - "teamNumber": 4467 + "teamNumber": 666 } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ab70bcc9..d874266a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,7 +6,6 @@ import com.ctre.phoenixpro.signals.InvertedValue; import com.gos.lib.properties.GosDoubleProperty; -import com.pathplanner.lib.auto.PIDConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -225,11 +224,11 @@ private AutoConstants() { public static final PIDController CONTROLLER_Y = new PIDController(4, 0.03, 0.3); - public static final PIDConstants CONSTANTS_X = - new PIDConstants(6.0, 0.01, 0.0); +// public static final PIDConstants CONSTANTS_X = +// new PIDConstants(6.0, 0.01, 0.0); - public static final PIDConstants THETA_CONSTANTS = - new PIDConstants(4.2, 0.0, 0.0); +// public static final PIDConstants THETA_CONSTANTS = +// new PIDConstants(4.2, 0.0, 0.0); //Auto balance constants public static final double BALANCE_P = -0.04; @@ -277,7 +276,7 @@ private ArmConstants() { public static final int ARM_ANGLE_ID_FOLLOWER = 17; public static final int LIMIT_SWITCH_PORT = 3; - public static final double KP_ANGLE = CURRENT_MODE == Mode.HELIOS_V1 ? 0.53 : 0.227; + public static final double KP_ANGLE = CURRENT_MODE == Mode.HELIOS_V2 ? 0.53 : 0.227; public static final double KI_ANGLE = 0.0007; public static final double KD_ANGLE = 0.08; @@ -292,7 +291,7 @@ private ArmConstants() { public static final double ARM_KG = 0.17; // offset for the absolute value sensor - public static final double ARM_OFFSET = CURRENT_MODE == Mode.HELIOS_V1 ? 280.0 : 294; + public static final double ARM_OFFSET = CURRENT_MODE == Mode.HELIOS_V2 ? 280.0 + 130.0 : 294; public static final int ENCODER_PORT = 4; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b3c51a0..eac51183 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,9 @@ package frc.robot; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; +//import com.pathplanner.lib.PathConstraints; +//import com.pathplanner.lib.PathPlanner; +//import com.pathplanner.lib.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.PowerDistribution; @@ -46,12 +46,12 @@ public void robotInit() { //Base code written from the AdvantageKit logging framework (6328 Mechanical Advantage) //Sets up a base logger for non-subsystem inputs - PathPlannerTrajectory traj = PathPlanner.loadPath("Mobility Right", new PathConstraints(1.0, 1.0)); - PathPlannerTrajectory ftraj = lib.utils.PathPlannerFlipper.flipTrajectory(traj); +// PathPlannerTrajectory traj = PathPlanner.loadPath("Mobility Right", new PathConstraints(1.0, 1.0)); +// PathPlannerTrajectory ftraj = lib.utils.PathPlannerFlipper.flipTrajectory(traj); Field2d ffield = new Field2d(); - ffield.getObject("Traj").setTrajectory(traj); - ffield.getObject("FTraj").setTrajectory(ftraj); +// ffield.getObject("Traj").setTrajectory(traj); +// ffield.getObject("FTraj").setTrajectory(ftraj); SmartDashboard.putData("FLIP FIELD", ffield); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 49c9cfa2..3ddb2695 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,10 +11,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import frc.robot.commands.*; -import frc.robot.commands.autonomous.AutoFactory; -import frc.robot.commands.autonomous.Balance; +//import frc.robot.commands.autonomous..AutoFactory; +//import frc.robot.commands.autonomous.Balance; import frc.robot.subsystems.arm.ArmExtSubsystem; -import frc.robot.subsystems.vision.CameraSubsystem; +//import frc.robot.subsystems.vision.CameraSubsystem; import frc.robot.supersystems.ArmPose; import frc.robot.supersystems.ArmSupersystem; import lib.controllers.FootPedal; @@ -50,14 +50,14 @@ public class RobotContainer { private LedSubsystem m_led; // Camera testing crap - private CameraSubsystem m_testingCamera; +// private CameraSubsystem m_testingCamera; //Controllers private final CommandXboxController m_driveController = new CommandXboxController(Constants.DRIVER_PORT); // private final CommandXboxController m_testController = new CommandXboxController(2); //Logged chooser for auto - private final AutoFactory m_autoFactory; +// private final AutoFactory m_autoFactory; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -77,7 +77,7 @@ public RobotContainer() { m_super = new ArmSupersystem(m_arm, m_ext, m_wrist, m_drive); m_foot = new FootPedal(1); - m_testingCamera = new CameraSubsystem("TestCamera", new Transform3d()); +// m_testingCamera = new CameraSubsystem("TestCamera", new Transform3d()); break; case SIM: @@ -89,7 +89,7 @@ public RobotContainer() { LiveWindow.disableAllTelemetry(); - m_autoFactory = new AutoFactory(m_super, m_drive, m_wrist, m_ext); +// m_autoFactory = new AutoFactory(m_super, m_drive, m_wrist, m_ext); // Configure the button bindings @@ -115,14 +115,15 @@ private void configureButtonBindings() { m_driveController.leftTrigger().whileTrue(new IntakeControlCommand(m_wrist, -0.5)); m_driveController.rightTrigger().whileTrue(new IntakeControlCommand(m_wrist, 1.0)); - if (Constants.CURRENT_MODE != Constants.Mode.HELIOS_V1) { + if (Constants.CURRENT_MODE != Constants.Mode.HELIOS_V1 && false) { m_driveController.x().whileTrue( new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY) .alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID()))); } - m_driveController.y().whileTrue( - new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY) - .alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID()))); + +// m_driveController.y().whileTrue( +// new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY) +// .alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID()))); m_driveController.a().whileTrue(new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); m_driveController.b().whileTrue( @@ -153,7 +154,7 @@ public void configDashboard() { SmartDashboard.putBoolean("Stella Mode", true); ShuffleboardTab testCommands = Shuffleboard.getTab("Commands"); - testCommands.add("balance", new Balance(m_drive)); +// testCommands.add("balance", new Balance(m_drive)); testCommands.add("Toggle Angle Brake Mode", new ToggleArmBrakeModeCommand(m_arm)).withSize(2, 1); testCommands.add("Toggle Wrist Brake Mode", new InstantCommand(() -> m_wrist.toggleBrakeMode()).runsWhenDisabled()); @@ -190,7 +191,7 @@ public void configDashboard() { testCommands.add("Auto Balance", new AutoBalanceTransCommand(m_drive)); testCommands.add("Reset Pose", new InstantCommand(() -> m_drive.resetPoseBase())).withSize(2, 1); - testCommands.add("Align to Zero Degrees", m_drive.alignToAngle(0).asProxy()); +// testCommands.add("Align to Zero Degrees", m_drive.alignToAngle(0).asProxy()); } @@ -201,8 +202,8 @@ public void configDashboard() { */ public Command getAutonomousCommand() { return new InstantCommand(() -> - m_drive.resetGyro(180)) - .andThen(m_autoFactory.getAutoRoutine()); + m_drive.resetGyro(180)); +// .andThen(m_autoFactory.getAutoRoutine()); } public ArmSupersystem getArmSupersystem() { diff --git a/src/main/java/frc/robot/commands/autonomous/AutoFactory.java b/src/main/java/frc/robot/commands/autonomous/AutoFactory.java deleted file mode 100644 index 5312369d..00000000 --- a/src/main/java/frc/robot/commands/autonomous/AutoFactory.java +++ /dev/null @@ -1,111 +0,0 @@ -package frc.robot.commands.autonomous; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.commands.autonomous.AutoUtils.ScoringHeights; -import frc.robot.commands.autonomous.AutoUtils.StartingZones; -import frc.robot.subsystems.arm.ArmExtSubsystem; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - - - -public class AutoFactory { - - private final LoggedDashboardChooser m_modeChooser; - private final LoggedDashboardChooser m_heightChooser; - private final LoggedDashboardChooser m_locationChooser; - public enum AutoMode { - MOBILITY, - ENGAGE, - SINGE_SCORE, - DOUBLE_SCORE, - SINGLE_ENGAGE, - DOUBLE_ENGAGE, - TRIPLE_CONE, - SINGLE_SCORE_ONLY - } - - private final SwerveDrivetrain m_swerve; - private final ArmSupersystem m_super; - private final WristSubsystem m_wrist; - private final ArmExtSubsystem m_armEx; - - public AutoFactory(ArmSupersystem supersystem, SwerveDrivetrain drive, WristSubsystem wrist, ArmExtSubsystem armEx) { - this.m_super = supersystem; - this.m_swerve = drive; - this.m_wrist = wrist; - this.m_armEx = armEx; - m_modeChooser = new LoggedDashboardChooser<>("Auto Mode"); - m_heightChooser = new LoggedDashboardChooser<>("Scoring Height"); - m_locationChooser = new LoggedDashboardChooser<>("Starting Location"); - - // Initialize dashboard choosers - m_locationChooser.addDefaultOption("LEFT", StartingZones.LEFT); - for (StartingZones start : StartingZones.values()) { - if (start != StartingZones.LEFT) { - m_locationChooser.addOption(start.toString(), start); - } - } - - m_heightChooser.addDefaultOption("LOW", ScoringHeights.LOW); - for (ScoringHeights height : ScoringHeights.values()) { - if (height != ScoringHeights.LOW) { - m_heightChooser.addOption(height.toString(), height); - } - } - - m_modeChooser.addDefaultOption("Mobility", AutoMode.MOBILITY); - - for (AutoMode mode : AutoMode.values()) { - if (mode != AutoMode.MOBILITY) { - m_modeChooser.addOption(mode.toString(), mode); - } - } - } - - public Command getAutoRoutine() { - StartingZones start = m_locationChooser.get(); - ScoringHeights height = m_heightChooser.get(); - AutoMode mode = m_modeChooser.get(); - Command chosenCommand; - m_wrist.resetHomed(); - m_armEx.resetHomed(); - m_wrist.setIntakeSpeed(0.0); - - Command homeChecker = new CheckHomedCommand(m_armEx, m_wrist); - - switch (mode) { - case MOBILITY: - chosenCommand = new MobilityCommandGroup(m_swerve, start); - break; - case ENGAGE: - chosenCommand = new BalanceCommandGroup(m_swerve, start); - break; - case SINGE_SCORE: - chosenCommand = new OneCubeCommandGroup(m_super, m_swerve, m_wrist, start, height); - break; - case SINGLE_ENGAGE: - chosenCommand = new ScoreOneEngageCommandGroup(m_swerve, m_super, m_wrist, start, height); - break; - case DOUBLE_SCORE: - chosenCommand = new ScoreTwoCommandGroup(m_swerve, m_super, height, start); - break; - case DOUBLE_ENGAGE: - chosenCommand = new ScoreTwoBalanceCommandGroup(m_swerve, m_super, height, start); - break; - // case TRIPLE_CONE: - // chosenCommand = new ScoreThreeCommandGroup(m_swerve, m_super, height, start); - // break; - case SINGLE_SCORE_ONLY: - chosenCommand = new ScoreOneOnlyCommandGroup(m_super, m_wrist, height); - break; - default: - chosenCommand = new InstantCommand(); - } - - return homeChecker.andThen(chosenCommand); - } -} diff --git a/src/main/java/frc/robot/commands/autonomous/AutoUtils.java b/src/main/java/frc/robot/commands/autonomous/AutoUtils.java deleted file mode 100644 index 269b8b1a..00000000 --- a/src/main/java/frc/robot/commands/autonomous/AutoUtils.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot.commands.autonomous; - -import java.util.HashMap; -import java.util.Map; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.commands.FollowPathWithEvents; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.*; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.subsystems.swerve.SwerveDrivetrain; - - -public class AutoUtils { - - public enum ScoringHeights { - HIGH, - MIDDLE, - LOW - } - - public enum StartingZones { - LEFT, - MIDDLE, - RIGHT - } - - private AutoUtils() { - throw new IllegalStateException("Utility Class"); - } - - // Default Constants - private static final PathConstraints m_defaultConfig = new PathConstraints( - AutoConstants.MAX_VELOCITY_MPS_AUTO, AutoConstants.MAX_ACCELERATION_MPS_AUTO); - - -// private static final PathPlannerTrajectory m_defaultAutoGen = PathPlanner.loadPath("DefaultPath", m_defaultConfig); - -// private static final SwerveAutoBuilder defaultAutoFactory = new SwerveAutoBuilder( -// -// ) - - public static PathConstraints getDefaultConstraints() { - return m_defaultConfig; - } - - public static Command getAutoRoutine(PathPlannerTrajectory traj, SwerveDrivetrain swerve, boolean firstTrajectory){ - return new SequentialCommandGroup( - new ConditionalCommand(new InstantCommand(() -> - swerve.resetPose( - PathPlannerTrajectory.transformTrajectoryForAlliance(traj, DriverStation.getAlliance()).getInitialHolonomicPose())), - new InstantCommand(), - () -> firstTrajectory - ), - - - new PPSwerveControllerCommand(traj, - swerve::getPose, - DriveConstants.DRIVE_KINEMATICS, - AutoConstants.CONTROLLER_X, - AutoConstants.CONTROLLER_Y, - AutoConstants.THETA_CONTROLLER, - swerve::setModuleStates, - true, - swerve) - ); - } - - public static Command getAutoEventRoutine(PathPlannerTrajectory traj, Map events, SwerveDrivetrain swerve) { - return new FollowPathWithEvents(getAutoRoutine(traj, swerve, true), traj.getMarkers(), events); - } -} diff --git a/src/main/java/frc/robot/commands/autonomous/Balance.java b/src/main/java/frc/robot/commands/autonomous/Balance.java deleted file mode 100644 index 6f7bdb55..00000000 --- a/src/main/java/frc/robot/commands/autonomous/Balance.java +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.autonomous; - -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.AutoBalanceTransCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class Balance extends SequentialCommandGroup { - /** Creates a new Balance. */ - public Balance(SwerveDrivetrain mSwerve) { - // Add your commands in the addCommands() call, e.g. - addCommands(new RunCommand(() -> - mSwerve.drive(Constants.AutoConstants.MAX_VELOCITY_MPS_AUTO, 0.0, 0.0), mSwerve) - .raceWith(new WaitCommand(1.0))); - addCommands(new AutoBalanceTransCommand(mSwerve)); - } -} diff --git a/src/main/java/frc/robot/commands/autonomous/BalanceCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/BalanceCommandGroup.java deleted file mode 100644 index 88f5b88e..00000000 --- a/src/main/java/frc/robot/commands/autonomous/BalanceCommandGroup.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc.robot.commands.autonomous; - - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.AutoBalance; -import frc.robot.commands.AutoBalanceTransCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; - -import java.util.HashMap; - -public class BalanceCommandGroup extends SequentialCommandGroup { - public BalanceCommandGroup(SwerveDrivetrain m_swerve, AutoUtils.StartingZones m_start) { - DriverStation.Alliance alliance = DriverStation.getAlliance(); - PathPlannerTrajectory trajectory; - PathConstraints constraints = new PathConstraints(Units.feetToMeters(14), Units.feetToMeters(14) / 3); - - switch(m_start) { - case LEFT: - if (alliance == DriverStation.Alliance.Blue) { - trajectory = PathPlanner.loadPath("Balance Left", constraints); - } else { - trajectory = PathPlanner.loadPath("Balance Right", constraints); - } - break; - case RIGHT: - if (alliance == DriverStation.Alliance.Blue) { - trajectory = PathPlanner.loadPath("Balance Right", constraints); - } else { - trajectory = PathPlanner.loadPath("Balance Left", constraints); - } - break; - - case MIDDLE: - trajectory = PathPlanner.loadPath("Balance Middle", constraints); - break; - default: - trajectory = new PathPlannerTrajectory(); - } - - addCommands(m_swerve.getAutoBuilder(new HashMap<>()).fullAuto(trajectory)); - addCommands(new AutoBalanceTransCommand(m_swerve)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/CheckHomedCommand.java b/src/main/java/frc/robot/commands/autonomous/CheckHomedCommand.java deleted file mode 100644 index e3d4a36b..00000000 --- a/src/main/java/frc/robot/commands/autonomous/CheckHomedCommand.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.commands.autonomous; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.arm.ArmExtSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; - - -public class CheckHomedCommand extends CommandBase { - private final ArmExtSubsystem armExtSubsystem; - private final WristSubsystem wristSubsystem; - Timer timer = new Timer(); - boolean timedOut = false; - public CheckHomedCommand(ArmExtSubsystem armExtSubsystem, WristSubsystem wristSubsystem) { - this.armExtSubsystem = armExtSubsystem; - this.wristSubsystem = wristSubsystem; - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.armExtSubsystem, this.wristSubsystem); - } - - @Override - public void initialize() { - timer.start(); - } - - @Override - public void execute() { - if (!wristSubsystem.hasWristHomed()) { - wristSubsystem.goWristToHome(); - } - - if (!armExtSubsystem.hasArmHomed()) { - armExtSubsystem.goArmToHome(); - } - - if (timer.hasElapsed(.75)) { - timedOut = true; - wristSubsystem.zeroWristAngle(); - armExtSubsystem.resetExtensionEncoder(); - } - } - - @Override - public boolean isFinished() { - return timedOut || (wristSubsystem.hasWristHomed() && armExtSubsystem.hasArmHomed()); - } - - @Override - public void end(boolean interrupted) { - armExtSubsystem.setArmSpeed(0); - wristSubsystem.setWristPower(0); - } -} diff --git a/src/main/java/frc/robot/commands/autonomous/MobilityCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/MobilityCommandGroup.java deleted file mode 100644 index 93057620..00000000 --- a/src/main/java/frc/robot/commands/autonomous/MobilityCommandGroup.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.commands.autonomous; - - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.commands.autonomous.AutoUtils.StartingZones; -import lib.utils.PathPlannerFlipper; - -import java.util.HashMap; - -public class MobilityCommandGroup extends SequentialCommandGroup { - public MobilityCommandGroup(SwerveDrivetrain m_swerve, StartingZones m_start) { - // Get proper path for driver POV - DriverStation.Alliance alliance = DriverStation.getAlliance(); - PathPlannerTrajectory trajectory; - PathConstraints constraints = AutoUtils.getDefaultConstraints(); - - switch (m_start) { - case LEFT: - if (alliance == DriverStation.Alliance.Blue) { - trajectory = PathPlanner.loadPath("Mobility Left", constraints); - } else { - trajectory = PathPlanner.loadPath("Mobility Right", constraints); - } - break; - case RIGHT: - if (alliance == DriverStation.Alliance.Blue) { - trajectory = PathPlanner.loadPath("Mobility Right", constraints); - } else { - trajectory = PathPlanner.loadPath("Mobility Left", constraints); - } - break; - case MIDDLE: - trajectory = PathPlanner.loadPath("Mobility Middle", constraints); - break; - default: - trajectory = new PathPlannerTrajectory(); - } - - // trajectory = PathPlannerFlipper.flipTrajectory(trajectory); - - addCommands(m_swerve.getAutoBuilder(new HashMap<>()).fullAuto(trajectory)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/OneConeCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/OneConeCommandGroup.java deleted file mode 100644 index 35401cb6..00000000 --- a/src/main/java/frc/robot/commands/autonomous/OneConeCommandGroup.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.commands.autonomous; - - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class OneConeCommandGroup extends SequentialCommandGroup { - public OneConeCommandGroup(ArmSupersystem m_super, SwerveDrivetrain m_swerve, WristSubsystem m_wrist, AutoUtils.StartingZones start, AutoUtils.ScoringHeights height) { -// addCommands(m_swerve.alignToTag(SwerveDrivetrain.AlignmentOptions.RIGHT_ALIGN)); - - addCommands(m_super.runIntake(0.0)); - if (height == AutoUtils.ScoringHeights.HIGH) { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.HIGH_GOAL)); - } else { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - addCommands(new IntakeControlCommand(m_wrist, -1.0) - .raceWith(new WaitCommand(0.5))); - addCommands(new IntakeControlCommand(m_wrist, 0.0) - .raceWith(new WaitCommand(0.0))); - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); - addCommands(new MobilityCommandGroup(m_swerve, start)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/OneCubeCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/OneCubeCommandGroup.java deleted file mode 100644 index 7a214d49..00000000 --- a/src/main/java/frc/robot/commands/autonomous/OneCubeCommandGroup.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.commands.autonomous; - - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.commands.SupersystemToPoseCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class OneCubeCommandGroup extends SequentialCommandGroup { - public OneCubeCommandGroup(ArmSupersystem m_super, SwerveDrivetrain m_swerve, WristSubsystem m_wrist, AutoUtils.StartingZones start, AutoUtils.ScoringHeights height) { -// addCommands(m_swerve.alignToTag(SwerveDrivetrain.AlignmentOptions.CENTER_ALIGN)); - - addCommands(m_super.runIntake(0.0)); - if (height == AutoUtils.ScoringHeights.HIGH) { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.HIGH_GOAL)); - } else { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - addCommands(new IntakeControlCommand(m_wrist, -1.0) - .raceWith(new WaitCommand(0.5))); - addCommands(new IntakeControlCommand(m_wrist, 0.0) - .raceWith(new WaitCommand(0.0))); - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); - addCommands(new MobilityCommandGroup(m_swerve, start)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreMiddleAndMobilityCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreMiddleAndMobilityCommandGroup.java deleted file mode 100644 index 33c0e8db..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreMiddleAndMobilityCommandGroup.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commands.autonomous; - - -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -import java.util.HashMap; - -public class ScoreMiddleAndMobilityCommandGroup extends SequentialCommandGroup { - public ScoreMiddleAndMobilityCommandGroup(SwerveDrivetrain drive, ArmSupersystem arm, WristSubsystem wrist) { - PathPlannerTrajectory scoreTraj = PathPlanner.loadPath("Score Cone Mobility", AutoUtils.getDefaultConstraints()); - addCommands(new ScoreMiddleCommandGroup(arm, wrist)); // Score first piece - - addCommands(drive.getAutoBuilder(new HashMap<>()).fullAuto(scoreTraj)); // Move out of starting zone - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreMiddleCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreMiddleCommandGroup.java deleted file mode 100644 index afa9d08f..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreMiddleCommandGroup.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.commands.autonomous; - - -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class ScoreMiddleCommandGroup extends SequentialCommandGroup { - public ScoreMiddleCommandGroup(ArmSupersystem m_super, WristSubsystem wrist) { - // TODO: Add your sequential commands in the super() call, e.g. - // super(new OpenClawCommand(), new MoveArmCommand()); - - super(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL), - new PrintCommand("Stopped first command"), - new IntakeControlCommand(wrist, -1.0).raceWith( - new WaitCommand(0.5) - ), - new IntakeControlCommand(wrist, 0.0).raceWith( - new WaitCommand(0.0) - ), - new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreOneEngageCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreOneEngageCommandGroup.java deleted file mode 100644 index dc909125..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreOneEngageCommandGroup.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.commands.autonomous; - - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class ScoreOneEngageCommandGroup extends SequentialCommandGroup { - public ScoreOneEngageCommandGroup(SwerveDrivetrain m_swerve, ArmSupersystem m_super, WristSubsystem m_wrist, AutoUtils.StartingZones start, AutoUtils.ScoringHeights height) { - addCommands(m_super.runIntake(0.0)); - if (height == AutoUtils.ScoringHeights.HIGH) { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.HIGH_GOAL)); - } else { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - addCommands(new IntakeControlCommand(m_wrist, -0.3) - .raceWith(new WaitCommand(0.25))); - addCommands(new IntakeControlCommand(m_wrist, 0.0) - .raceWith(new WaitCommand(0.0))); - - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); - addCommands(new BalanceCommandGroup(m_swerve, start)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreOneOnlyCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreOneOnlyCommandGroup.java deleted file mode 100644 index d0f23880..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreOneOnlyCommandGroup.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.commands.autonomous; - - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class ScoreOneOnlyCommandGroup extends SequentialCommandGroup { - public ScoreOneOnlyCommandGroup(ArmSupersystem m_super, WristSubsystem m_wrist, - AutoUtils.ScoringHeights height) { - addCommands(m_super.runIntake(0.0)); - if (height == AutoUtils.ScoringHeights.HIGH) - { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.HIGH_GOAL)); - } - else - { - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - addCommands(new IntakeControlCommand(m_wrist, -1.0).raceWith(new WaitCommand(0.5))); - addCommands(new IntakeControlCommand(m_wrist, 0.0).raceWith(new WaitCommand(0.0))); - addCommands(new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreTwoBalanceCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreTwoBalanceCommandGroup.java deleted file mode 100644 index a5712f9b..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreTwoBalanceCommandGroup.java +++ /dev/null @@ -1,74 +0,0 @@ -package frc.robot.commands.autonomous; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; -import frc.robot.commands.AutoBalanceTransCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.supersystems.ArmPose; -import frc.robot.supersystems.ArmSupersystem; - -import java.util.HashMap; -import java.util.List; - -public class ScoreTwoBalanceCommandGroup extends SequentialCommandGroup { - public ScoreTwoBalanceCommandGroup(SwerveDrivetrain m_swerve, ArmSupersystem m_armSupersystem, AutoUtils.ScoringHeights scoreHeight, AutoUtils.StartingZones start) { - addRequirements(m_swerve); - m_armSupersystem.addRequirements(this); - - PathConstraints constraints = AutoUtils.getDefaultConstraints(); - DriverStation.Alliance alliance = DriverStation.getAlliance(); - - if (scoreHeight == AutoUtils.ScoringHeights.HIGH) { - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.HIGH_GOAL)); - } else { - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - List trajectory; - - if((start == AutoUtils.StartingZones.LEFT && alliance == DriverStation.Alliance.Blue) || (start == AutoUtils.StartingZones.RIGHT && alliance == DriverStation.Alliance.Red)) { - trajectory = PathPlanner.loadPathGroup("PickUp Left Engage", constraints); - } else if ((start == AutoUtils.StartingZones.RIGHT && alliance == DriverStation.Alliance.Blue) || (start == AutoUtils.StartingZones.LEFT && alliance == DriverStation.Alliance.Red)) { - trajectory = PathPlanner.loadPathGroup("PickUp Right Engage", constraints); - } else { - trajectory = PathPlanner.loadPathGroup("PickUp Middle Engage", constraints); - } - - ArmPose armScoringPose; - - // We don't score low (for now at least) - if (scoreHeight == AutoUtils.ScoringHeights.MIDDLE) { - armScoringPose = Constants.ArmSetpoints.MIDDLE_GOAL; - } else { - armScoringPose = Constants.ArmSetpoints.HIGH_GOAL; - } - - HashMap autoEvents = new HashMap<>(); - autoEvents.put("LowerIntake", (new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.INTAKE_BATTERY)) - .andThen(m_armSupersystem.runIntakeForTime(0.75, 1.0)) - .andThen(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.STOW_POSITION))); - autoEvents.put("ClearGround", (new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.STOW_POSITION))); - autoEvents.put("Score", - (new SupersystemToPoseAutoCommand(m_armSupersystem, armScoringPose)) - .andThen(m_armSupersystem.runIntakeForTime(0.09, -0.6))); - autoEvents.put("RaiseArm", - new SupersystemToPoseAutoCommand(m_armSupersystem, armScoringPose)); - - // trajectory = lib.utils.PathPlannerFlipper.flipTrajectory(trajectory); - - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, armScoringPose)); - addCommands(m_armSupersystem.runIntakeForTime(0.15, -0.6)); - addCommands(m_swerve.getAutoBuilder(autoEvents).fullAuto(trajectory).andThen(new AutoBalanceTransCommand(m_swerve))); - } - - - -} - diff --git a/src/main/java/frc/robot/commands/autonomous/ScoreTwoCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/ScoreTwoCommandGroup.java deleted file mode 100644 index 1a0c3dfe..00000000 --- a/src/main/java/frc/robot/commands/autonomous/ScoreTwoCommandGroup.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.commands.autonomous; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.supersystems.ArmPose; -import frc.robot.supersystems.ArmSupersystem; -import lib.utils.PathPlannerFlipper; - -import java.util.HashMap; - -public class ScoreTwoCommandGroup extends SequentialCommandGroup { - public ScoreTwoCommandGroup(SwerveDrivetrain m_swerve, ArmSupersystem m_armSupersystem, AutoUtils.ScoringHeights scoreHeight, AutoUtils.StartingZones start) { - addRequirements(m_swerve); - m_armSupersystem.addRequirements(this); - - PathConstraints constraints; - DriverStation.Alliance alliance = DriverStation.getAlliance(); - - if (scoreHeight == AutoUtils.ScoringHeights.HIGH) { - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.HIGH_GOAL)); - } else { - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.MIDDLE_GOAL)); - } - - PathPlannerTrajectory trajectory; - - if((start == AutoUtils.StartingZones.LEFT && alliance == DriverStation.Alliance.Blue) || (start == AutoUtils.StartingZones.RIGHT && alliance == DriverStation.Alliance.Red)) { - constraints = new PathConstraints(Units.feetToMeters(14), Units.feetToMeters(14) / 3); - trajectory = PathPlanner.loadPath("PickUp Left", constraints); - } else { - constraints = AutoUtils.getDefaultConstraints(); - trajectory = PathPlanner.loadPath("PickUp Right", constraints); - } - - ArmPose armScoringPose; - - // We don't score low (for now at least) - if (scoreHeight == AutoUtils.ScoringHeights.MIDDLE) { - armScoringPose = Constants.ArmSetpoints.MIDDLE_GOAL; - } else { - armScoringPose = Constants.ArmSetpoints.HIGH_GOAL; - } - - HashMap autoEvents = new HashMap<>(); - autoEvents.put("LowerIntake", (new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.INTAKE_BATTERY)) - .andThen(m_armSupersystem.runIntakeForTime(1, 1.0)) - .andThen(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.STOW_POSITION))); - autoEvents.put("ClearGround", new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.STOW_POSITION)); - autoEvents.put("Score", (new SupersystemToPoseAutoCommand(m_armSupersystem, armScoringPose)) - .andThen(m_armSupersystem.runIntakeForTime(0.25, -0.05)) - .andThen(new SupersystemToPoseAutoCommand(m_armSupersystem, Constants.ArmSetpoints.STOW_POSITION))); - - // trajectory = PathPlannerFlipper.flipTrajectory(trajectory); - - addCommands(new SupersystemToPoseAutoCommand(m_armSupersystem, armScoringPose)); - addCommands(m_armSupersystem.runIntakeForTime(0.3, -0.4)); - addCommands(m_swerve.getAutoBuilder(autoEvents).fullAuto(trajectory)); - } - - - -} - diff --git a/src/main/java/frc/robot/commands/autonomous/TimerForwardAutoCommand.java b/src/main/java/frc/robot/commands/autonomous/TimerForwardAutoCommand.java deleted file mode 100644 index 934fbb9a..00000000 --- a/src/main/java/frc/robot/commands/autonomous/TimerForwardAutoCommand.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.commands.autonomous; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.swerve.SwerveDrivetrain; - - -public class TimerForwardAutoCommand extends CommandBase { - private final SwerveDrivetrain m_swerveDrivetrain; - private final Timer m_timer; - private double m_speed = 0.0; - - public TimerForwardAutoCommand(SwerveDrivetrain swerveDrivetrain, double speed) { - this.m_swerveDrivetrain = swerveDrivetrain; - m_timer = new Timer(); - m_speed = speed; - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.m_swerveDrivetrain); - } - - /** - * The initial subroutine of a command. Called once when the command is initially scheduled. - */ - @Override - public void initialize() { - m_swerveDrivetrain.drive(0.0, 0.0, 0.0); - m_timer.start(); - } - - /** - * The main body of a command. Called repeatedly while the command is scheduled. - * (That is, it is called repeatedly until {@link #isFinished()}) returns true.) - */ - @Override - public void execute() { - m_swerveDrivetrain.drive(m_speed, 0.0, 0.0); - } - - /** - *

- * Returns whether this command has finished. Once a command finishes -- indicated by - * this method returning true -- the scheduler will call its {@link #end(boolean)} method. - *

- * Returning false will result in the command never ending automatically. It may still be - * cancelled manually or interrupted by another command. Hard coding this command to always - * return true will result in the command executing once and finishing immediately. It is - * recommended to use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} - * for such an operation. - *

- * - * @return whether this command has finished. - */ - @Override - public boolean isFinished() { - // TODO: Make this return true when this Command no longer needs to run execute() - return m_timer.get() >= 2.5; - } - - /** - * The action to take when the command ends. Called when either the command - * finishes normally -- that is it is called when {@link #isFinished()} returns - * true -- or when it is interrupted/canceled. This is where you may want to - * wrap up loose ends, like shutting off a motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ - @Override - public void end(boolean interrupted) { - m_swerveDrivetrain.drive(0.0, 0.0, 0.0); - } -} diff --git a/src/main/java/frc/robot/commands/autonomous/test/ScoreMidAndMoveCommandGroup.java b/src/main/java/frc/robot/commands/autonomous/test/ScoreMidAndMoveCommandGroup.java deleted file mode 100644 index 0eb7c6b5..00000000 --- a/src/main/java/frc/robot/commands/autonomous/test/ScoreMidAndMoveCommandGroup.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.commands.autonomous.test; - - -import com.sun.jdi.event.MonitorWaitedEvent; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.SupersystemToPoseAutoCommand; -import frc.robot.commands.SupersystemToPoseCommand; -import frc.robot.commands.autonomous.TimerForwardAutoCommand; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.supersystems.ArmSupersystem; - -public class ScoreMidAndMoveCommandGroup extends SequentialCommandGroup { - public ScoreMidAndMoveCommandGroup(SwerveDrivetrain m_drive, ArmSupersystem m_super, WristSubsystem m_wrist) { - // TODO: Add your sequential commands in the super() call, e.g. - // super(new OpenClawCommand(), new MoveArmCommand()); - super( - new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.MIDDLE_GOAL) - .alongWith(new WaitCommand(5).andThen(new IntakeControlCommand(m_wrist, -1.0))) - .raceWith(new WaitCommand(6)), - new SupersystemToPoseAutoCommand(m_super, Constants.ArmSetpoints.STOW_POSITION) - .raceWith(new TimerForwardAutoCommand(m_drive, -0.5)), - new InstantCommand(() -> {m_drive.resetGyro(180);}) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autonomous/test/TestAutoWithArm.java b/src/main/java/frc/robot/commands/autonomous/test/TestAutoWithArm.java deleted file mode 100644 index f841b265..00000000 --- a/src/main/java/frc/robot/commands/autonomous/test/TestAutoWithArm.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commands.autonomous.test; - -import com.pathplanner.lib.PathPlanner; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.commands.SupersystemToPoseCommand; -import frc.robot.commands.autonomous.AutoUtils; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.supersystems.ArmSupersystem; - -import java.util.HashMap; - -public class TestAutoWithArm { - public static Command getAuto(SwerveDrivetrain swerve, ArmSupersystem supersystem) { - HashMap eventMap = new HashMap<>(); - eventMap.put("ArmPose", new SupersystemToPoseCommand(supersystem, Constants.ArmSetpoints.INTAKE_CONE)); - return AutoUtils.getAutoEventRoutine(PathPlanner.loadPath("TestGoForward", AutoUtils.getDefaultConstraints()), - eventMap, - swerve); - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java index 4a45ef46..73a6a88f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java @@ -71,7 +71,7 @@ public ArmAngleSubsystem() { config.setFrame0Rate(SparkMaxFactory.MAX_CAN_FRAME_PERIOD); config.setFollowingMotor(m_armAngleMaster); - config.setInverted(Constants.CURRENT_MODE == Constants.Mode.HELIOS_V1); + config.setInverted(true); // The SparkMaxFactory will set the motor to follow the given motor m_armAngleFollower = SparkMaxFactory.Companion.createSparkMax(ArmConstants.ARM_ANGLE_ID_FOLLOWER, config); m_armAngleFollower.enableVoltageCompensation(12); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java index 26c7f7ca..fe6ad9cd 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java @@ -49,7 +49,7 @@ public static class ArmExtIOInputs { public ArmExtSubsystem() { SparkMaxFactory.SparkMaxConfig config = new SparkMaxFactory.SparkMaxConfig(); config.setCurrentLimit(50); - config.setInverted(Constants.CURRENT_MODE == Constants.Mode.HELIOS_V1); + config.setInverted(true); m_armExt = SparkMaxFactory.Companion.createSparkMax(Constants.ArmConstants.ARM_EXTENSION_ID, config); m_armExt.setClosedLoopRampRate(0.1); @@ -61,7 +61,7 @@ public ArmExtSubsystem() { m_extPID.setP(Constants.ArmConstants.ARM_EXT_KP.getValue()); m_extPID.setI(Constants.ArmConstants.ARM_EXT_KI.getValue()); m_extPID.setD(Constants.ArmConstants.ARM_EXT_KD.getValue()); - m_extPID.setOutputRange(-1, 1); + m_extPID.setOutputRange(-0.5, 0.5); m_armLimitSwitch = new DigitalInput(Constants.ArmConstants.LIMIT_SWITCH_PORT); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java index a8112d6e..c455b4e4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java @@ -1,11 +1,11 @@ package frc.robot.subsystems.swerve; import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; -import com.pathplanner.lib.auto.SwerveAutoBuilder; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; +//import com.pathplanner.lib.PathPlanner; +//import com.pathplanner.lib.PathPlannerTrajectory; +//import com.pathplanner.lib.PathPoint; +//import com.pathplanner.lib.auto.SwerveAutoBuilder; +//import com.pathplanner.lib.commands.PPSwerveControllerCommand; import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; @@ -19,21 +19,18 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ProxyCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.*; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.AutoConstants; -import frc.robot.commands.autonomous.AutoUtils; +//import frc.robot.commands.autonomous.AutoUtils; import frc.robot.subsystems.swerve.module.FalconProModule; import frc.robot.subsystems.swerve.module.SwerveModNeo; import frc.robot.subsystems.swerve.module.SwerveModuleInterface; -import frc.robot.subsystems.vision.CameraSubsystem; +//import frc.robot.subsystems.vision.CameraSubsystem; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; +//import org.photonvision.EstimatedRobotPose; import java.util.HashMap; import java.util.Map; @@ -56,8 +53,8 @@ public class SwerveDrivetrain extends SubsystemBase { private final SwerveDrivePoseEstimator m_poseEstimator; private final SwerveDrivePoseEstimator m_visionEstimator; - private final CameraSubsystem m_leftCam; - private final CameraSubsystem m_rightCam; +// private final CameraSubsystem m_leftCam; +// private final CameraSubsystem m_rightCam; private double m_currentPitch = 0; @@ -101,7 +98,7 @@ public static class SwerveIOInputs { public SwerveDrivetrain() { // Check which robot code is on - if (Constants.CURRENT_MODE == Constants.Mode.HELIOS_V1) { + if (Constants.CURRENT_MODE == Constants.Mode.HELIOS_V2) { m_flMod = new SwerveModNeo(0, DriveConstants.MOD_FL_OFFSET, DriveConstants.MOD_FL_CANS, false); m_frMod = new SwerveModNeo(1, DriveConstants.MOD_FR_OFFSET, DriveConstants.MOD_FR_CANS, false); m_blMod = new SwerveModNeo(2, DriveConstants.MOD_BL_OFFSET, DriveConstants.MOD_BL_CANS, false); @@ -136,8 +133,8 @@ public SwerveDrivetrain() { getModulePositions(), new Pose2d()); - m_leftCam = new CameraSubsystem(DriveConstants.LEFT_GLOBAL_CAM, DriveConstants.LEFT_CAM_POSE); - m_rightCam = new CameraSubsystem(DriveConstants.RIGHT_GLOBAL_CAM, DriveConstants.RIGHT_CAM_POSE); +// m_leftCam = new CameraSubsystem(DriveConstants.LEFT_GLOBAL_CAM, DriveConstants.LEFT_CAM_POSE); +// m_rightCam = new CameraSubsystem(DriveConstants.RIGHT_GLOBAL_CAM, DriveConstants.RIGHT_CAM_POSE); SmartDashboard.putData("Field", m_field); @@ -184,8 +181,8 @@ public void periodic() { //getFrontCamTagID(); - m_leftCam.periodic(); - m_rightCam.periodic(); +// m_leftCam.periodic(); +// m_rightCam.periodic(); } // Getters @@ -309,19 +306,19 @@ public void updatePoseEstimator() { /* * Add each vision measurement to the pose estimator if it exists for each camera */ - Optional leftCamPose = m_leftCam.getPose(getPose()); - Optional rightCamPose = m_rightCam.getPose(getPose()); +// Optional leftCamPose = m_leftCam.getPose(getPose()); +// Optional rightCamPose = m_rightCam.getPose(getPose()); - Logger.getInstance().recordOutput("L Cam Pose", leftCamPose.isPresent() ? leftCamPose.get().estimatedPose.toPose2d() : new Pose2d()); - Logger.getInstance().recordOutput("R Cam Pose", rightCamPose.isPresent() ? rightCamPose.get().estimatedPose.toPose2d() : new Pose2d()); +// Logger.getInstance().recordOutput("L Cam Pose", leftCamPose.isPresent() ? leftCamPose.get().estimatedPose.toPose2d() : new Pose2d()); +// Logger.getInstance().recordOutput("R Cam Pose", rightCamPose.isPresent() ? rightCamPose.get().estimatedPose.toPose2d() : new Pose2d()); - leftCamPose.ifPresent(estimatedRobotPose -> m_visionEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), leftCamPose.get().timestampSeconds)); - rightCamPose.ifPresent(estimatedRobotPose -> m_visionEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), rightCamPose.get().timestampSeconds)); +// leftCamPose.ifPresent(estimatedRobotPose -> m_visionEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), leftCamPose.get().timestampSeconds)); +// rightCamPose.ifPresent(estimatedRobotPose -> m_visionEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), rightCamPose.get().timestampSeconds)); - Logger.getInstance().recordOutput("Merged Cam pose", m_visionEstimator.getEstimatedPosition()); +// Logger.getInstance().recordOutput("Merged Cam pose", m_visionEstimator.getEstimatedPosition()); - Logger.getInstance().recordOutput("Left Cam Has Pose", leftCamPose.isPresent()); - Logger.getInstance().recordOutput("Right Cam Has Pose", rightCamPose.isPresent()); +// Logger.getInstance().recordOutput("Left Cam Has Pose", leftCamPose.isPresent()); +// Logger.getInstance().recordOutput("Right Cam Has Pose", rightCamPose.isPresent()); // leftCamPose.ifPresent(estimatedRobotPose -> m_poseEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), leftCamPose.get().timestampSeconds)); // rightCamPose.ifPresent(estimatedRobotPose -> m_poseEstimator.addVisionMeasurement(estimatedRobotPose.estimatedPose.toPose2d(), rightCamPose.get().timestampSeconds)); @@ -369,7 +366,7 @@ public Command alignToTag(AlignmentOptions align) { Translation2d translatedEnd; Translation2d translatedMiddle; - PathPlannerTrajectory traj; +// PathPlannerTrajectory traj; // Plan which offset to use switch (align) { @@ -410,47 +407,49 @@ public Command alignToTag(AlignmentOptions align) { //generate a path based on the tag you see, flipped 180 from tag pose - traj = PathPlanner.generatePath( - AutoUtils.getDefaultConstraints(), - new PathPoint(getPose().getTranslation(), new Rotation2d(), getPose().getRotation(), chassisSpeed.getNorm()), - new PathPoint(translatedMiddle, new Rotation2d(), tagPose.getRotation().rotateBy(Rotation2d.fromDegrees(180))), - new PathPoint(translatedEnd, new Rotation2d(), tagPose.getRotation().rotateBy(Rotation2d.fromDegrees(180))) - ); - - Logger.getInstance().recordOutput("Current Trajectory", traj); - - return new PPSwerveControllerCommand( - traj, - this::getPose, - DriveConstants.DRIVE_KINEMATICS, - Constants.AutoConstants.CONTROLLER_X, - Constants.AutoConstants.CONTROLLER_Y, - Constants.AutoConstants.THETA_CONTROLLER, - this::setModuleStates, - this - ); - } - - public Command alignToAngle(double angle) { - PathPlannerTrajectory traj = PathPlanner.generatePath( - AutoUtils.getDefaultConstraints(), - new PathPoint(getPose().getTranslation(), new Rotation2d(), getPose().getRotation()), - new PathPoint(getPose().getTranslation(), new Rotation2d(), Rotation2d.fromDegrees(angle))); - - return getAutoBuilder(new HashMap<>()).followPath(traj); - } - - public void followTag(Translation2d offset) { - // Determine the "end point" for the follow - Transform3d tagToRobot = new Transform3d(); //m_frontCamSubsystem.robotToTag(); - tagToRobot.plus(new Transform3d(new Translation3d(offset.getX(), offset.getY(), 0.0), new Rotation3d())); - - double xOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_X.calculate(getPose().getX(), tagToRobot.getX()), -1.0, 1.0); - double yOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_Y.calculate(getPose().getY(), tagToRobot.getY()), -1.0, 1.0); - double zOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_THETA.calculate(getPose().getRotation().getDegrees(), tagToRobot.getRotation().toRotation2d().getDegrees()), -1.0, 1.0); +// traj = PathPlanner.generatePath( +// AutoUtils.getDefaultConstraints(), +// new PathPoint(getPose().getTranslation(), new Rotation2d(), getPose().getRotation(), chassisSpeed.getNorm()), +// new PathPoint(translatedMiddle, new Rotation2d(), tagPose.getRotation().rotateBy(Rotation2d.fromDegrees(180))), +// new PathPoint(translatedEnd, new Rotation2d(), tagPose.getRotation().rotateBy(Rotation2d.fromDegrees(180))) +// ); + +// Logger.getInstance().recordOutput("Current Trajectory", traj); + +// return new PPSwerveControllerCommand( +// traj, +// this::getPose, +// DriveConstants.DRIVE_KINEMATICS, +// Constants.AutoConstants.CONTROLLER_X, +// Constants.AutoConstants.CONTROLLER_Y, +// Constants.AutoConstants.THETA_CONTROLLER, +// this::setModuleStates, +// this +// ); + + return new InstantCommand(); + } + +// public Command alignToAngle(double angle) { +// PathPlannerTrajectory traj = PathPlanner.generatePath( +// AutoUtils.getDefaultConstraints(), +// new PathPoint(getPose().getTranslation(), new Rotation2d(), getPose().getRotation()), +// new PathPoint(getPose().getTranslation(), new Rotation2d(), Rotation2d.fromDegrees(angle))); +// +// return getAutoBuilder(new HashMap<>()).followPath(traj); +// } - drive(xOut, yOut, zOut); - } +// public void followTag(Translation2d offset) { +// Determine the "end point" for the follow +// Transform3d tagToRobot = new Transform3d(); //m_frontCamSubsystem.robotToTag(); +// tagToRobot.plus(new Transform3d(new Translation3d(offset.getX(), offset.getY(), 0.0), new Rotation3d())); +// +// double xOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_X.calculate(getPose().getX(), tagToRobot.getX()), -1.0, 1.0); +// double yOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_Y.calculate(getPose().getY(), tagToRobot.getY()), -1.0, 1.0); +// double zOut = MathUtil.clamp(DriveConstants.FOLLOW_CONTROLLER_THETA.calculate(getPose().getRotation().getDegrees(), tagToRobot.getRotation().toRotation2d().getDegrees()), -1.0, 1.0); +// +// drive(xOut, yOut, zOut); +// } public ChassisSpeeds getChassisSpeed() { return DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); @@ -460,19 +459,19 @@ public double getDetectionRange() { return m_tofSensor.getRange(); } - public SwerveAutoBuilder getAutoBuilder(Map eventMap) { - return new SwerveAutoBuilder( - this::getPose, - this::resetPose, - DriveConstants.DRIVE_KINEMATICS, - AutoConstants.CONSTANTS_X, - AutoConstants.THETA_CONSTANTS, - this::setModuleStates, - eventMap, - true, - this - ); - } +// public SwerveAutoBuilder getAutoBuilder(Map eventMap) { +// return new SwerveAutoBuilder( +// this::getPose, +// this::resetPose, +// DriveConstants.DRIVE_KINEMATICS, +// AutoConstants.CONSTANTS_X, +// AutoConstants.THETA_CONSTANTS, +// this::setModuleStates, +// eventMap, +// true, +// this +// ); +// } public Command resetGyroBase() { return runOnce(this::resetGyro); diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/vision/CameraSubsystem.java index 03113de5..20463269 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraSubsystem.java @@ -1,272 +1,272 @@ -package frc.robot.subsystems.vision; - - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.Constants; - -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - - -public class CameraSubsystem implements Subsystem { - - private final PhotonCamera m_camera; - private final Transform3d robotToCam; - private final CameraPose m_campose; - - private PhotonPoseEstimator m_photonPoseEstimator; - - private DriverStation.Alliance m_prevAlliance; - private AprilTagFieldLayout m_aprilTagFieldLayout; - - private int m_prevTag = 1; - private String m_name; - - /** - * Creates a new CameraSubsystem - * - * @param camName Name of the camera in the system - * @param camPose Transform3d of the camera's position relative to the robot - */ - public CameraSubsystem(String camName, Transform3d camPose) { - m_camera = new PhotonCamera(camName); - m_prevAlliance = DriverStation.getAlliance(); - m_name = camName; - - SmartDashboard.putBoolean("Use Vision Filtering?", true); - SmartDashboard.putBoolean(camName + "/filtering", true); - - robotToCam = camPose; - - // set origin point for allience. This is done because pathplanner flips paths for alliances weirdly, - // causing need for this - try { - m_aprilTagFieldLayout = - AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - if (DriverStation.getAlliance() == DriverStation.Alliance.Blue) { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); - } - - m_photonPoseEstimator = new PhotonPoseEstimator - (m_aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, m_camera, - robotToCam); - } catch (IOException e) { - throw new IllegalStateException(e); - } - - m_campose = new CameraPose(camName, robotToCam.getTranslation(), robotToCam.getRotation()); - SmartDashboard.putBoolean(camName + "/update original", false); - } - - /** - * Gets optional estimatedRobotPose from AprilTag data - * @param prevEstimatedRobotPose Pose 2d reference position for the Photon Vision pose estimator to work off of - * @return Optional estimatedRobotPose, a Pose3d and a timestamp in seconds - */ - public Optional getPose(Pose2d prevEstimatedRobotPose) { - m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - - // Check for alllience switch, used mainly for non-comp testing - if (DriverStation.getAlliance() != m_prevAlliance) { - m_prevAlliance = DriverStation.getAlliance(); - if (DriverStation.getAlliance() == DriverStation.Alliance.Red) { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); - - } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } - m_photonPoseEstimator.setFieldTags(m_aprilTagFieldLayout); - } - - PhotonPipelineResult camResult = m_camera.getLatestResult(); - - if (!SmartDashboard.getBoolean(m_name + "/filtering", true)) { - return m_photonPoseEstimator.update(camResult); - } else { - ArrayList goodTargets = new ArrayList<>(); - - double robotToTagDist; - - if (!camResult.hasTargets()) { - return Optional.empty(); - } else { - for (PhotonTrackedTarget target : camResult.getTargets()) { - /** - * Get the current target position and check if it is a valid target. - */ - Optional targetPose = m_aprilTagFieldLayout.getTagPose(target.getFiducialId()); - - if (targetPose.isEmpty()) { - continue; - } - - if (SmartDashboard.getBoolean("Use Vision Filtering?", true)) { - - /** - * Calculate the position of the robot based on the best target result and alternate target result. - */ - Pose3d bestTagPose = targetPose.get().transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCam); - Pose3d altTagPose = targetPose.get().transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCam); - - SmartDashboard.putNumber("Best Tag X", bestTagPose.getX()); - SmartDashboard.putNumber("Best Tag Y", bestTagPose.getY()); - SmartDashboard.putNumber("Alt Tag X", altTagPose.getX()); - SmartDashboard.putNumber("Alt Tag Y", altTagPose.getY()); - - /** - * Calculate the distance between the bestTagPose and the previous pose. - * And calculate the distance between the altTagPose and the previous pose. - */ - double bestPoseAndPrevPoseDist = Math.sqrt((bestTagPose.getX() - prevEstimatedRobotPose.getX() - * (bestTagPose.getX() - prevEstimatedRobotPose.getX())) - + ((bestTagPose.getY() - prevEstimatedRobotPose.getY()) - * (bestTagPose.getY() - prevEstimatedRobotPose.getY()))); - double altPoseAndPrevPoseDist = Math.sqrt((altTagPose.getX() - prevEstimatedRobotPose.getX() - * (altTagPose.getX() - prevEstimatedRobotPose.getX())) - + ((altTagPose.getY() - prevEstimatedRobotPose.getY()) - * (altTagPose.getY() - prevEstimatedRobotPose.getY()))); - - /** - * check which robot position based on the two different results is closer to the previous pose. - * Then calculate the distance between the desired robot position and the tag position. - */ - if (bestPoseAndPrevPoseDist < altPoseAndPrevPoseDist) { - /* - robotToTagDist = Math.sqrt(((bestTagPose.getX() - targetPose.get().getX()) - * (bestTagPose.getX() - targetPose.get().getX())) - + ((bestTagPose.getY() - targetPose.get().getY()) - * (bestTagPose.getY() - targetPose.get().getY()))); - - */ - - robotToTagDist = bestTagPose.minus(targetPose.get()).getTranslation().getNorm(); - } else { - /* - robotToTagDist = Math.sqrt(((altTagPose.getX() - targetPose.get().getX()) - * (altTagPose.getX() - targetPose.get().getX())) - + ((altTagPose.getY() - targetPose.get().getY()) - * (altTagPose.getY() - targetPose.get().getY()))); - */ - robotToTagDist = altTagPose.minus(targetPose.get()).getTranslation().getNorm(); - } - - SmartDashboard.putNumber("Robot to Tag Distance", robotToTagDist); - - /** - * Check if the ambiguity of the tag is below the predetermined threshold. - * And check if the distance between the robot and tag is below the predetermined threshold. - */ - if (target.getPoseAmbiguity() <= Constants.DriveConstants.CAM_AMBIGUITY_THRESHOLD.getValue() - && robotToTagDist < Constants.DriveConstants.CAM_DISTANCE_THRESHOLD.getValue()) { - goodTargets.add(target); - } - } else { - goodTargets = (ArrayList) camResult.getTargets(); - } - } - - - /** - * Create a new photon pipeline with the list of good targets - */ - PhotonPipelineResult filteredCamResults = new PhotonPipelineResult - (camResult.getLatencyMillis(), goodTargets); - filteredCamResults.setTimestampSeconds(camResult.getTimestampSeconds()); - - return m_photonPoseEstimator.update(filteredCamResults); - } - } - } - - - public Pose2d getTagPose() { - PhotonPipelineResult results = m_camera.getLatestResult(); - Optional pose; - - if (results.hasTargets()) { - pose = m_aprilTagFieldLayout.getTagPose(results.getBestTarget().getFiducialId()); - m_prevTag = results.getBestTarget().getFiducialId(); - } else { - pose = m_aprilTagFieldLayout.getTagPose(m_prevTag); - } - - // return the pose of the tag currently visible to the camera - if(pose.isPresent()) { - return pose.get().toPose2d(); - } else { - return new Pose2d(); - } - } - - public Transform3d robotToTag() { - PhotonPipelineResult result = m_camera.getLatestResult(); - - if (result.hasTargets()) { - PhotonTrackedTarget target = result.getBestTarget(); - return target.getBestCameraToTarget().plus(robotToCam.inverse()); - } else { - return new Transform3d(); - } - } - - public int getTagID() { - PhotonPipelineResult results = m_camera.getLatestResult(); - - if (results.hasTargets()) { - m_prevTag = results.getBestTarget().getFiducialId(); - return results.getBestTarget().getFiducialId(); - } else { - return m_prevTag; - } - } - - public void useSingleTag(int tagId) { - AprilTagFieldLayout field = new AprilTagFieldLayout( - List.of(new AprilTag(tagId, new Pose3d())), - 0.0, - 0.0 - ); - - m_photonPoseEstimator.setFieldTags(field); - } - - public void resetTagField() { - m_photonPoseEstimator.setFieldTags(m_aprilTagFieldLayout); - } - - @Override - public void periodic() { - if (m_campose.hasChanged()) { - m_photonPoseEstimator = new PhotonPoseEstimator(m_aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, - m_camera, - new Transform3d(m_campose.getTranslation(), m_campose.getRotation())); - } - - if (SmartDashboard.getBoolean(m_name + "/update original", false)) { - m_campose.setOriginal(); - SmartDashboard.putBoolean(m_name + "/update original", false); - } - m_campose.updateVisualization(); - } -} +//package frc.robot.subsystems.vision; +// +// +//import edu.wpi.first.apriltag.AprilTag; +//import edu.wpi.first.apriltag.AprilTagFieldLayout; +//import edu.wpi.first.apriltag.AprilTagFields; +//import edu.wpi.first.math.geometry.*; +//import edu.wpi.first.wpilibj.DriverStation; +//import edu.wpi.first.wpilibj2.command.Subsystem; +//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// +//import frc.robot.Constants; +// +//import org.littletonrobotics.junction.Logger; +//import org.photonvision.EstimatedRobotPose; +//import org.photonvision.PhotonCamera; +//import org.photonvision.PhotonPoseEstimator; +//import org.photonvision.targeting.PhotonPipelineResult; +//import org.photonvision.targeting.PhotonTrackedTarget; +// +//import java.io.IOException; +//import java.util.ArrayList; +//import java.util.List; +//import java.util.Optional; +// +// +//public class CameraSubsystem implements Subsystem { +// +// private final PhotonCamera m_camera; +// private final Transform3d robotToCam; +// private final CameraPose m_campose; +// +// private PhotonPoseEstimator m_photonPoseEstimator; +// +// private DriverStation.Alliance m_prevAlliance; +// private AprilTagFieldLayout m_aprilTagFieldLayout; +// +// private int m_prevTag = 1; +// private String m_name; +// +// /** +// * Creates a new CameraSubsystem +// * +// * @param camName Name of the camera in the system +// * @param camPose Transform3d of the camera's position relative to the robot +// */ +// public CameraSubsystem(String camName, Transform3d camPose) { +// m_camera = new PhotonCamera(camName); +// m_prevAlliance = DriverStation.getAlliance(); +// m_name = camName; +// +// SmartDashboard.putBoolean("Use Vision Filtering?", true); +// SmartDashboard.putBoolean(camName + "/filtering", true); +// +// robotToCam = camPose; +// +// // set origin point for allience. This is done because pathplanner flips paths for alliances weirdly, +// // causing need for this +// try { +// m_aprilTagFieldLayout = +// AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); +// if (DriverStation.getAlliance() == DriverStation.Alliance.Blue) { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); +// } else { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); +// } +// +// m_photonPoseEstimator = new PhotonPoseEstimator +// (m_aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, m_camera, +// robotToCam); +// } catch (IOException e) { +// throw new IllegalStateException(e); +// } +// +// m_campose = new CameraPose(camName, robotToCam.getTranslation(), robotToCam.getRotation()); +// SmartDashboard.putBoolean(camName + "/update original", false); +// } +// +// /** +// * Gets optional estimatedRobotPose from AprilTag data +// * @param prevEstimatedRobotPose Pose 2d reference position for the Photon Vision pose estimator to work off of +// * @return Optional estimatedRobotPose, a Pose3d and a timestamp in seconds +// */ +// public Optional getPose(Pose2d prevEstimatedRobotPose) { +// m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); +// +// // Check for alllience switch, used mainly for non-comp testing +// if (DriverStation.getAlliance() != m_prevAlliance) { +// m_prevAlliance = DriverStation.getAlliance(); +// if (DriverStation.getAlliance() == DriverStation.Alliance.Red) { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); +// +// } else { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); +// } +// m_photonPoseEstimator.setFieldTags(m_aprilTagFieldLayout); +// } +// +// PhotonPipelineResult camResult = m_camera.getLatestResult(); +// +// if (!SmartDashboard.getBoolean(m_name + "/filtering", true)) { +// return m_photonPoseEstimator.update(camResult); +// } else { +// ArrayList goodTargets = new ArrayList<>(); +// +// double robotToTagDist; +// +// if (!camResult.hasTargets()) { +// return Optional.empty(); +// } else { +// for (PhotonTrackedTarget target : camResult.getTargets()) { +// /** +// * Get the current target position and check if it is a valid target. +// */ +// Optional targetPose = m_aprilTagFieldLayout.getTagPose(target.getFiducialId()); +// +// if (targetPose.isEmpty()) { +// continue; +// } +// +// if (SmartDashboard.getBoolean("Use Vision Filtering?", true)) { +// +// /** +// * Calculate the position of the robot based on the best target result and alternate target result. +// */ +// Pose3d bestTagPose = targetPose.get().transformBy(target.getBestCameraToTarget().inverse()) +// .transformBy(robotToCam); +// Pose3d altTagPose = targetPose.get().transformBy(target.getAlternateCameraToTarget().inverse()) +// .transformBy(robotToCam); +// +// SmartDashboard.putNumber("Best Tag X", bestTagPose.getX()); +// SmartDashboard.putNumber("Best Tag Y", bestTagPose.getY()); +// SmartDashboard.putNumber("Alt Tag X", altTagPose.getX()); +// SmartDashboard.putNumber("Alt Tag Y", altTagPose.getY()); +// +// /** +// * Calculate the distance between the bestTagPose and the previous pose. +// * And calculate the distance between the altTagPose and the previous pose. +// */ +// double bestPoseAndPrevPoseDist = Math.sqrt((bestTagPose.getX() - prevEstimatedRobotPose.getX() +// * (bestTagPose.getX() - prevEstimatedRobotPose.getX())) +// + ((bestTagPose.getY() - prevEstimatedRobotPose.getY()) +// * (bestTagPose.getY() - prevEstimatedRobotPose.getY()))); +// double altPoseAndPrevPoseDist = Math.sqrt((altTagPose.getX() - prevEstimatedRobotPose.getX() +// * (altTagPose.getX() - prevEstimatedRobotPose.getX())) +// + ((altTagPose.getY() - prevEstimatedRobotPose.getY()) +// * (altTagPose.getY() - prevEstimatedRobotPose.getY()))); +// +// /** +// * check which robot position based on the two different results is closer to the previous pose. +// * Then calculate the distance between the desired robot position and the tag position. +// */ +// if (bestPoseAndPrevPoseDist < altPoseAndPrevPoseDist) { +// /* +// robotToTagDist = Math.sqrt(((bestTagPose.getX() - targetPose.get().getX()) +// * (bestTagPose.getX() - targetPose.get().getX())) +// + ((bestTagPose.getY() - targetPose.get().getY()) +// * (bestTagPose.getY() - targetPose.get().getY()))); +// +// */ +// +// robotToTagDist = bestTagPose.minus(targetPose.get()).getTranslation().getNorm(); +// } else { +// /* +// robotToTagDist = Math.sqrt(((altTagPose.getX() - targetPose.get().getX()) +// * (altTagPose.getX() - targetPose.get().getX())) +// + ((altTagPose.getY() - targetPose.get().getY()) +// * (altTagPose.getY() - targetPose.get().getY()))); +// */ +// robotToTagDist = altTagPose.minus(targetPose.get()).getTranslation().getNorm(); +// } +// +// SmartDashboard.putNumber("Robot to Tag Distance", robotToTagDist); +// +// /** +// * Check if the ambiguity of the tag is below the predetermined threshold. +// * And check if the distance between the robot and tag is below the predetermined threshold. +// */ +// if (target.getPoseAmbiguity() <= Constants.DriveConstants.CAM_AMBIGUITY_THRESHOLD.getValue() +// && robotToTagDist < Constants.DriveConstants.CAM_DISTANCE_THRESHOLD.getValue()) { +// goodTargets.add(target); +// } +// } else { +// goodTargets = (ArrayList) camResult.getTargets(); +// } +// } +// +// +// /** +// * Create a new photon pipeline with the list of good targets +// */ +// PhotonPipelineResult filteredCamResults = new PhotonPipelineResult +// (camResult.getLatencyMillis(), goodTargets); +// filteredCamResults.setTimestampSeconds(camResult.getTimestampSeconds()); +// +// return m_photonPoseEstimator.update(filteredCamResults); +// } +// } +// } +// +// +// public Pose2d getTagPose() { +// PhotonPipelineResult results = m_camera.getLatestResult(); +// Optional pose; +// +// if (results.hasTargets()) { +// pose = m_aprilTagFieldLayout.getTagPose(results.getBestTarget().getFiducialId()); +// m_prevTag = results.getBestTarget().getFiducialId(); +// } else { +// pose = m_aprilTagFieldLayout.getTagPose(m_prevTag); +// } +// +// // return the pose of the tag currently visible to the camera +// if(pose.isPresent()) { +// return pose.get().toPose2d(); +// } else { +// return new Pose2d(); +// } +// } +// +// public Transform3d robotToTag() { +// PhotonPipelineResult result = m_camera.getLatestResult(); +// +// if (result.hasTargets()) { +// PhotonTrackedTarget target = result.getBestTarget(); +// return target.getBestCameraToTarget().plus(robotToCam.inverse()); +// } else { +// return new Transform3d(); +// } +// } +// +// public int getTagID() { +// PhotonPipelineResult results = m_camera.getLatestResult(); +// +// if (results.hasTargets()) { +// m_prevTag = results.getBestTarget().getFiducialId(); +// return results.getBestTarget().getFiducialId(); +// } else { +// return m_prevTag; +// } +// } +// +// public void useSingleTag(int tagId) { +// AprilTagFieldLayout field = new AprilTagFieldLayout( +// List.of(new AprilTag(tagId, new Pose3d())), +// 0.0, +// 0.0 +// ); +// +// m_photonPoseEstimator.setFieldTags(field); +// } +// +// public void resetTagField() { +// m_photonPoseEstimator.setFieldTags(m_aprilTagFieldLayout); +// } +// +// @Override +// public void periodic() { +// if (m_campose.hasChanged()) { +// m_photonPoseEstimator = new PhotonPoseEstimator(m_aprilTagFieldLayout, +// PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, +// m_camera, +// new Transform3d(m_campose.getTranslation(), m_campose.getRotation())); +// } +// +// if (SmartDashboard.getBoolean(m_name + "/update original", false)) { +// m_campose.setOriginal(); +// SmartDashboard.putBoolean(m_name + "/update original", false); +// } +// m_campose.updateVisualization(); +// } +//} diff --git a/src/main/java/lib/utils/PathPlannerFlipper.java b/src/main/java/lib/utils/PathPlannerFlipper.java index 0a259bbd..e0696b82 100644 --- a/src/main/java/lib/utils/PathPlannerFlipper.java +++ b/src/main/java/lib/utils/PathPlannerFlipper.java @@ -1,80 +1,80 @@ -package lib.utils; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlannerTrajectory; -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.trajectory.Trajectory; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; - -import java.util.ArrayList; -import java.util.List; - -public class PathPlannerFlipper { - public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(651.25); // Area of actual gameplay area, not carpeted area - public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(315.5); - private PathPlannerFlipper() { - new RuntimeException("Utility Class should not be constructed"); - } - - public static PathPlannerTrajectory flipTrajectory(PathPlannerTrajectory trajectory) { - List origStates = trajectory.getStates(); - List trajStatesNew = new ArrayList<>(List.of()); - - if (DriverStation.getAlliance() == DriverStation.Alliance.Red) { - for (Trajectory.State state : origStates) { - trajStatesNew.add(flipState((PathPlannerTrajectory.PathPlannerState) state, DriverStation.getAlliance())); - } - - return new PathPlannerTrajectory(trajStatesNew, - trajectory.getMarkers(), - trajectory.getStartStopEvent(), - trajectory.getEndStopEvent(), - trajectory.fromGUI); - } else { - return trajectory; - } - } - - public static List flipTrajectory(List paths) { - List reverse = List.of(); - - for (PathPlannerTrajectory p : paths) { - reverse.add(flipTrajectory(p)); - } - - return reverse; - } - - public static Trajectory.State flipState(PathPlannerTrajectory.PathPlannerState state, DriverStation.Alliance alliance) { - if (alliance == DriverStation.Alliance.Red) { - // Create a new state so that we don't overwrite the original - PathPlannerTrajectory.PathPlannerState transformedState = new PathPlannerTrajectory.PathPlannerState(); - - Translation2d transformedTranslation = - new Translation2d(FIELD_LENGTH_METERS - state.poseMeters.getX(), state.poseMeters.getY()); - - Rotation2d oldRot = state.poseMeters.getRotation(); - Rotation2d transformedHeading = new Rotation2d(oldRot.getCos(), -oldRot.getSin()); - - Rotation2d oldHolo = state.poseMeters.getRotation(); - Rotation2d transformedHolonomicRotation = new Rotation2d(oldHolo.getCos(), -oldHolo.getSin()); - - - transformedState.timeSeconds = state.timeSeconds; - transformedState.velocityMetersPerSecond = state.velocityMetersPerSecond; - transformedState.accelerationMetersPerSecondSq = state.accelerationMetersPerSecondSq; - transformedState.poseMeters = new Pose2d(transformedTranslation, transformedHeading); - transformedState.angularVelocityRadPerSec = -state.angularVelocityRadPerSec; - transformedState.holonomicRotation = transformedHolonomicRotation; - transformedState.holonomicAngularVelocityRadPerSec = -state.holonomicAngularVelocityRadPerSec; - transformedState.curvatureRadPerMeter = -state.curvatureRadPerMeter; - - return transformedState; - } else { - return state; - } - } -} \ No newline at end of file +//package lib.utils; +// +////import com.pathplanner.lib.PathConstraints; +////import com.pathplanner.lib.PathPlannerTrajectory; +//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.trajectory.Trajectory; +//import edu.wpi.first.math.util.Units; +//import edu.wpi.first.wpilibj.DriverStation; +// +//import java.util.ArrayList; +//import java.util.List; +// +//public class PathPlannerFlipper { +// public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(651.25); // Area of actual gameplay area, not carpeted area +// public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(315.5); +// private PathPlannerFlipper() { +// new RuntimeException("Utility Class should not be constructed"); +// } +// +// public static PathPlannerTrajectory flipTrajectory(PathPlannerTrajectory trajectory) { +// List origStates = trajectory.getStates(); +// List trajStatesNew = new ArrayList<>(List.of()); +// +// if (DriverStation.getAlliance() == DriverStation.Alliance.Red) { +// for (Trajectory.State state : origStates) { +// trajStatesNew.add(flipState((PathPlannerTrajectory.PathPlannerState) state, DriverStation.getAlliance())); +// } +// +// return new PathPlannerTrajectory(trajStatesNew, +// trajectory.getMarkers(), +// trajectory.getStartStopEvent(), +// trajectory.getEndStopEvent(), +// trajectory.fromGUI); +// } else { +// return trajectory; +// } +// } +// +// public static List flipTrajectory(List paths) { +// List reverse = List.of(); +// +// for (PathPlannerTrajectory p : paths) { +// reverse.add(flipTrajectory(p)); +// } +// +// return reverse; +// } +// +// public static Trajectory.State flipState(PathPlannerTrajectory.PathPlannerState state, DriverStation.Alliance alliance) { +// if (alliance == DriverStation.Alliance.Red) { +// // Create a new state so that we don't overwrite the original +// PathPlannerTrajectory.PathPlannerState transformedState = new PathPlannerTrajectory.PathPlannerState(); +// +// Translation2d transformedTranslation = +// new Translation2d(FIELD_LENGTH_METERS - state.poseMeters.getX(), state.poseMeters.getY()); +// +// Rotation2d oldRot = state.poseMeters.getRotation(); +// Rotation2d transformedHeading = new Rotation2d(oldRot.getCos(), -oldRot.getSin()); +// +// Rotation2d oldHolo = state.poseMeters.getRotation(); +// Rotation2d transformedHolonomicRotation = new Rotation2d(oldHolo.getCos(), -oldHolo.getSin()); +// +// +// transformedState.timeSeconds = state.timeSeconds; +// transformedState.velocityMetersPerSecond = state.velocityMetersPerSecond; +// transformedState.accelerationMetersPerSecondSq = state.accelerationMetersPerSecondSq; +// transformedState.poseMeters = new Pose2d(transformedTranslation, transformedHeading); +// transformedState.angularVelocityRadPerSec = -state.angularVelocityRadPerSec; +// transformedState.holonomicRotation = transformedHolonomicRotation; +// transformedState.holonomicAngularVelocityRadPerSec = -state.holonomicAngularVelocityRadPerSec; +// transformedState.curvatureRadPerMeter = -state.curvatureRadPerMeter; +// +// return transformedState; +// } else { +// return state; +// } +// } +//} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index fab5e5b8..787450f4 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,8 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.3", + "version": "2024.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -11,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.3" + "version": "2024.2.4" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.3", + "version": "2024.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -28,8 +29,10 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena" + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ] -} +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index dad31057..f0a903f2 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,20 +1,36 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.4.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "version": "v2024.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", "jniDependencies": [], "cppDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2023.4.2", - "libName": "Photon", + "artifactId": "photonlib-cpp", + "version": "v2024.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.3.1", + "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -29,13 +45,13 @@ "javaDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2023.4.2" + "artifactId": "photonlib-java", + "version": "v2024.3.1" }, { "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2023.4.2" + "artifactId": "photontargeting-java", + "version": "v2024.3.1" } ] -} \ No newline at end of file +}