diff --git a/build.gradle b/build.gradle index c4a681a8..aeaa71d9 100644 --- a/build.gradle +++ b/build.gradle @@ -91,7 +91,7 @@ test { // Simulation configuration (e.g. environment variables). wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() +wpi.sim.addDriverstation().defaultEnabled = true // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 215f72a2..9173df48 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -1,374 +1,370 @@ -package frc.robot; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.trajectory.PathPlannerTrajectory; -import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -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.Commands; -import frc.robot.constants.AligningConstants; -import frc.robot.constants.AutoConstants; -import frc.robot.constants.GlobalConstants; -import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState; -import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState.Position; -import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.Set; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - -public class Auto { - private final LoggedDashboardChooser autoChooser; - private Command previousAuto = Commands.none(); - private Alliance previousAlliance = Alliance.Blue; - private RobotConfig config; // PathPlanner robot configuration - - // #146 - private RobotContainer robotContainer; - private DriveLocation targetLocation = DriveLocation.GH; - private ArmHeight targetHeight = ArmHeight.Home; - - // *NEW - private final Field2d m_trajectoryField = new Field2d(); - - public Auto(CommandSwerveDrivetrain drivetrain, RobotContainer robotContainer) { - this.robotContainer = robotContainer; - setUpPathPlanner(drivetrain); - configureBindings(); - autoChooser = new LoggedDashboardChooser<>("Auto Routine", AutoBuilder.buildAutoChooser()); - SmartDashboard.putData("Auto Path", m_trajectoryField); - } - - public void logAutoInformation() { - Alliance currentAlliance = DriverStation.getAlliance().orElse(Alliance.Blue); - Command currentCommand = autoChooser.get(); - - if (previousAuto != currentCommand - || (!DriverStation.isEnabled() && previousAlliance != currentAlliance)) { - previousAlliance = currentAlliance; - - previousAuto = currentCommand; - - if (AutoConstants.autoList.get(currentCommand.getName()) != null) { - Logger.recordOutput( - "Auto/AutoDescription", - AutoConstants.autoList.get(currentCommand.getName())); - } else { - Logger.recordOutput( - "Auto/AutoDescription", - "404: Description not found. Validate that there is description in AutoConstants."); - } - - Command command = previousAuto; - Optional alliance = DriverStation.getAlliance(); - { - try { - var paths = PathPlannerAuto.getPathGroupFromAutoFile(command.getName()); - - if (alliance.isPresent() && alliance.get() == Alliance.Red) { - for (int i = 0; i < paths.size(); i++) { - paths.set(i, paths.get(i).flipPath()); - } - } - - var trajectories = new PathPlannerTrajectory[paths.size()]; - - for (int i = 0; i < trajectories.length; i++) { - trajectories[i] = paths.get(i).getIdealTrajectory(config).get(); - } - - // A list of all paths contained in this auto - List poses = - new ArrayList<>(); // This will be a list of all points during the auto - - for (var path : trajectories) { // For each path assigned, split into segments - for (var point : path.getStates()) { // For each segment, split into points - poses.add(point.pose); - } - } - - // Generate a trajectory from the "poses" list. This is our entire path - // "config" is used for unit conversions; Reference Field2d Widget - var m_trajectory = - TrajectoryGenerator.generateTrajectory( - poses, - new TrajectoryConfig( - Units.feetToMeters(3.0), Units.feetToMeters(3.0))); - - // Log the trajectory - m_trajectoryField.getObject("traj").setTrajectory(m_trajectory); - // Log the start and end positions - - Pose2d startingPose = trajectories[0].getInitialPose(); - Pose2d endingPose = trajectories[trajectories.length - 1].getEndState().pose; - - m_trajectoryField.getObject("start_and_end").setPoses(startingPose, endingPose); - - System.out.println("Pathplanner auto successfully shared."); - - } catch (Exception e) { - // Fallback in case the path is set to none, or the path file referenced does - // not exist - e.printStackTrace(); - System.out.println("Pathplanner file not found! Skipping..."); - m_trajectoryField.getObject("traj").setPoses(); - m_trajectoryField.getObject("start_and_end").setPoses(); - } - } - } - } - - public Command getAuto() { - if (AutoBuilder.isConfigured()) { - return autoChooser.get(); - } else { - return Commands.none(); - } - } - - public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { - config = GlobalConstants.getRobotConfigPathplanner(); - - AutoBuilder.configure( - drivetrain::getRobotPose, - drivetrain::resetPose, - drivetrain::getChassisSpeeds, - (speeds, feedforwards) -> - drivetrain.setControl(drivetrain.driveRobotRelative(speeds, feedforwards)), - new PPHolonomicDriveController( // PPHolonomicController is the built in path - // following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), - config, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - drivetrain); - - // Logging callback for target robot pose - PathPlannerLogging.setLogTargetPoseCallback( - (pose) -> { - // Do whatever you want with the pose here - m_trajectoryField.getObject("Robot").setPose(pose); - }); - } - - // #146 Constants - public static double leftOffset = AligningConstants.leftOffset; - public static double rightOffset = AligningConstants.rightOffset; - public static double centerOffset = AligningConstants.centerOffset; - - public enum DriveLocation { - SourceLeft(1, 13, 0), - SourceRight(2, 12, 0), - A(7, 18, leftOffset), - AB(7, 18, centerOffset), - B(7, 18, rightOffset), - C(8, 17, leftOffset), - CD(8, 17, centerOffset), - D(8, 17, rightOffset), - E(9, 22, leftOffset), - EF(9, 22, centerOffset), - F(9, 22, rightOffset), - G(10, 21, leftOffset), - GH(10, 21, centerOffset), - H(10, 21, rightOffset), - I(11, 20, leftOffset), - JH(11, 20, centerOffset), - J(11, 20, rightOffset), - K(6, 19, leftOffset), - KL(6, 19, centerOffset), - L(6, 19, rightOffset); - - public int tagRed; - public int tagBlue; - public double offset; - - private DriveLocation(int tagRed, int tagBlue, double offset) { - this.tagRed = tagRed; - this.tagBlue = tagBlue; - this.offset = offset; - } - - public int getTagByTeam() { - Optional ally = DriverStation.getAlliance(); - if (ally.isPresent()) { - if (ally.get() == Alliance.Red) return tagRed; - if (ally.get() == Alliance.Blue) return tagBlue; - } - - return tagRed; - } - } - - public enum ArmHeight { - Home(Position.Home, Position.Home), - L1(Position.L1, Position.L1), - L2(Position.L2, Position.L2Prep), - L3(Position.L3, Position.L3Prep), - L4(Position.L4, Position.L4Prep), - Source(Position.Source, Position.Source); - - public Position position; - public double armMotorSpeed; - public Position prep; - - private ArmHeight(Position position, Position prep) { - this.position = position; - this.prep = prep; - } - } - - // #146: Add a function that will register all triggers - private double placeTimeout = 0.5; - - public void configureBindings() { - NamedCommands.registerCommand( - "wait", Commands.waitUntil(() -> armInPlace() && robotInPlace())); - - Command alignCommand = - Commands.defer( - () -> - robotContainer.alignAndDriveToReef( - targetLocation.getTagByTeam(), targetLocation.offset), - Set.of(robotContainer.drivetrain)); - NamedCommands.registerCommand("align", alignCommand); - - Command armCommand = - Commands.defer( - () -> robotContainer.stateManager.moveToPosition(targetHeight.position), - Set.of( - robotContainer.armSubsystem, - robotContainer.elevatorSubsystem, - robotContainer.wristSubsystem, - robotContainer.stateManager)); - NamedCommands.registerCommand("arm", armCommand.asProxy()); - - Command prepArmCommand = - Commands.defer( - () -> robotContainer.stateManager.moveToPosition(targetHeight.prep), - Set.of( - robotContainer.armSubsystem, - robotContainer.elevatorSubsystem, - robotContainer.wristSubsystem, - robotContainer.stateManager)); - NamedCommands.registerCommand("preparm", prepArmCommand.asProxy()); - - Command scoreCommand = - robotContainer.gripperSubsystem.ejectSpinCoral().withTimeout(placeTimeout); - NamedCommands.registerCommand("score", scoreCommand.asProxy()); - - Command intakeCommand = robotContainer.intakeSubsystem.intake(); - NamedCommands.registerCommand("intake", intakeCommand.asProxy()); - - // spotless:off - Command placeCommand = - prepArmCommand.asProxy() - .until(() -> robotInPlace()) // Wait until arm and align are in position - .andThen( - ( - Commands.waitUntil(() -> armInPlace()) - .andThen(scoreCommand.asProxy()) - ) - .deadlineFor(armCommand.asProxy()) - ) - .deadlineFor(alignCommand); - NamedCommands.registerCommand("place", placeCommand); - // spotless:on - - // #region Auto create locations and heights - for (DriveLocation location : DriveLocation.values()) { - NamedCommands.registerCommand( - "location ".concat(location.name()), - Commands.runOnce( - () -> { - targetLocation = location; - Logger.recordOutput("Auto/Chosen Location", location); - })); - } - - for (ArmHeight height : ArmHeight.values()) { - NamedCommands.registerCommand( - "height ".concat(height.name()), - Commands.runOnce( - () -> { - targetHeight = height; - Logger.recordOutput("Auto/Chosen Height", height); - })); - } - // #endregion - - } - - @AutoLogOutput(key = "Auto/Arm In Place") - private boolean armInPlace() { - return SuperstructureState.AUTO.checksOut( - targetHeight.position, robotContainer.stateManager); - } - - @AutoLogOutput(key = "Auto/Arm In Prep") - private boolean armInPrep() { - return SuperstructureState.AUTO.checksOut(targetHeight.prep, robotContainer.stateManager); - } - - @AutoLogOutput(key = "Auto/Robot In Place") - private boolean robotInPlace() { - Pose2d alignmentPose = - VisionConstants.aprilTagLayout - .getTagPose(targetLocation.getTagByTeam()) - .get() - .toPose2d() - .plus( - new Transform2d( - new Translation2d( - Units.feetToMeters(3) / 2, targetLocation.offset), - Rotation2d.k180deg)); - Logger.recordOutput("Auto/Physical Target Pose", alignmentPose); - Pose2d currentPose = robotContainer.drivetrain.getRobotPose(); - Pose2d relativePos = alignmentPose.relativeTo(currentPose); - Logger.recordOutput("Auto/Physical Relative Pose", relativePos); - return (Math.abs(relativePos.getX()) < Units.inchesToMeters(0.7)) - && (Math.abs(relativePos.getY()) < Units.inchesToMeters(0.7)) - && ((Math.abs(relativePos.getRotation().getRadians()) % Math.PI) - < Units.degreesToRadians(2)); - } - - public static boolean robotInPlace(Pose2d robotPose, Pose2d pose, double offset) { - Pose2d alignmentPose = - pose.plus( - new Transform2d( - new Translation2d(Units.feetToMeters(3) / 2, offset), - Rotation2d.k180deg)); - Pose2d offsetPose = robotPose.relativeTo(alignmentPose); - return (offsetPose.getX() > -Units.inchesToMeters(0.7)) - && (Math.abs(offsetPose.getY()) < Units.inchesToMeters(0.7)) - && ((Math.abs(offsetPose.getRotation().getRadians()) % (2 * Math.PI)) - < Units.degreesToRadians(2)); - } -} +// package frc.robot; + +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.auto.NamedCommands; +// import com.pathplanner.lib.commands.PathPlannerAuto; +// import com.pathplanner.lib.config.PIDConstants; +// import com.pathplanner.lib.config.RobotConfig; +// import com.pathplanner.lib.controllers.PPHolonomicDriveController; +// import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +// import com.pathplanner.lib.util.PathPlannerLogging; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Transform2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// 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.Commands; +// import frc.robot.constants.AligningConstants; +// import frc.robot.constants.AutoConstants; +// import frc.robot.constants.GlobalConstants; +// import frc.robot.constants.VisionConstants; +// import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; +// import java.util.ArrayList; +// import java.util.List; +// import java.util.Optional; +// import java.util.Set; +// import org.littletonrobotics.junction.AutoLogOutput; +// import org.littletonrobotics.junction.Logger; +// import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +// public class Auto { +// private final LoggedDashboardChooser autoChooser; +// private Command previousAuto = Commands.none(); +// private Alliance previousAlliance = Alliance.Blue; +// private RobotConfig config; // PathPlanner robot configuration + +// // #146 +// private RobotContainer robotContainer; +// private DriveLocation targetLocation = DriveLocation.GH; +// private ArmHeight targetHeight = ArmHeight.Home; + +// // *NEW +// private final Field2d m_trajectoryField = new Field2d(); + +// public Auto(CommandSwerveDrivetrain drivetrain, RobotContainer robotContainer) { +// this.robotContainer = robotContainer; +// setUpPathPlanner(drivetrain); +// configureBindings(); +// autoChooser = new LoggedDashboardChooser<>("Auto Routine", +// AutoBuilder.buildAutoChooser()); +// SmartDashboard.putData("Auto Path", m_trajectoryField); +// } + +// public void logAutoInformation() { +// Alliance currentAlliance = DriverStation.getAlliance().orElse(Alliance.Blue); +// Command currentCommand = autoChooser.get(); + +// if (previousAuto != currentCommand +// || (!DriverStation.isEnabled() && previousAlliance != currentAlliance)) { +// previousAlliance = currentAlliance; + +// previousAuto = currentCommand; + +// if (AutoConstants.autoList.get(currentCommand.getName()) != null) { +// Logger.recordOutput( +// "Auto/AutoDescription", +// AutoConstants.autoList.get(currentCommand.getName())); +// } else { +// Logger.recordOutput( +// "Auto/AutoDescription", +// "404: Description not found. Validate that there is description in +// AutoConstants."); +// } + +// Command command = previousAuto; +// Optional alliance = DriverStation.getAlliance(); +// { +// try { +// var paths = PathPlannerAuto.getPathGroupFromAutoFile(command.getName()); + +// if (alliance.isPresent() && alliance.get() == Alliance.Red) { +// for (int i = 0; i < paths.size(); i++) { +// paths.set(i, paths.get(i).flipPath()); +// } +// } + +// var trajectories = new PathPlannerTrajectory[paths.size()]; + +// for (int i = 0; i < trajectories.length; i++) { +// trajectories[i] = paths.get(i).getIdealTrajectory(config).get(); +// } + +// // A list of all paths contained in this auto +// List poses = +// new ArrayList<>(); // This will be a list of all points during the +// auto + +// for (var path : trajectories) { // For each path assigned, split into +// segments +// for (var point : path.getStates()) { // For each segment, split into +// points +// poses.add(point.pose); +// } +// } + +// // Generate a trajectory from the "poses" list. This is our entire path +// // "config" is used for unit conversions; Reference Field2d Widget +// var m_trajectory = +// TrajectoryGenerator.generateTrajectory( +// poses, +// new TrajectoryConfig( +// Units.feetToMeters(3.0), Units.feetToMeters(3.0))); + +// // Log the trajectory +// m_trajectoryField.getObject("traj").setTrajectory(m_trajectory); +// // Log the start and end positions + +// Pose2d startingPose = trajectories[0].getInitialPose(); +// Pose2d endingPose = trajectories[trajectories.length - 1].getEndState().pose; + +// m_trajectoryField.getObject("start_and_end").setPoses(startingPose, +// endingPose); + +// System.out.println("Pathplanner auto successfully shared."); + +// } catch (Exception e) { +// // Fallback in case the path is set to none, or the path file referenced does +// // not exist +// e.printStackTrace(); +// System.out.println("Pathplanner file not found! Skipping..."); +// m_trajectoryField.getObject("traj").setPoses(); +// m_trajectoryField.getObject("start_and_end").setPoses(); +// } +// } +// } +// } + +// public Command getAuto() { +// if (AutoBuilder.isConfigured()) { +// return autoChooser.get(); +// } else { +// return Commands.none(); +// } +// } + +// public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { +// config = GlobalConstants.getRobotConfigPathplanner(); + +// AutoBuilder.configure( +// drivetrain::getRobotPose, +// drivetrain::resetPose, +// drivetrain::getChassisSpeeds, +// (speeds, feedforwards) -> +// drivetrain.setControl(drivetrain.driveRobotRelative(speeds, +// feedforwards)), +// new PPHolonomicDriveController( // PPHolonomicController is the built in path +// // following controller for holonomic drive trains +// new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants +// new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants +// ), +// config, +// () -> { +// // Boolean supplier that controls when the path will be mirrored for the red +// // alliance +// Optional alliance = DriverStation.getAlliance(); +// if (alliance.isPresent()) { +// return alliance.get() == DriverStation.Alliance.Red; +// } +// return false; +// }, +// drivetrain); + +// // Logging callback for target robot pose +// PathPlannerLogging.setLogTargetPoseCallback( +// (pose) -> { +// // Do whatever you want with the pose here +// m_trajectoryField.getObject("Robot").setPose(pose); +// }); +// } + +// // #146 Constants +// public static double leftOffset = AligningConstants.leftOffset; +// public static double rightOffset = AligningConstants.rightOffset; +// public static double centerOffset = AligningConstants.centerOffset; + +// public enum DriveLocation { +// SourceLeft(1, 13, 0), +// SourceRight(2, 12, 0), +// A(7, 18, leftOffset), +// AB(7, 18, centerOffset), +// B(7, 18, rightOffset), +// C(8, 17, leftOffset), +// CD(8, 17, centerOffset), +// D(8, 17, rightOffset), +// E(9, 22, leftOffset), +// EF(9, 22, centerOffset), +// F(9, 22, rightOffset), +// G(10, 21, leftOffset), +// GH(10, 21, centerOffset), +// H(10, 21, rightOffset), +// I(11, 20, leftOffset), +// JH(11, 20, centerOffset), +// J(11, 20, rightOffset), +// K(6, 19, leftOffset), +// KL(6, 19, centerOffset), +// L(6, 19, rightOffset); + +// public int tagRed; +// public int tagBlue; +// public double offset; + +// private DriveLocation(int tagRed, int tagBlue, double offset) { +// this.tagRed = tagRed; +// this.tagBlue = tagBlue; +// this.offset = offset; +// } + +// public int getTagByTeam() { +// Optional ally = DriverStation.getAlliance(); +// if (ally.isPresent()) { +// if (ally.get() == Alliance.Red) return tagRed; +// if (ally.get() == Alliance.Blue) return tagBlue; +// } + +// return tagRed; +// } +// } + +// public enum position { +// Home(Position.Home, Position.Home), +// L1(Position.L1, Position.L1), +// L2(Position.L2, Position.L2Prep), +// L3(Position.L3, Position.L3Prep), +// L4(Position.L4, Position.L4Prep), +// Source(Position.Source, Position.Source); + +// public Position position; +// public double armMotorSpeed; +// public Position prep; + +// private ArmHeight(Position position, Position prep) { +// this.position = position; +// this.prep = prep; +// } +// } + +// // #146: Add a function that will register all triggers +// private double placeTimeout = 0.5; + +// public void configureBindings() { +// NamedCommands.registerCommand( +// "wait", Commands.waitUntil(() -> armInPlace() && robotInPlace())); + +// Command alignCommand = +// Commands.defer( +// () -> +// robotContainer.alignAndDriveToReef( +// targetLocation.getTagByTeam(), targetLocation.offset), +// Set.of(robotContainer.drivetrain)); +// NamedCommands.registerCommand("align", alignCommand); + +// Command armCommand = +// Commands.defer( +// () -> robotContainer.stateManager.moveToPosition(targetHeight.position), +// Set.of( +// robotContainer.armSubsystem, +// robotContainer.elevatorSubsystem, +// robotContainer.stateManager)); +// NamedCommands.registerCommand("arm", armCommand.asProxy()); + +// Command scoreCommand = +// robotContainer.gripperSubsystem.ejectReverse(12).withTimeout(placeTimeout); +// NamedCommands.registerCommand("score", scoreCommand.asProxy()); + +// Command intakeCommand = robotContainer.intakeSubsystem.intake(); +// NamedCommands.registerCommand("intake", intakeCommand.asProxy()); + +// // spotless:off +// Command placeCommand = +// prepArmCommand.asProxy() +// .until(() -> robotInPlace()) // Wait until arm and align are in position +// .andThen( +// ( +// Commands.waitUntil(() -> armInPlace()) +// .andThen(scoreCommand.asProxy()) +// ) +// .deadlineFor(armCommand.asProxy()) +// ) +// .deadlineFor(alignCommand); +// NamedCommands.registerCommand("place", placeCommand); +// // spotless:on + +// // #region Auto create locations and heights +// for (DriveLocation location : DriveLocation.values()) { +// NamedCommands.registerCommand( +// "location ".concat(location.name()), +// Commands.runOnce( +// () -> { +// targetLocation = location; +// Logger.recordOutput("Auto/Chosen Location", location); +// })); +// } + +// for (ArmHeight height : ArmHeight.values()) { +// NamedCommands.registerCommand( +// "height ".concat(height.name()), +// Commands.runOnce( +// () -> { +// targetHeight = height; +// Logger.recordOutput("Auto/Chosen Height", height); +// })); +// } +// // #endregion + +// } + +// @AutoLogOutput(key = "Auto/Arm In Place") +// private boolean armInPlace() { +// return SuperstructureState.AUTO.checksOut( +// targetHeight.position, robotContainer.stateManager); +// } + +// @AutoLogOutput(key = "Auto/Arm In Prep") +// private boolean armInPrep() { +// return SuperstructureState.AUTO.checksOut(targetHeight.prep, +// robotContainer.stateManager); +// } + +// @AutoLogOutput(key = "Auto/Robot In Place") +// private boolean robotInPlace() { +// Pose2d alignmentPose = +// VisionConstants.aprilTagLayout +// .getTagPose(targetLocation.getTagByTeam()) +// .get() +// .toPose2d() +// .plus( +// new Transform2d( +// new Translation2d( +// Units.feetToMeters(3) / 2, +// targetLocation.offset), +// Rotation2d.k180deg)); +// Logger.recordOutput("Auto/Physical Target Pose", alignmentPose); +// Pose2d currentPose = robotContainer.drivetrain.getRobotPose(); +// Pose2d relativePos = alignmentPose.relativeTo(currentPose); +// Logger.recordOutput("Auto/Physical Relative Pose", relativePos); +// return (Math.abs(relativePos.getX()) < Units.inchesToMeters(0.7)) +// && (Math.abs(relativePos.getY()) < Units.inchesToMeters(0.7)) +// && ((Math.abs(relativePos.getRotation().getRadians()) % Math.PI) +// < Units.degreesToRadians(2)); +// } + +// public static boolean robotInPlace(Pose2d robotPose, Pose2d pose, double offset) { +// Pose2d alignmentPose = +// pose.plus( +// new Transform2d( +// new Translation2d(Units.feetToMeters(3) / 2, offset), +// Rotation2d.k180deg)); +// Pose2d offsetPose = robotPose.relativeTo(alignmentPose); +// return (offsetPose.getX() > -Units.inchesToMeters(0.7)) +// && (Math.abs(offsetPose.getY()) < Units.inchesToMeters(0.7)) +// && ((Math.abs(offsetPose.getRotation().getRadians()) % (2 * Math.PI)) +// < Units.degreesToRadians(2)); +// } +// } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e84838f3..bdabdeb4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,7 +71,7 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - m_robotContainer.auto.logAutoInformation(); + // m_robotContainer.auto.logAutoInformation(); } @Override @@ -85,7 +85,7 @@ public void disabledExit() {} @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_autonomousCommand = null; // m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 901aeeb6..7b570581 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,11 +10,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -23,31 +20,26 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.controller.LogitechController; import frc.lib.controller.ThrustmasterJoystick; -import frc.lib.vision.PinholeModel3D; import frc.robot.commands.AlignAndDriveToReef; -import frc.robot.commands.AlignToPiece; import frc.robot.commands.AlignToReef; import frc.robot.commands.WheelRadiusCharacterization; -import frc.robot.constants.AligningConstants; import frc.robot.constants.GlobalConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; import frc.robot.constants.TunerConstants; import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.ModeManager.SuperstructureStateManager; -import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState.Position; +import frc.robot.subsystems.ModeManager.ModeManager; +import frc.robot.subsystems.ModeManager.ModeManager.CoralAlgaeMode; +import frc.robot.subsystems.ModeManager.ModeManager.State.Position; import frc.robot.subsystems.arm.ArmPivotIOSim; import frc.robot.subsystems.arm.ArmPivotIOTalonFX; import frc.robot.subsystems.arm.ArmSubsystem; -import frc.robot.subsystems.chute.ChuteIONeo550; -import frc.robot.subsystems.chute.ChuteIOSim; -import frc.robot.subsystems.chute.ChuteSubsystem; import frc.robot.subsystems.climber.ClimberHeadIONeo550; import frc.robot.subsystems.climber.ClimberIOTalonFX; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorIOSim; import frc.robot.subsystems.elevator.ElevatorIOTalonFX; import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.gripper.GripperIOFalcon; +import frc.robot.subsystems.gripper.GripperIONeo550; import frc.robot.subsystems.gripper.GripperIOSim; import frc.robot.subsystems.gripper.GripperSubsystem; import frc.robot.subsystems.intake.FlipperIOSim; @@ -60,10 +52,6 @@ import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.robot.subsystems.vision.VisionIOPhotonVisionSimML; -import frc.robot.subsystems.wrist.WristIONeo550; -import frc.robot.subsystems.wrist.WristIOSim; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.util.Elastic; import java.util.Set; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -78,16 +66,16 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - public Auto auto; // #146: Pass in RobotContainer + // public Auto auto; // #146: Pass in RobotContainer public IntakeSubsystem intakeSubsystem; public ElevatorSubsystem elevatorSubsystem; public ClimberSubsystem climberSubsystem; public ArmSubsystem armSubsystem; - public WristSubsystem wristSubsystem; + public Vision vision; public LightsSubsystem lights; - public ChuteSubsystem chuteSubsystem; - public SuperstructureStateManager stateManager; + + public ModeManager stateManager; public GripperSubsystem gripperSubsystem; private DoubleSupplier leftJoystickVelocityX; @@ -118,19 +106,18 @@ public RobotContainer() { VisionConstants.camera1Name, () -> drivetrain.getRobotPose().getRotation()), new DummyPhotonCamera()); - gripperSubsystem = - new GripperSubsystem(new GripperIOFalcon()); // new GripperIOFalcon()); elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX()); // new ElevatorIOTalonFX()); armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX()); - wristSubsystem = new WristSubsystem(new WristIONeo550()); // new WristIONeo550()); + + gripperSubsystem = new GripperSubsystem(new GripperIONeo550()); + climberSubsystem = new ClimberSubsystem( new ClimberIOTalonFX(), new ClimberHeadIONeo550()); // new ClimberIOTalonFX(), new // ClimberHeadIONeo550()); lights = new LightsSubsystem(); - chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550()); // new ChuteIONeo550()); intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim()); } else { @@ -153,23 +140,16 @@ public RobotContainer() { gripperSubsystem = new GripperSubsystem(new GripperIOSim()); elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOSim()); armSubsystem = new ArmSubsystem(new ArmPivotIOSim()); - wristSubsystem = new WristSubsystem(new WristIOSim()); intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim()); climberSubsystem = new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550()); lights = new LightsSubsystem(); - chuteSubsystem = new ChuteSubsystem(new ChuteIOSim()); } - stateManager = - new SuperstructureStateManager( - elevatorSubsystem, armSubsystem, wristSubsystem, chuteSubsystem); - - auto = new Auto(drivetrain, this); - - wristSubsystem.elevatorHeight = () -> elevatorSubsystem.getPosition(); - wristSubsystem.armHeight = () -> armSubsystem.getPosition(); + // auto = new Auto(drivetrain, this); + stateManager = new ModeManager(elevatorSubsystem, armSubsystem); + stateManager.setDefaultCommand(Commands.runOnce(() -> {}, stateManager)); configureBindings(); // Establish the "Trajectory Field" Field2d into the dashboard } @@ -213,26 +193,13 @@ private void configureBindings() { DROP_TRIGGER = leftDriveController.getTrigger(); final Trigger EJECT_TRIGGER = rightDriveController.getTrigger(); - final Trigger ALIGN_TRIGGER = rightDriveController.getBottomThumb(); - final Trigger NORMAL_ALIGN = - (stateManager - .STATION_GRIPPER - .or(stateManager.STATION_HANDOFF) - .or(stateManager.PROCESSOR)) - .negate(); - - USING_AUTO_ALIGN = new Trigger(ALIGN_TRIGGER); + // final Trigger ALIGN_TRIGGER = rightDriveController.getBottomThumb(); + // final Trigger NORMAL_ALIGN = + // (stateManager.STATION_GRIPPER.or(stateManager.PROCESSOR)).negate(); - AUTO_ALIGNED = - new Trigger( - () -> { - return Auto.robotInPlace( - drivetrain.getRobotPose(), - stateManager.getLastScoringPose(), - stateManager.getLastScoringOffset()); - }); + // USING_AUTO_ALIGN = new Trigger(ALIGN_TRIGGER); - AUTO_DRIVER_TRIGGER = (USING_AUTO_ALIGN.negate().or(AUTO_ALIGNED)).and(DROP_TRIGGER); + // AUTO_DRIVER_TRIGGER = (USING_AUTO_ALIGN.negate().or(AUTO_ALIGNED)).and(DROP_TRIGGER); // drive.withVelocityX(-leftDriveController.getYAxis().get() * // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y @@ -384,9 +351,23 @@ private void configureBindings() { // .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(Pose2d.kZero))); // Operator Mode Setting - operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode()); - operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode()); - operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode()); + operatorController + .getLeftBumper() + .onTrue( + Commands.runOnce( + () -> stateManager.setMode(CoralAlgaeMode.LeftCoral), + stateManager)); + operatorController + .getRightBumper() + .onTrue( + Commands.runOnce( + () -> stateManager.setMode(CoralAlgaeMode.RightCoral), + stateManager)); + operatorController + .getRightTrigger() + .onTrue( + Commands.runOnce( + () -> stateManager.setMode(CoralAlgaeMode.Algae), stateManager)); Trigger LEFT_JOYSTICK_BUMP = new Trigger( @@ -417,64 +398,33 @@ private void configureBindings() { (() -> LightsSubsystem.LEDSegment.MainStrip.setRainbowAnimation(1)), () -> {})); // L3 Rainbow - // Coral Mode Bindings - final Trigger CORAL = stateManager.LEFT_CORAL.or(stateManager.RIGHT_CORAL); - final Trigger ALGAE = stateManager.ALGAE; - final Trigger ARMWRIST = stateManager.ARMWRIST; - ARMWRIST.and(operatorController.getY()).whileTrue(armSubsystem.armpivotUp()); - ARMWRIST.and(operatorController.getA()).whileTrue(armSubsystem.armpivotDown()); - ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft()); - ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight()); - // CORAL.and(operatorController.getY()) // .onTrue(stateManager.moveToPosition(Position.L4Prep)) // .onFalse(stateManager.moveToPosition(Position.L4)); // CORAL.and(operatorController.getX()) // .onTrue(stateManager.moveToPosition(Position.L3Prep)) // .onFalse(stateManager.moveToPosition(Position.L3)); - // CORAL.and(operatorController.getB()) - // .onTrue(stateManager.moveToPosition(Position.L2Prep)) - // .onFalse(stateManager.moveToPosition(Position.L2)); - // CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1)); - - operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Climb)); - - bindPlaceSeq(CORAL.and(operatorController.getY()), Position.L4Prep, Position.L4, 0.1); - - bindPlaceSeq(CORAL.and(operatorController.getX()), Position.L3Prep, Position.L3, 0.1); - - bindPlaceSeq(CORAL.and(operatorController.getB()), Position.L2Prep, Position.L2, 0.1); - - operatorController.getA().onTrue(stateManager.moveToPosition(Position.L1)); - - CORAL.and(operatorController.getStart()) - .onTrue(stateManager.moveToPosition(Position.Source)); - - CORAL.and(operatorController.getDPadDown()) - .onTrue(stateManager.moveToPosition(Position.Home)); - CORAL.and(operatorController.getDPadUp()) - .onFalse(stateManager.moveToPosition(Position.HandoffPrep)) - .onTrue(stateManager.moveToPosition(Position.Handoff)); - CORAL.and(operatorController.getDPadUp()).whileTrue(gripperSubsystem.intakeSpinCoral()); - - CORAL.and(operatorController.getDPadLeft()).onTrue(chuteSubsystem.moveChuteUp()); - CORAL.and(operatorController.getDPadRight()).onTrue(chuteSubsystem.moveChuteDown()); - - // ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.NetAlgae)); - ALGAE.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3Algae)); - ALGAE.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2Algae)); - ALGAE.and(operatorController.getA()) - .onTrue(stateManager.moveToPosition(Position.Processor)); - ALGAE.and(operatorController.getStart()) - .onTrue(stateManager.moveToPosition(Position.GroundAlgae)); - ALGAE.and(operatorController.getDPadDown()) - .onTrue(stateManager.moveToPosition(Position.AlgaeHome)); - // ALGAE.and(operatorController.getDPadUp()) - // .onTrue(stateManager.moveToPosition(Position.Handoff)); - ALGAE.and(operatorController.getDPadLeft()) - .onTrue(stateManager.moveToPosition(Position.Quick34)); - ALGAE.and(operatorController.getDPadRight()) - .onTrue(stateManager.moveToPosition(Position.Quick23)); + // CORAL + + operatorController.getLeftTrigger().onTrue(stateManager.setGoal(Position.Climb)); + + operatorController.getY().onTrue(stateManager.setGoal(Position.L4)); + + operatorController.getY().onTrue(stateManager.setGoal(Position.L4)); + + // operatorController.getY().onTrue(Commands.runOnce(() -> System.out.println("Y + // PRESSED"))); + operatorController.getX().onTrue(stateManager.setGoal(Position.L3)); + + operatorController.getB().onTrue(stateManager.setGoal(Position.L2)); + + operatorController.getA().onTrue(stateManager.setGoal(Position.L1)); + + operatorController.getDPadDown().onTrue(stateManager.setGoal(Position.Home)); + operatorController.getDPadUp().onTrue(stateManager.setGoal(Position.Handoff)); + + operatorController.getDPadLeft().onTrue(stateManager.setGoal(Position.Quick3)); + operatorController.getDPadRight().onTrue(stateManager.setGoal(Position.Quick2)); // operatorController.getBack().onTrue(wristSubsystem.flipWristPosition()); @@ -496,87 +446,25 @@ private void configureBindings() { lights.setAlgaeModeSupplier(gripperSubsystem.HAS_PIECE); - CORAL.and(rightDriveController.getLeftThumb()) + (rightDriveController.getLeftThumb()) .whileTrue( gripperSubsystem - .intakeSpinCoral() + .ejectReverse(12) .withDeadline( Commands.waitSeconds(0.2) .andThen( Commands.waitUntil( gripperSubsystem.HAS_PIECE)))); - CORAL.and(EJECT_TRIGGER).whileTrue(gripperSubsystem.ejectSpinCoral()); - - ALGAE.and(rightDriveController.getLeftThumb()).onTrue(gripperSubsystem.intakeSpinAlgae()); - ALGAE.and(EJECT_TRIGGER) - .and(stateManager.PROCESSOR) - .whileTrue(gripperSubsystem.slowEjectSpinAlgae()); - ALGAE.and(EJECT_TRIGGER) - .and(stateManager.PROCESSOR.negate()) - .whileTrue(gripperSubsystem.ejectSpinAlgae()); - - ALIGN_TRIGGER.onTrue( - Commands.runOnce( - () -> - stateManager.setLastScoringPose( - drivetrain.findNearestAprilTagPose()))); - - stateManager - .LEFT_CORAL - .and(ALIGN_TRIGGER) - .whileTrue(alignToReef(AligningConstants.leftOffset)); - - stateManager - .ALGAE - .and(ALIGN_TRIGGER) - .whileTrue(alignToReef(AligningConstants.centerOffset)); - - stateManager - .RIGHT_CORAL - .and(ALIGN_TRIGGER) - .whileTrue(alignToReef(AligningConstants.rightOffset)); - - // stateManager - // .ALGAE - // .and(ALIGN_TRIGGER.and(stateManager.PROCESSOR)) - // .whileTrue(alignToProcessor()); - - // stateManager - // .LEFT_CORAL - // .and(ALIGN_TRIGGER.and(stateManager.STATION_GRIPPER)) - // .whileTrue(alignToStation(0.6, Rotation2d.kZero)); - - // stateManager - // .ALGAE - // .and(ALIGN_TRIGGER.and(stateManager.STATION_GRIPPER)) - // .whileTrue(alignToStation(0.0, Rotation2d.kZero)); - - // stateManager - // .RIGHT_CORAL - // .and(ALIGN_TRIGGER.and(stateManager.STATION_GRIPPER)) - // .whileTrue(alignToStation(-0.6, Rotation2d.kZero)); - - // stateManager - // .LEFT_CORAL - // .and(ALIGN_TRIGGER.and(stateManager.STATION_HANDOFF)) - // .whileTrue(alignToStation(0.6, Rotation2d.k180deg)); - - // stateManager - // .ALGAE - // .and(ALIGN_TRIGGER.and(stateManager.STATION_HANDOFF)) - // .whileTrue(alignToStation(0.0, Rotation2d.k180deg)); - - // stateManager - // .RIGHT_CORAL - // .and(ALIGN_TRIGGER.and(stateManager.STATION_HANDOFF)) - // .whileTrue(alignToStation(-0.6, Rotation2d.k180deg)); + (EJECT_TRIGGER).whileTrue(gripperSubsystem.ejectReverse(12)); + + rightDriveController + .getBottomThumb() + .whileTrue(alignToReef(stateManager.getAligningOffset())); // Technical Bindings leftDriveController.getLeftBottomMiddle().onTrue(climberSubsystem.zeroClimberCommand()); - rightDriveController - .getLeftBottomMiddle() - .onTrue(stateManager.moveToPosition(Position.Start)); + rightDriveController.getLeftBottomMiddle().onTrue(stateManager.setGoal(Position.Start)); leftDriveController.getLeftTopMiddle().whileTrue(climberSubsystem.climberTuneable()); rightDriveController @@ -589,65 +477,13 @@ private void configureBindings() { // leftDriveController.getLeftBottomLeft().whileTrue(intakeSubsystem.rollerTuneable()); // leftDriveController.getLeftTopRight().whileTrue(intakeSubsystem.flipperTuneable()); - leftDriveController.getLeftBottomLeft().whileTrue(chuteSubsystem.moveChuteUp()); - leftDriveController.getLeftTopRight().whileTrue(chuteSubsystem.moveChuteDown()); - leftDriveController.getLeftBottomRight().onTrue(intakeSubsystem.zeroflipper()); - leftDriveController.getLeftTopLeft().whileTrue(gripperSubsystem.gripperTuneable()); - { - var tunableCommand = - Commands.runOnce( - () -> { - Elastic.sendNotification( - new Elastic.Notification( - Elastic.Notification.NotificationLevel.INFO, - "Scheduled Supestructure Tunable", - "YAYYAYYA.")); - }) - .andThen(stateManager.moveToTunablePosition()); - - tunableCommand.setName("Tunable Superstructure"); - - leftDriveController - .getRightTopLeft() - .onTrue( - Commands.runOnce( - () -> { - tunableCommand.cancel(); - tunableCommand.schedule(); - })); - - SmartDashboard.putData(tunableCommand); - - SmartDashboard.putData(stateManager); - } + // leftDriveController.getLeftTopLeft().whileTrue(gripperSubsystem.gripperTuneable()); - // leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand()); - } + SmartDashboard.putData(stateManager); - private void bindPlaceSeq(Trigger button, Position prep, Position end, double timeout) { - (button) - .onTrue( - (stateManager - .moveToPosition(prep) - .until(DROP_TRIGGER) - .andThen( - stateManager - .moveToPosition(end) - // .alongWith( - // - // Commands.waitSeconds(timeout) - // - // .andThen( - // - // gripperSubsystem - // - // .ejectSpinCoral())) - .until(DROP_TRIGGER.negate()))) - .repeatedly() - .beforeStarting(() -> normalRelease.setPressed(false)) - .finallyDo(() -> normalRelease.setPressed(true))); + // leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand()); } private double deadband(double value, double deadband) { @@ -662,9 +498,9 @@ private double sps(double value) { return value * Math.abs(value); } - public Command getAutonomousCommand() { - return auto.getAuto(); - } + // public Command getAutonomousCommand() { + // return auto.getAuto(); + // } public Command alignToReef(int tag, double offset, Rotation2d rotOffset) { Pose2d alignmentPose = @@ -692,28 +528,19 @@ public Command alignToReef(int tag, double offset) { // Automatically chooses closest tag public Command alignToReef(double offset) { return Commands.defer( - () -> { - Pose2d alignmentPose = - stateManager - .getLastScoringPose() - .plus( - new Transform2d( - new Translation2d( - Units.feetToMeters(3) / 2, - offset), - new Rotation2d())); - // return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, - // Rotation2d.kPi); - return new AlignToReef( - drivetrain, - leftJoystickVelocityX, - leftJoystickVelocityY, - 0, - alignmentPose, - Rotation2d.kPi); - }, - Set.of(drivetrain)) - .beforeStarting(() -> stateManager.setLastScoringOffset(offset)); + () -> { + Pose2d alignmentPose = + VisionConstants.aprilTagLayout.getTagPose(3).get().toPose2d(); + + return new AlignToReef( + drivetrain, + leftJoystickVelocityX, + leftJoystickVelocityY, + 0, + alignmentPose, + Rotation2d.kPi); + }, + Set.of(drivetrain)); } public Command alignAndDriveToReef(int tag, double offset) { @@ -728,71 +555,4 @@ public Command alignAndDriveToReef(int tag, double offset) { new Rotation2d())); return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, Rotation2d.kPi); } - - public Command alignToPiece() { - Supplier piecePositionSupplier = - () -> { - var lastObservation = vision.getLastTargetObersevation(2); - Pose2d robotPose = drivetrain.getRobotPose(); - Translation2d lastPieceTranslation = - PinholeModel3D.getTranslationToTarget( - new Translation3d( - 1, - lastObservation.tx().unaryMinus().getTan(), - lastObservation.ty().getTan()), - VisionConstants.robotToCamera2, - 0); - Pose2d poseAtTime = robotPose; - - Pose2d newPiecePose = - poseAtTime.plus( - new Transform2d(lastPieceTranslation, new Rotation2d())); - - return newPiecePose; - }; - return new AlignToPiece( - drivetrain, - driverVelocitySupplier, - .15, - piecePositionSupplier, - Rotation2d.kCCW_90deg); - } - - public Command alignToProcessor() { - // getAlignmentcolor 3 = r - // set a trigger based on the thumbpad - // - return Commands.either( - alignToReef(3, 0), - alignToReef(16, 0), - () -> { - return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; - }); - } - - public Command alignToStation(double offset, Rotation2d rotOffset) { - // getAlignmentcolor 3 = r - // set a trigger based on the thumbpad - // - return Commands.defer( - () -> { - int closestTag = 0; - Pose2d currentPose = drivetrain.getRobotPose(); - if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { - if (currentPose.getY() > 4.026) { - closestTag = 2; - } else { - closestTag = 1; - } - } else { - if (currentPose.getY() > 4.026) { - closestTag = 13; - } else { - closestTag = 12; - } - } - return alignToReef(closestTag, offset, rotOffset); - }, - Set.of(drivetrain)); - } } diff --git a/src/main/java/frc/robot/constants/GripperConstants.java b/src/main/java/frc/robot/constants/GripperConstants.java index e0cb037d..6d45154d 100644 --- a/src/main/java/frc/robot/constants/GripperConstants.java +++ b/src/main/java/frc/robot/constants/GripperConstants.java @@ -3,12 +3,13 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; public class GripperConstants { - public static final int id = 12; + public static final int leftMotorId = 11; + public static final int rightMotorId = 12; public static final CurrentLimitsConfigs currentLimit = new CurrentLimitsConfigs().withStatorCurrentLimit(50); public static final String canbus = "rio"; - public static final int gripperSensorChannel = 0; + public static final int pieceSensorChannel = 0; } diff --git a/src/main/java/frc/robot/subsystems/ModeManager/ModeManager.java b/src/main/java/frc/robot/subsystems/ModeManager/ModeManager.java new file mode 100644 index 00000000..12f88be3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ModeManager/ModeManager.java @@ -0,0 +1,88 @@ +package frc.robot.subsystems.ModeManager; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AligningConstants; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +public class ModeManager extends SubsystemBase { + + public ElevatorSubsystem elevator; + public ArmSubsystem arm; + + public ModeManager(ElevatorSubsystem elevator, ArmSubsystem arm) { + this.elevator = elevator; + this.arm = arm; + } + + public static class State { + private static LoggedNetworkNumber elevatorHeightLogged = + new LoggedNetworkNumber("Elevator Height", 170); + private static LoggedNetworkNumber armHeightLogged = + new LoggedNetworkNumber("Arm Height", 0); + + public static enum Position { + Home(170.0, 1.58), + L1(130.0, 0), + L2(90.0, -1.58), + L3(160.0, -1.58), + L4(297.0, -1.58), + Quick2(150.0, 0.0), + Quick3(250.0, 0.0), + Handoff(160.0, 1.58), + HandoffFlipped(160.0, 1.58), + Source(130.0, 0.0), + Start(0.0, -1.58), + Climb(10.0, -1.58), + Tunable(170.0, 1.58); + + private double elevatorHeight; + private double armHeight; + + private Position(double elevatorHeight, double armHeight) { + this.elevatorHeight = elevatorHeight; + this.armHeight = armHeight; + } + + public double elevatorHeight() { + return elevatorHeight; + } + + public double armHeight() { + return armHeight; + } + } + } + + public enum CoralAlgaeMode { + LeftCoral, + RightCoral, + Algae; + } + + private CoralAlgaeMode currentMode = CoralAlgaeMode.LeftCoral; + + public void setMode(CoralAlgaeMode mode) { + currentMode = mode; + } + + public double getAligningOffset() { + if (currentMode == CoralAlgaeMode.LeftCoral) { + return AligningConstants.leftOffset; + } else if (currentMode == CoralAlgaeMode.RightCoral) { + return AligningConstants.rightOffset; + } else { + return AligningConstants.centerOffset; + } + } + + public Command setGoal(frc.robot.subsystems.ModeManager.ModeManager.State.Position position) { + + return Commands.parallel( + Commands.runOnce(() -> elevator.setPosition(position.elevatorHeight()), elevator), + Commands.runOnce(() -> arm.setPosition(position.armHeight()), arm)); + } +} diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java deleted file mode 100644 index 59c529bf..00000000 --- a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java +++ /dev/null @@ -1,669 +0,0 @@ -package frc.robot.subsystems.ModeManager; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.FasterRepeatCommand; -import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState.Position; -import frc.robot.subsystems.arm.ArmSubsystem; -import frc.robot.subsystems.chute.ChuteSubsystem; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.util.Elastic; -import java.util.ArrayList; -import java.util.List; -import java.util.Set; -import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; -import org.littletonrobotics.junction.networktables.LoggedNetworkString; - -public class SuperstructureStateManager extends SubsystemBase { - - public final LoggedMechanismLigament2d m_elevator; - public final LoggedMechanismLigament2d m_wrist; - - private static final double kElevatorMinimumLength = 0.5; - - public static class SuperstructureState { - private static LoggedNetworkNumber elevatorHeightLogged = - new LoggedNetworkNumber("Elevator Height", 170); - private static LoggedNetworkNumber armHeightLogged = - new LoggedNetworkNumber("Arm Height", 0); - private static LoggedNetworkNumber wristRotationLogged = - new LoggedNetworkNumber("Wrist Rotation", 1.58); - private static LoggedNetworkString parentLogged = - new LoggedNetworkString("Superstructure Parent", "CenterZoneNull"); - private static LoggedNetworkString buttonTargetLogged = - new LoggedNetworkString("Superstructure Button Target", "Tunable"); - - @FunctionalInterface - public interface StateChecker { - boolean checksOut( - Position position, SuperstructureStateManager stateManager, boolean logExtra); - - default boolean checksOut(Position position, SuperstructureStateManager stateManager) { - return checksOut(position, stateManager, false); - } - } - - public static final StateChecker FALSE = (p, s, e) -> false; - public static final StateChecker TRUE = (p, s, e) -> true; - public static final StateChecker DEFAULT = - (p, s, e) -> { - // return (s.internalPosition == p); - double armPosition = s.armSubsystem.getPosition(); - double wristPosition = s.wristSubsystem.getFlippedPosition(); - double elevatorPosition = s.elevatorSubsystem.getPosition(); - - boolean armAtPosition = Math.abs(armPosition - p.armHeight()) < 0.1; - boolean wristAtPosition = Math.abs(wristPosition - p.wristRotation()) < 0.1; - boolean elevatorAtPosition = - Math.abs(elevatorPosition - p.elevatorHeight()) < 1.5; - if (e) { - Logger.recordOutput("/Superstructure/Arm/ArmAtPosition", armAtPosition); - Logger.recordOutput("/Superstructure/Arm/Setpoint", p.armHeight()); - Logger.recordOutput("/Superstructure/Arm/Position", armPosition); - Logger.recordOutput( - "/Superstructure/Arm/Error", armPosition - p.armHeight()); - - Logger.recordOutput( - "/Superstructure/Wrist/WristAtPosition", wristAtPosition); - Logger.recordOutput("/Superstructure/Wrist/Setpoint", p.wristRotation()); - Logger.recordOutput("/Superstructure/Wrist/Position", wristPosition); - Logger.recordOutput( - "/Superstructure/Wrist/Error", wristPosition - p.wristRotation()); - - Logger.recordOutput( - "/Superstructure/Elevator/ElevatorAtPosition", elevatorAtPosition); - Logger.recordOutput( - "/Superstructure/Elevator/Setpoint", p.elevatorHeight()); - Logger.recordOutput("/Superstructure/Elevator/Position", elevatorPosition); - Logger.recordOutput( - "/Superstructure/Elevator/Error", - elevatorPosition - p.elevatorHeight()); - - Logger.recordOutput("/Superstructure/Checker", "DEFAULT"); - - Logger.recordOutput("Superstructure/Heartbeat", Timer.getTimestamp()); - } - if (armAtPosition && wristAtPosition && elevatorAtPosition) { - return true; - - } else return false; - }; - public static final StateChecker AUTO = DEFAULT; - - // Any positions going lower than elevator 160 need to have Chute Up as a parent eventually. - // (The handoff is the exception). - // No arm positions should be negative unless they are for the handoff. - // Wrist positions should be 1.58 unless the arm angle is positive. Then they can be - // whatever. - public static enum Position { - Sussy(1, 1, 1, null), - CenterZoneNull(1, 1, 1, null, TRUE, FALSE), - Home(170, 0, 1.58, CenterZoneNull), - ChuteDownPre( - 170, - 0, - 1.58, - CenterZoneNull, - (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean() || DEFAULT.checksOut(a, s, e), - (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), - ChuteDown( - 170, - 0, - 1.58, - ChuteDownPre, - (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean(), - (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), - ChuteDownNull(0, 0, 0, ChuteDown, TRUE, FALSE), - HandoffPrep(165, -0.45, 1.58, ChuteDownNull), - Handoff(130, -0.45, 1.58, HandoffPrep), - PointUpPrep( - 170, - 0.5, - 1.58, - Home, - (a, s, e) -> { - // boolean armChecksOut = s.armSubsystem.getPosition() > 0.5; - boolean wristAtPosition = - Math.abs(s.wristSubsystem.getFlippedPosition() - a.wristRotation()) - < 0.1; - boolean elevatorAtPosition = s.elevatorSubsystem.getPosition() > 165; - return elevatorAtPosition && wristAtPosition; - // return armChecksOut && wristAtPosition && elevatorAtPosition; - }, - TRUE), - PointUp( - 170, - 2.15, - 1.58, - PointUpPrep, - (a, s, e) -> { - boolean armChecksOut = s.armSubsystem.getPosition() > 0.5; - // boolean wristAtPosition = - // Math.abs(s.wristSubsystem.getFlippedPosition() - - // a.wristRotation()) < 0.1; - // boolean elevatorAtPosition = - // Math.abs(s.elevatorSubsystem.getPosition() - - // a.elevatorHeight()) < 1.5; - return armChecksOut; - // return armChecksOut; && wristAtPosition && elevatorAtPosition; - }, - TRUE), - UpZoneNull(0, 0, 0, PointUp, TRUE, FALSE), - ChuteUpPre( - 170, - 2.15, - -1.58, - UpZoneNull, - (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean() || DEFAULT.checksOut(a, s, e), - (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), - ChuteUp( - 170, - 2.15, - -1.58, - ChuteUpPre, - (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean(), - (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), - ChuteUpNull(0, 0, 0, ChuteUp, TRUE, FALSE), - // L1Prep(297, 2.15, -1.58, ChuteUpNull), - L1(130, 1.1, 0, UpZoneNull), - L2Prep(120, 2.2, -1.58, UpZoneNull), - L2(90, 2.2, -1.58, L2Prep), - L3Prep(190, 2.2, -1.58, UpZoneNull), - L3(160, 2.2, -1.58, L3Prep), - L4Prep(297, 2.2, -1.58, UpZoneNull), - L4(297, 1.7, -1.58, L4Prep), - Quick34(250, 0.7, 0, UpZoneNull), - Quick23(150, 0.7, 0, UpZoneNull), - // L2AlgaePrep(297, 2.15, -1.58, UpZoneNull), - L2Algae(170, 1.2, -1.58, UpZoneNull), - // L3AlgaePrep(297, 2.15, -1.58, UpZoneNull), - L3Algae(250, 1.2, -1.58, UpZoneNull), - // NetAlgaePrep(297, 2.15, -1.58, UpZoneNull), - // NetAlgae(297, 2.15, -1.58, NetAlgaePrep), - Source(130, 2.15, 0, PointUp), - AlgaeHome(100, 2.15, -1.58, ChuteUpNull), - AlgaeHomeNull(0.0, 0.0, 0.0, AlgaeHome, TRUE, FALSE), - // Climb(170, 0, 1.58, ChuteUpNull), - Processor(70, 1.58, -1.58, AlgaeHomeNull), - - // IcecreamCoral(170, 0, 1.58, AlgaeHome), - // IcecreamAlgae(170, 0, 1.58, AlgaeHome), - GroundAlgaePrep(65, 1.58, 1.58, AlgaeHomeNull), - GroundAlgae(20, 1.58, 1.58, GroundAlgaePrep), - StartPrepPrepPrep(170, 2.15, 1.58, ChuteUpNull), - StartPrepPrep(140, 0, 1.58, StartPrepPrepPrep), - StartPrep(140, 0, -1.58, StartPrepPrep), - Start(0, 0, -1.58, StartPrep), - Climb(10, 0, -1.58, StartPrep), - Tunable(170, 0, 1.58, CenterZoneNull, FALSE); - - private double elevatorHeight; - private double armHeight; - private double wristRotation; - public Position position; - private Position parent; - public StateChecker isAtTarget; - public StateChecker realPosition; - - private Position( - double elevatorHeight, - double armHeight, - double wristRotation, - Position parent, - StateChecker isAtTarget, - StateChecker realPosition) { - this.armHeight = armHeight; - this.elevatorHeight = elevatorHeight; - this.position = this; - this.parent = parent; - this.wristRotation = wristRotation; - this.isAtTarget = isAtTarget; - this.realPosition = realPosition; - } - - private Position( - double elevatorHeight, - double armHeight, - double wristRotation, - Position parent, - StateChecker isAtTarget) { - this(elevatorHeight, armHeight, wristRotation, parent, DEFAULT, TRUE); - } - - private Position( - double elevatorHeight, - double armHeight, - double wristRotation, - Position parent) { - this(elevatorHeight, armHeight, wristRotation, parent, DEFAULT); - } - - public boolean isAtTarget(SuperstructureStateManager stateManager) { - return isAtTarget.checksOut(this, stateManager); - } - - public boolean isRealPosition(SuperstructureStateManager stateManager) { - return realPosition.checksOut(this, stateManager); - } - - // #region Pointer Methods - public double elevatorHeight() { - if (this == Position.Tunable) { - return SuperstructureState.elevatorHeightLogged - .get(); // ref: tunable variable armHeight - } - return elevatorHeight; - } - - public double armHeight() { - if (this == Position.Tunable) { - return SuperstructureState.armHeightLogged - .get(); // ref: tunable variable armHeight - } - return armHeight; - } - - public double wristRotation() { - if (this == Position.Tunable) { - return SuperstructureState.wristRotationLogged.get(); - // ref: tunable variable wristRotation - } - return wristRotation; - } - - public Position parent() { - if (this == Position.Tunable) { - return Position.valueOf(parentLogged.get()); // ref: tunable variable parent - } - return parent; - } - // #endregion - - } - } - - @AutoLogOutput - public boolean isAtTargetAllegedly() { - return SuperstructureState.DEFAULT.checksOut(targetPostition, this, true); - } - - private SuperstructureState.Position targetPostition = SuperstructureState.Position.Start; - private SuperstructureState.Position lastPosition = SuperstructureState.Position.Start; - - private SuperstructureState.Position trueFinalTarget = SuperstructureState.Position.Start; - - private List outList = new ArrayList<>(); - - private enum CoralAlgaeMode { - LeftCoral, - RightCoral, - Algae, - ArmWrist; - } - - private CoralAlgaeMode coralAlgaeMode = CoralAlgaeMode.LeftCoral; - - private SuperstructureState.Position lastRealPosition = Position.Home; - - private Pose2d lastScoringPose = Pose2d.kZero; - private double lastScoringOffset = 0; - - @AutoLogOutput private boolean chuteCanMove = false; - - public void setLastScoringPose(Pose2d pose) { - lastScoringPose = pose; - } - - public Pose2d getLastScoringPose() { - return lastScoringPose; - } - - public void setLastScoringOffset(double offset) { - lastScoringOffset = offset; - } - - public double getLastScoringOffset() { - return lastScoringOffset; - } - - public final Trigger LEFT_CORAL = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.LeftCoral); - public final Trigger RIGHT_CORAL = - new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.RightCoral); - public final Trigger ALGAE = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.Algae); - public final Trigger ARMWRIST = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.ArmWrist); - - public final Trigger PROCESSOR = new Trigger(() -> trueFinalTarget == Position.Processor); - - public final Trigger STATION_HANDOFF = - new Trigger( - () -> - trueFinalTarget == Position.Handoff - || trueFinalTarget == Position.HandoffPrep); - - public final Trigger STATION_GRIPPER = new Trigger(() -> trueFinalTarget == Position.Source); - - public Command setLeftCoralMode() { - return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.LeftCoral); - } - - public Command setRightCoralMode() { - return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.RightCoral); - } - - public Command setAlgaeMode() { - return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.Algae); - } - - public Command setArmWristMode() { - return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.ArmWrist); - } - - // public List inList = new ArrayList<>(); - private ElevatorSubsystem elevatorSubsystem; - private ArmSubsystem armSubsystem; - private WristSubsystem wristSubsystem; - private ChuteSubsystem chuteSubsystem; - - public void periodic() { - Logger.recordOutput("Superstructure/Target", targetPostition); - Logger.recordOutput("Superstructure/Last", lastPosition); - Logger.recordOutput("Superstructure/Internal", internalPosition); - Logger.recordOutput( - "Superstructure/InternalPersistant", - internalPosition == Position.Sussy ? null : internalPosition); - Logger.recordOutput( - "Superstructure/OutList", outList.toArray(new SuperstructureState.Position[0])); - - Logger.recordOutput("Superstructure/Algae", ALGAE.getAsBoolean()); - Logger.recordOutput("Superstructure/LeftCoral", LEFT_CORAL.getAsBoolean()); - Logger.recordOutput("Superstructure/RightCoral", RIGHT_CORAL.getAsBoolean()); - Logger.recordOutput( - "Superstructure/AnyCoral", LEFT_CORAL.getAsBoolean() || RIGHT_CORAL.getAsBoolean()); - - m_elevator.setLength(elevatorSubsystem.getPosition()); - m_wrist.setAngle(Math.toDegrees(armSubsystem.getPosition()) + 180); - } - - private void setFinalTarget(SuperstructureState.Position myPosition) { - trueFinalTarget = myPosition; - - outList.clear(); - // Set the [ (A') => (C') ] target of the system (initialize pathing command) - boolean found = false; - - // outList.add(0,myPosition); - - for (int i = 0; i < 20; i++) { - // SuperstructureState head = SuperstructureState.posDictionary.get(myPosition); - outList.add(0, myPosition); - if (myPosition == SuperstructureState.Position.CenterZoneNull) { - found = true; - break; - } - myPosition = myPosition.parent(); - } - - if (!found || myPosition == null) { - throw new RuntimeException("AY CARAMBA. (Your state machine didnt resolve.)"); - } - } - - private void updateTarget(SuperstructureState.Position myPosition) { - if (myPosition == lastPosition) { - lastPosition = targetPostition; - } - targetPostition = myPosition; - } - - // private void setCurrentTarget(SuperstructureState.Position myPosition) { - - // // Set the [ (A) => (A') ] target of the system (initialize movement command) - // } - - // public Command inList(){ - // for() - - // } - - /** Nodes must either be identical or neighbors. */ - private SuperstructureState.Position getChilderNodeInBranch( - SuperstructureState.Position nodeA, SuperstructureState.Position nodeB) { - var higherPose = nodeA; - if (nodeB.parent() == nodeA) { - higherPose = nodeB; - } - return higherPose; - } - - private SuperstructureState.Position getParenterNodeInBranch( - SuperstructureState.Position nodeA, SuperstructureState.Position nodeB) { - var lowerPose = nodeA; - if (nodeA.parent() == nodeB) { - lowerPose = nodeB; - } - return lowerPose; - } - - private SuperstructureState.Position internalPosition = Position.Sussy; - - /* - * The `internalGoToPosition` command needs to actually command the elevator and - * the wrist and the arm to go to a position - */ - private Command internalGoToPosition(SuperstructureState.Position myPosition) { - if (myPosition == Position.ChuteDownNull) { - chuteCanMove = false; - } - if (myPosition == Position.ChuteUpNull) { - chuteCanMove = false; - } - if ((lastPosition.parent() == Position.CenterZoneNull && lastPosition.isRealPosition(this)) - || (lastPosition.parent() == Position.UpZoneNull - && lastPosition.isRealPosition(this))) { - chuteCanMove = true; - } - - if (myPosition.isRealPosition(this)) { - lastRealPosition = myPosition; - } - - return elevatorSubsystem - .setPosition(lastRealPosition.elevatorHeight()) - .alongWith(armSubsystem.setPosition(lastRealPosition.armHeight())) - .alongWith(wristSubsystem.setPosition(lastRealPosition.wristRotation())); - } - - private static Command unlessUntil(Command command, BooleanSupplier condition) { - return command.until(condition).unless(condition); - } - - public Command moveToPosition(SuperstructureState.Position myPosition) { - Command setFinalTarget = Commands.runOnce(() -> setFinalTarget(myPosition)); - Command followInPath = - unlessUntil( - followInPath( - () -> { - return outList.contains(lastPosition) - && outList.contains(targetPostition); - }), - () -> { - return outList.contains(lastPosition) - && outList.contains(targetPostition); - }); - - Command clearOutPath = - Commands.runOnce( - () -> { - var childerPose = getChilderNodeInBranch(lastPosition, targetPostition); - - for (int i = 0; i < 20; i++) { - if (outList.contains(childerPose) - && outList.get(0) != childerPose) { - outList.remove(0); - } - } - }); - - Command clearOutPath2 = - Commands.runOnce( - () -> { - var childerPose = getChilderNodeInBranch(lastPosition, targetPostition); - - for (int i = 0; i < 20; i++) { - if (outList.contains(childerPose) - && outList.get(0) != childerPose) { - outList.remove(0); - } - } - }); - - Command followOutPath = followOutPath(() -> false); - - Command followOutPath2 = followOutPath(() -> false); - - Command chuteUp = - unlessUntil(Commands.idle(), () -> chuteCanMove) - .andThen(chuteSubsystem.moveChuteUp()::schedule); - Command chuteDown = - unlessUntil(Commands.idle(), () -> chuteCanMove) - .andThen(chuteSubsystem.moveChuteDown()::schedule); - - Command chuteMover = - chuteUp.onlyIf(() -> outList.contains(Position.ChuteUp)) - .andThen(chuteDown.onlyIf(() -> outList.contains(Position.ChuteDown))); - - Command outputCommand = - setFinalTarget.alongWith( - Commands.either( - clearOutPath.alongWith(followOutPath), - followInPath.andThen(clearOutPath2.alongWith(followOutPath2)), - () -> { - return outList.contains(lastPosition) - && outList.contains(targetPostition); - }), - chuteMover); - - outputCommand.addRequirements(this); - return outputCommand; - } - - public Command moveToTunablePosition() { - return Commands.defer( - () -> { - try { - var nextPos = - Position.valueOf(SuperstructureState.buttonTargetLogged.get()); - nextPos.parent(); - return moveToPosition(nextPos).asProxy(); - } catch (IllegalArgumentException e) { - return Commands.runOnce( - () -> { - Elastic.sendNotification( - new Elastic.Notification( - Elastic.Notification.NotificationLevel.ERROR, - "Tunable Does Not Exist", - "The tunable in the superstructure button does not exist.")); - }); - } - }, - Set.of()); - } - - private Command followOutPath(BooleanSupplier finisher) { - return new FasterRepeatCommand( - Commands.defer( - () -> { - SuperstructureState.Position nextPose = outList.remove(0); - if (outList.size() == 0) { - return Commands.runOnce(() -> updateTarget(nextPose)) - .alongWith(internalGoToPosition(nextPose)); - } - return runOnce(() -> updateTarget(nextPose)) - .alongWith( - unlessUntil( - internalGoToPosition(nextPose), - () -> { - boolean atPose = nextPose.isAtTarget(this); - if (atPose) { - lastPosition = nextPose; - } - return atPose; - })); - }, - Set.of(elevatorSubsystem, armSubsystem, wristSubsystem)), - finisher); - } - - private Command followInPath(BooleanSupplier finisher) { - return new FasterRepeatCommand( - Commands.defer( - () -> { - SuperstructureState.Position nextPose = - getChilderNodeInBranch(lastPosition, targetPostition).parent(); - if (nextPose == null) { - return internalGoToPosition(Position.CenterZoneNull); - } - return runOnce(() -> updateTarget(nextPose)) - .alongWith( - unlessUntil( - internalGoToPosition(nextPose), - () -> { - boolean atPose = nextPose.isAtTarget(this); - if (atPose) { - lastPosition = nextPose; - } - return atPose; - })); - }, - Set.of(elevatorSubsystem, armSubsystem, wristSubsystem)), - finisher); - } - - public SuperstructureStateManager( - ElevatorSubsystem elevatorSubsystem, - ArmSubsystem armSubsystem, - WristSubsystem wristSubsystem, - ChuteSubsystem chuteSubsystem) { - this.elevatorSubsystem = elevatorSubsystem; - this.armSubsystem = armSubsystem; - this.wristSubsystem = wristSubsystem; - this.chuteSubsystem = chuteSubsystem; - - LoggedMechanism2d mech = new LoggedMechanism2d(3, 6); - - LoggedMechanismRoot2d root = mech.getRoot("Elevator", 2, 0); - - m_elevator = new LoggedMechanismLigament2d("elevator", kElevatorMinimumLength, 90); - - m_wrist = - new LoggedMechanismLigament2d("wrist", 0.5, 90, 6, new Color8Bit(Color.kAliceBlue)); - - m_elevator.append(m_wrist); - root.append(m_elevator); - - SmartDashboard.putData("Mech2d", mech); - - Command defaultcom = - Commands.either( - Commands.idle(), - moveToPosition(Position.Home).asProxy(), - () -> !RobotState.isAutonomous()); - defaultcom.addRequirements(this); - setDefaultCommand(defaultcom); - } -} diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteIO.java b/src/main/java/frc/robot/subsystems/chute/ChuteIO.java deleted file mode 100644 index b67535d4..00000000 --- a/src/main/java/frc/robot/subsystems/chute/ChuteIO.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.subsystems.chute; - -import org.littletonrobotics.junction.AutoLog; - -public interface ChuteIO { - public void updateInputs(ChuteIOInputs inputs); - - @AutoLog - public class ChuteIOInputs { - public double position = 0; - public double temperature = 0; - public double current = 0; - public double voltage = 0; - public boolean shutdown = false; - // public double throughboreEncoderPosition = 0; - // public boolean throughboreConnected = false; - } - - public void setVoltage(double voltage); -} diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteIONeo550.java b/src/main/java/frc/robot/subsystems/chute/ChuteIONeo550.java deleted file mode 100644 index 22081ea1..00000000 --- a/src/main/java/frc/robot/subsystems/chute/ChuteIONeo550.java +++ /dev/null @@ -1,56 +0,0 @@ -package frc.robot.subsystems.chute; - -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import frc.robot.constants.ChuteConstants; - -public class ChuteIONeo550 implements ChuteIO { - private SparkMax chuteMotor = - new SparkMax(ChuteConstants.CHUTE_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); - - public ChuteIONeo550() { - chuteMotor.getEncoder().setPosition(0); - - SparkBaseConfig config = - new SparkMaxConfig() - .smartCurrentLimit((int) ChuteConstants.ChuteCurrent) - .secondaryCurrentLimit(ChuteConstants.ChuteCurrent) - .idleMode(IdleMode.kBrake); - chuteMotor.configure( - config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - } - - private boolean shutdown = false; - - private double lastVoltage = 0; - - public void updateInputs(ChuteIOInputs inputs) { - inputs.position = chuteMotor.getEncoder().getPosition(); - inputs.voltage = chuteMotor.getBusVoltage() * chuteMotor.getAppliedOutput(); - inputs.current = chuteMotor.getOutputCurrent(); - inputs.temperature = chuteMotor.getMotorTemperature(); - - if (inputs.temperature > 60) { - shutdown = true; - } else if (inputs.temperature < 58) { - shutdown = false; - } - - inputs.shutdown = shutdown; - - if (shutdown) { - lastVoltage = 0; - } - - chuteMotor.setVoltage(lastVoltage); - } - - public void setVoltage(double voltage) { - lastVoltage = voltage; - } -} diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java b/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java deleted file mode 100644 index 3096a951..00000000 --- a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.subsystems.chute; - -import frc.robot.constants.ChuteConstants; - -public class ChuteIOSim implements ChuteIO { - private double position = 0; - private double voltage = 0; - - public void updateInputs(ChuteIOInputs inputs) { - position += 0.02 * (voltage + 0.5) * 20; - - inputs.voltage = voltage; - - if (position > ChuteConstants.upperLimit && voltage > 0) { - position = ChuteConstants.upperLimit; - inputs.current = 30; - } else if (position < ChuteConstants.lowerLimit && voltage < 0) { - position = ChuteConstants.lowerLimit; - inputs.current = 30; - } else { - inputs.current = 0; - } - inputs.position = position; - // inputs.throughboreEncoderPosition = position; - // inputs.throughboreConnected = true; - } - - public void setVoltage(double voltage) { - this.voltage = voltage; - } - - public void setPosition(double position) {} -} diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java b/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java deleted file mode 100644 index 5e5145e7..00000000 --- a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java +++ /dev/null @@ -1,163 +0,0 @@ -package frc.robot.subsystems.chute; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.constants.ChuteConstants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; - -public class ChuteSubsystem extends SubsystemBase { - private ChuteIO chuteIO; - private ChuteIOInputsAutoLogged chuteInputs = new ChuteIOInputsAutoLogged(); - private LoggedNetworkNumber chuteTuneable = new LoggedNetworkNumber("chute tuneable", 0); - - @AutoLogOutput private boolean isUp = false; - public final Trigger UP = new Trigger(() -> isUp); - - @AutoLogOutput private boolean isDown = false; - public final Trigger DOWN = new Trigger(() -> isDown); - - private PIDController controller = - new PIDController( - ChuteConstants.CHUTE_KP, ChuteConstants.CHUTE_KI, ChuteConstants.CHUTE_KD); - - // private double reference = 0; - - // private double upSetpoint = 10; - // private double downSetpoint = -10; - - public ChuteSubsystem(ChuteIO chuteIO) { - controller.setTolerance(ChuteConstants.CHUTE_TOLERANCE); - this.chuteIO = chuteIO; - setDefaultCommand(Commands.either(moveChuteDown(), moveChuteUp(), () -> lastSetDown)); - } - - public void periodic() { - chuteIO.updateInputs(chuteInputs); - Logger.processInputs("RealOutputs/Chute", chuteInputs); - } - - public Command tuneableVoltage() { - return run(() -> setVoltage(chuteTuneable.get())); - } - - // public Command tunablePose() { - // return runOnce(() -> reference = chuteTuneable.get()); - // } - - public Command setVoltage(double voltage) { - return run(() -> chuteIO.setVoltage(voltage)); - } - - // public Command movetoPosition(double position) { - // if (position > ChuteConstants.upperLimit) { - // position = ChuteConstants.upperLimit; - // } - // if (position < ChuteConstants.lowerLimit) { - // position = ChuteConstants.lowerLimit; - // } - // double nextPosition = position; - // return runOnce( - // () -> { - // reference = nextPosition; - // }); - // } - - public final Trigger STALLING = - new Trigger(() -> chuteInputs.current >= ChuteConstants.ChuteCurrentTrigger); - - private boolean lastSetDown = false; - - public Command moveChuteUp() { - return setVoltage(-4) - .withTimeout(0.05) - .andThen(setVoltage(-4).until(STALLING)) - .andThen(setUp()) - .andThen(setVoltage(-1)) - .beforeStarting( - () -> { - isDown = false; - lastSetDown = false; - }); - } - - public Command moveChuteDown() { - return (setVoltage(1.5) - .withTimeout(0.05) - .andThen( - setVoltage(1.5) - .until(STALLING) - .andThen(setDown()) - .andThen(setVoltage(0.0)))) - .beforeStarting( - () -> { - isUp = false; - lastSetDown = true; - }); - } - - // public void setUp() { - // isUp = true; - // isDown = false; - // } - - // public void setDown() { - // isUp = false; - // isDown = true; - // } - - public Command setUp() { - return runOnce( - () -> { - isUp = true; - isDown = false; - }); - } - - public Command setDown() { - return runOnce( - () -> { - isUp = false; - isDown = true; - }); - } - - // private Command followReferenceThrubore() { - // return run( - // () -> { - // double voltage = - // controller.calculate( - // wristInputs.throughboreEncoderPosition, - // isWristFlipped ? -reference : reference); - // if (controller.atSetpoint()) { - // voltage = 0; - // } else { - // voltage = Math.min(12.0, Math.max(-12.0, voltage)); // Clamp voltage - // } - // wristIO.setVoltage(voltage); - // }); - // } - - // public double getPosition() { - // return wristInputs.throughboreEncoderPosition; - // } - - // public double getFlippedPosition() { - // return isWristFlipped - // ? -wristInputs.throughboreEncoderPosition - // : wristInputs.throughboreEncoderPosition; - // } - - // public double getInternalEncoderPosition() { - // return wristInputs.position; - // } - - // public boolean isEncoderConnected() { - // return wristInputs.throughboreConnected; - // } - -} diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperIO.java b/src/main/java/frc/robot/subsystems/gripper/GripperIO.java index 71edbada..d4041ef5 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperIO.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperIO.java @@ -7,12 +7,22 @@ public interface GripperIO { @AutoLog public class GripperIOInputs { - public double speed = 0; - public double voltage = 0; - public double temperature = 0; - public double current = 0; - public boolean sensor = false; + public double speedLeft = 0; + public double voltageLeft = 0; + public double temperatureLeft = 0; + public double currentLeft = 0; + + public double speedRight = 0; + public double voltageRight = 0; + public double temperatureRight = 0; + public double currentRight = 0; + + public boolean hasPiece = false; } public void setVoltage(double voltage); + + public void setVoltageLeft(double voltage); + + public void setVoltageRight(double voltage); } diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java b/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java deleted file mode 100644 index 5e2f3c1d..00000000 --- a/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java +++ /dev/null @@ -1,41 +0,0 @@ -package frc.robot.subsystems.gripper; - -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.AnalogInput; -import frc.robot.constants.GripperConstants; - -public class GripperIOFalcon implements GripperIO { - private final TalonFX armRoller = new TalonFX(GripperConstants.id, GripperConstants.canbus); - - private AnalogInput gripperGPSensor = new AnalogInput(GripperConstants.gripperSensorChannel); - - public GripperIOFalcon() { - armRoller.setPosition(0); - - armRoller - .getConfigurator() - .apply( - new TalonFXConfiguration() - .withCurrentLimits(GripperConstants.currentLimit) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive))); - armRoller.setNeutralMode(NeutralModeValue.Brake); - } - - public void updateInputs(GripperIOInputs inputs) { - inputs.speed = armRoller.getVelocity().getValueAsDouble(); - inputs.voltage = armRoller.getMotorVoltage().getValueAsDouble(); - inputs.current = armRoller.getStatorCurrent().getValueAsDouble(); - inputs.temperature = armRoller.getDeviceTemp().getValueAsDouble(); - inputs.sensor = gripperGPSensor.getValue() < 50; - } - - public void setVoltage(double voltage) { - armRoller.setVoltage(voltage); - } -} diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperIONeo550.java b/src/main/java/frc/robot/subsystems/gripper/GripperIONeo550.java new file mode 100644 index 00000000..82fbad18 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/gripper/GripperIONeo550.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.gripper; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.constants.GripperConstants; +import frc.robot.constants.WristConstants; + +public class GripperIONeo550 implements GripperIO { + private SparkMax leftSparkMax = + new SparkMax(GripperConstants.leftMotorId, SparkLowLevel.MotorType.kBrushless); + private SparkMax rightSparkMax = + new SparkMax(GripperConstants.rightMotorId, SparkLowLevel.MotorType.kBrushless); + + private double lastVoltageLeft = 0; + + private double lastVoltageRight = 0; + + private DigitalInput sensor = new DigitalInput(GripperConstants.pieceSensorChannel); + + public GripperIONeo550() { + SparkBaseConfig config = + new SparkMaxConfig() + .smartCurrentLimit((int) WristConstants.WristCurrent) + .secondaryCurrentLimit(WristConstants.WristCurrent) + .idleMode(IdleMode.kBrake); + + leftSparkMax.configure(config, null, PersistMode.kPersistParameters); + rightSparkMax.configure(config, null, PersistMode.kPersistParameters); + } + + public void updateInputs(GripperIOInputs inputs) { + inputs.voltageLeft = leftSparkMax.getBusVoltage() * leftSparkMax.getAppliedOutput(); + inputs.currentLeft = leftSparkMax.getOutputCurrent(); + inputs.temperatureLeft = leftSparkMax.getMotorTemperature(); + + leftSparkMax.setVoltage(lastVoltageLeft); + + inputs.voltageRight = rightSparkMax.getBusVoltage() * rightSparkMax.getAppliedOutput(); + inputs.currentRight = rightSparkMax.getOutputCurrent(); + inputs.temperatureRight = rightSparkMax.getMotorTemperature(); + + rightSparkMax.setVoltage(lastVoltageRight); + + inputs.hasPiece = sensor.get(); + } + + public void setVoltage(double voltage) { + lastVoltageRight = voltage; + lastVoltageLeft = voltage; + } + + public void setVoltageLeft(double voltage) { + lastVoltageLeft = voltage; + } + + public void setVoltageRight(double voltage) { + lastVoltageRight = voltage; + } +} diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperIOSim.java b/src/main/java/frc/robot/subsystems/gripper/GripperIOSim.java index 4675ea13..7f5cb7a5 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperIOSim.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperIOSim.java @@ -3,18 +3,29 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class GripperIOSim implements GripperIO { - public double voltage; - public double speed; - public LoggedNetworkBoolean gripperGPSensor = + private double leftVoltage; + private double rightVoltage; + private double speed; + private LoggedNetworkBoolean gripperGPSensor = new LoggedNetworkBoolean("Gripper Sensor Sim", true); public void updateInputs(GripperIOInputs inputs) { - inputs.voltage = voltage; - inputs.speed = speed; - inputs.sensor = gripperGPSensor.get(); + inputs.voltageLeft = leftVoltage; + inputs.voltageRight = rightVoltage; + inputs.speedLeft = speed; + inputs.hasPiece = gripperGPSensor.get(); } public void setVoltage(double voltage) { - this.voltage = voltage; + this.leftVoltage = voltage; + this.rightVoltage = voltage; + } + + public void setVoltageLeft(double voltage) { + this.leftVoltage = voltage; + } + + public void setVoltageRight(double voltage) { + this.rightVoltage = voltage; } } diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java index ecfab383..3cf9d4b7 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java @@ -9,74 +9,60 @@ public class GripperSubsystem extends SubsystemBase { - private GripperIO piviotIO; + private GripperIO gripperIO; - private GripperIOInputsAutoLogged armrollerInputs = new GripperIOInputsAutoLogged(); + private GripperIOInputsAutoLogged gripperInputs = new GripperIOInputsAutoLogged(); public final Trigger HAS_PIECE = new Trigger(this::hasPiece); - // NetworkTableInstance nInstance = NetworkTableInstance.getDefault(); - // NetworkTable table = nInstance.getTable("SmartDashboard"); - // NetworkTableValue grippervoltage = table.getValue("grippervoltage"); - - LoggedNetworkNumber grippervoltage = new LoggedNetworkNumber("Gripper Voltage", 0); + LoggedNetworkNumber leftGripperVoltage = + new LoggedNetworkNumber("Left Gripper Motor Voltage", 0); + LoggedNetworkNumber rightGripperVoltage = + new LoggedNetworkNumber("Right Gripper Motor Voltage ", 0); // TODO: NAME? public GripperSubsystem(GripperIO armrollerIO) { - this.piviotIO = armrollerIO; - setDefaultCommand(Commands.either(holdCoral(), setVoltage(0), HAS_PIECE)); + this.gripperIO = armrollerIO; + setDefaultCommand(setVoltage(0)); } public void periodic() { - piviotIO.updateInputs(armrollerInputs); - Logger.processInputs("RealOutputs/Gripper", armrollerInputs); + gripperIO.updateInputs(gripperInputs); + Logger.processInputs("RealOutputs/Gripper", gripperInputs); } - public Command gripperTuneable() { + public Command gripperLeftTuneable() { return run( () -> { - double voltage = grippervoltage.get(); - piviotIO.setVoltage(voltage); + double voltage = leftGripperVoltage.get(); + gripperIO.setVoltage(voltage); }); } - public Command intakeSpinCoral() { - return setVoltage(12); - } - - public Command holdCoral() { - return setVoltage(0.25); - } - - public Command ejectSpinCoral() { - return setVoltage(-1); - } - - public Command intakeSpinAlgae() { - return setVoltage(12); + public Command gripperRightTuneable() { + return run( + () -> { + double voltage = rightGripperVoltage.get(); + gripperIO.setVoltage(voltage); + }); } - public Command ejectSpinAlgae() { - return setVoltage(-12); + public Command intake(double voltage) { + return setVoltage(voltage); } - public Command slowEjectSpinAlgae() { - return setVoltage(-3); + public Command ejectReverse(double voltage) { + return setVoltage(-voltage); } public Command setVoltage(double voltage) { - return run( + return Commands.runOnce( () -> { - piviotIO.setVoltage(voltage); - }); + gripperIO.setVoltage(voltage); + }, + this); } public boolean hasPiece() { - return armrollerInputs.sensor; - } - - private boolean hasAlgae = false; - - public void setHasAlgae(boolean hasAlgae) { - hasAlgae = true; + return false; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java deleted file mode 100644 index 0e0507cd..00000000 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.wrist; - -import org.littletonrobotics.junction.AutoLog; - -public interface WristIO { - public void updateInputs(WristIOInputs inputs); - - @AutoLog - public class WristIOInputs { - public double position = 0; - public double temperature = 0; - public double current = 0; - public double voltage = 0; - public double throughboreEncoderPosition = 0; - public boolean shutdown = false; - public boolean throughboreConnected = false; - } - - public void setVoltage(double voltage); - - public void setPositionControl(double reference); -} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java b/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java deleted file mode 100644 index 7a6b740a..00000000 --- a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java +++ /dev/null @@ -1,103 +0,0 @@ -package frc.robot.subsystems.wrist; - -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import frc.robot.constants.WristConstants; - -public class WristIONeo550 implements WristIO { - private SparkMax wristMotor = - new SparkMax(WristConstants.WRIST_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); - - private final DutyCycleEncoder throughboreEncoder = - new DutyCycleEncoder(WristConstants.WRIST_THROUGHBORE_ENCODER_ID, 2 * Math.PI, 0); - - public WristIONeo550() { - SparkBaseConfig config = - new SparkMaxConfig() - .smartCurrentLimit((int) WristConstants.WristCurrent) - .secondaryCurrentLimit(WristConstants.WristCurrent) - .idleMode(IdleMode.kBrake); - - config.encoder.positionConversionFactor(Math.PI / 76.1); - - config.closedLoop - .p(WristConstants.WRIST_KP / 12) - .i(WristConstants.WRIST_KI / 12) - .d(WristConstants.WRIST_KD / 12); - - config.softLimit - .forwardSoftLimit(WristConstants.upperLimit) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(WristConstants.lowerLimit) - .reverseSoftLimitEnabled(true); - - wristMotor.configure( - config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - - wristMotor.getEncoder().setPosition(-Math.PI / 2); - } - - private boolean shutdown = false; - - private double lastVoltage = 0; - - private boolean usingVoltage = true; - - public void updateInputs(WristIOInputs inputs) { - inputs.voltage = wristMotor.getBusVoltage() * wristMotor.getAppliedOutput(); - inputs.current = wristMotor.getOutputCurrent(); - inputs.temperature = wristMotor.getMotorTemperature(); - inputs.throughboreEncoderPosition = throughboreEncoder.get() - 4.22; - wristMotor.getEncoder().setPosition(inputs.throughboreEncoderPosition); - inputs.position = wristMotor.getEncoder().getPosition(); - inputs.throughboreConnected = throughboreEncoder.isConnected(); - - if (inputs.temperature > 60) { - shutdown = true; - } else if (inputs.temperature < 58) { - shutdown = false; - } - - inputs.shutdown = shutdown; - - if (inputs.throughboreEncoderPosition >= WristConstants.upperLimit - && (inputs.voltage > 0 || lastVoltage > 0)) { - lastVoltage = 0; - usingVoltage = true; - } - if (inputs.throughboreEncoderPosition <= WristConstants.lowerLimit - && (inputs.voltage < 0 || lastVoltage < 0)) { - lastVoltage = 0; - usingVoltage = true; - } - if (!inputs.throughboreConnected) { - lastVoltage = 0; - usingVoltage = true; - } - if (shutdown) { - lastVoltage = 0; - usingVoltage = true; - } - - if (usingVoltage) { - wristMotor.setVoltage(lastVoltage); - } - } - - public void setVoltage(double voltage) { - lastVoltage = voltage; - usingVoltage = true; - } - - public void setPositionControl(double reference) { - wristMotor.getClosedLoopController().setReference(reference, ControlType.kPosition); - usingVoltage = false; - } -} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java deleted file mode 100644 index ced810d7..00000000 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.subsystems.wrist; - -import edu.wpi.first.math.controller.PIDController; -import frc.robot.constants.WristConstants; - -public class WristIOSim implements WristIO { - private double position = 0; - private double voltage = 0; - - private double reference = 0; - private boolean usingPosition = false; - - private PIDController controller = - new PIDController( - WristConstants.WRIST_KP, WristConstants.WRIST_KI, WristConstants.WRIST_KD); - - public void updateInputs(WristIOInputs inputs) { - if (usingPosition) { - voltage = controller.calculate(position, reference); - } - - position += 0.02 * voltage; - - inputs.position = position; - inputs.throughboreEncoderPosition = position; - inputs.throughboreConnected = true; - } - - public void setVoltage(double voltage) { - this.voltage = voltage; - usingPosition = false; - } - - public void setPositionControl(double reference) { - this.reference = reference; - usingPosition = true; - } -} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java deleted file mode 100644 index b1856a66..00000000 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ /dev/null @@ -1,146 +0,0 @@ -package frc.robot.subsystems.wrist; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.WristConstants; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; - -public class WristSubsystem extends SubsystemBase { - private WristIO wristIO; - private WristIOInputsAutoLogged wristInputs = new WristIOInputsAutoLogged(); - private LoggedNetworkNumber wristTuneable = new LoggedNetworkNumber("wrist tuneable", 0); - - private PIDController controller = - new PIDController( - WristConstants.WRIST_KP, WristConstants.WRIST_KI, WristConstants.WRIST_KD); - - private double reference = 0; - - private boolean isWristFlipped = false; - - public WristSubsystem(WristIO wristIO) { - controller.setTolerance(WristConstants.WRIST_TOLERANCE); - this.wristIO = wristIO; - setDefaultCommand(setVoltage(0)); - } - - public void periodic() { - wristIO.updateInputs(wristInputs); - Logger.processInputs("RealOutputs/Wrist", wristInputs); - - if (!isSafeToMove()) { - wristIO.setVoltage(0); - } - } - - public Command turnWristRight() { - return setVoltage(1); - } - - public Command turnWristLeft() { - return setVoltage(-1); - } - - public Command tuneableVoltage() { - return run(() -> setVoltageSave(wristTuneable.get())); - } - - public Command tunablePose() { - return runOnce(() -> reference = wristTuneable.get()).andThen(followReferenceThrubore()); - } - - public Command setVoltage(double voltage) { - return run(() -> setVoltageSave(voltage)); - } - - public Command setPosition(double position) { - if (position > WristConstants.upperLimit) { - position = WristConstants.upperLimit; - } - if (position < WristConstants.lowerLimit) { - position = WristConstants.lowerLimit; - } - double nextPosition = position; - return runOnce( - () -> { - reference = nextPosition; - }) - .andThen(followReferenceThrubore()); - } - - // public Command flipWristPosition() { - // return Commands.runOnce( - // () -> { - // isWristFlipped = !isWristFlipped; - // }); - // } - - private Command followReferenceThrubore() { - return run( - () -> { - double voltage = - controller.calculate( - wristInputs.throughboreEncoderPosition, - isWristFlipped ? -reference : reference); - if (controller.atSetpoint()) { - voltage = 0; - setVoltageSave(voltage); - } else { - voltage = Math.min(12.0, Math.max(-12.0, voltage)); - setPositionSave(isWristFlipped ? -reference : reference); // Clamp voltage - } - }); - } - - public double getPosition() { - return wristInputs.throughboreEncoderPosition; - } - - public double getFlippedPosition() { - return isWristFlipped - ? -wristInputs.throughboreEncoderPosition - : wristInputs.throughboreEncoderPosition; - } - - public double getInternalEncoderPosition() { - return wristInputs.position; - } - - public boolean isEncoderConnected() { - return wristInputs.throughboreConnected; - } - - public DoubleSupplier elevatorHeight = () -> 0; - public DoubleSupplier armHeight = () -> -1; - - @AutoLogOutput - private boolean isSafeToMove() { - return elevatorHeight.getAsDouble() > 139 || armHeight.getAsDouble() > 0.5; - } - - private void setVoltageSave(double voltage) { - if (isSafeToMove()) { - wristIO.setVoltage(voltage); - } else { - wristIO.setVoltage(0); - } - // if ( - // wristIO.setVoltage(voltage); - // ) - } - - private void setPositionSave(double referenceInternal) { - if (isSafeToMove()) { - wristIO.setPositionControl(referenceInternal); - } else { - wristIO.setVoltage(0); - } - // if ( - // wristIO.setVoltage(voltage); - // ) - } -}