From 5d847d764b3f1aa73e15a90ad7ac3622d9daf507 Mon Sep 17 00:00:00 2001 From: qwertycloudhub Date: Thu, 7 Nov 2024 20:28:17 -0500 Subject: [PATCH 1/5] 11/07/24 --- src/main/java/frc/robot/Constants.java | 30 + src/main/java/frc/robot/Main.java | 16 +- src/main/java/frc/robot/Robot.java | 46 -- src/main/java/frc/robot/RobotContainer.java | 53 +- src/main/java/frc/robot/TalonFXConstants.java | 7 + .../frc/robot/commands/AutoAimCommand.java | 37 + .../commands/InboundingBoxAlignCommand.java | 63 ++ .../frc/robot/subsystems/DumperSubsystem.java | 41 +- .../frc/robot/subsystems/IntakeSubsystem.java | 25 +- .../frc/robot/subsystems/VisionSubsystem.java | 17 +- .../drivetrain/CommandSwerveDrivetrain.java | 1 - .../java/frc/robot/util/AllianceFlipUtil.java | 4 +- .../java/frc/robot/util/LimelightHelpers.java | 780 ++++++++++++++++++ .../controllerUtils/ControllerContainer.java | 15 +- 14 files changed, 1005 insertions(+), 130 deletions(-) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/TalonFXConstants.java create mode 100644 src/main/java/frc/robot/commands/AutoAimCommand.java create mode 100644 src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java create mode 100644 src/main/java/frc/robot/util/LimelightHelpers.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..4f7df8c --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,30 @@ +package frc.robot; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +import java.util.Map; + +public class Constants { + + public class FieldConstants { + public static final double fieldLength = Units.inchesToMeters(651.223); + public final class InboundingBox { + public static final Translation3d inboundingBoxPosition = new Translation3d(0, 0, 0); + } + } + + public class OIConstants{ + public static final Map CONTROLLERS = Map.of( + 0, ControllerType.CUSTOM, 1, ControllerType.XBOX + ); + + public static final int CONTROLLER_COUNT = CONTROLLERS.size(); + + + public enum ControllerType { + CUSTOM, + XBOX + } + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 37104bd..643ccb6 100755 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,25 +1,13 @@ -// Copyright (c) FIRST and other WPILib contributors. - -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot call. - */ + public final class Main { private Main() { } - /** - * Main initialization method. Do not perform any initialization here. - *

- * If you change your main Robot class (name), change the parameter type. - */ + public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f07f6da..06eb36e 100755 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,31 +1,15 @@ -// Copyright (c) FIRST and other WPILib contributors. - -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - -/** - * The VM is configured to automatically run this class, and to call the methods corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { private Command autonomousCommand; private RobotContainer robotContainer; - /** - * This method is run when the robot is first started up and should be used for any - * initialization code. - */ @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our @@ -33,14 +17,6 @@ public void robotInit() { robotContainer = new RobotContainer(); } - - /** - * This method is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic methods, but before LiveWindow and - * SmartDashboard integrated updating. - */ @Override public void robotPeriodic() { // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled @@ -51,9 +27,6 @@ public void robotPeriodic() { } - /** - * This method is called once each time the robot enters Disabled mode. - */ @Override public void disabledInit() { } @@ -64,9 +37,6 @@ public void disabledPeriodic() { } - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ @Override public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); @@ -78,9 +48,6 @@ public void autonomousInit() { } - /** - * This method is called periodically during autonomous. - */ @Override public void autonomousPeriodic() { } @@ -98,9 +65,6 @@ public void teleopInit() { } - /** - * This method is called periodically during operator control. - */ @Override public void teleopPeriodic() { } @@ -112,26 +76,16 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); } - - /** - * This method is called periodically during test mode. - */ @Override public void testPeriodic() { } - /** - * This method is called once when the robot is first started up. - */ @Override public void simulationInit() { } - /** - * This method is called periodically whilst in simulation. - */ @Override public void simulationPeriodic() { } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a65ad18..ff47566 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,50 +5,51 @@ package frc.robot; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.constants.Constants; +import frc.robot.commands.InboundingBoxAlignCommand; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.subsystems.drivetrain.generated.TunerConstants; +import frc.robot.util.controllerUtils.ButtonHelper; +import frc.robot.util.controllerUtils.ControllerContainer; - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ public class RobotContainer { - - XboxController xboxController; + public ControllerContainer controllerContainer = new ControllerContainer(); + public final CommandXboxController joystick = new CommandXboxController(1); + ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers()); + final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(CommandSwerveDrivetrain.MAX_VELOCITY_METERS_PER_SECOND * 0.1) + .withRotationalDeadband(CommandSwerveDrivetrain.MaFxAngularRate * 0.1) + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); configureBindings(); } - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { + private void configureBindings() { + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> + drive + .withVelocityX(-joystick.getLeftY() * TunerConstants.kSpeedAt12VoltsMps) + .withVelocityY(-joystick.getLeftX() * TunerConstants.kSpeedAt12VoltsMps) + // +rotational rate = counterclockwise (left), -X in joystick = left + .withRotationalRate(-joystick.getRightX() * CommandSwerveDrivetrain.MaFxAngularRate) + )); + + joystick.leftTrigger().onTrue(new InboundingBoxAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ public Command getAutonomousCommand() { return null; } diff --git a/src/main/java/frc/robot/TalonFXConstants.java b/src/main/java/frc/robot/TalonFXConstants.java new file mode 100644 index 0000000..312203f --- /dev/null +++ b/src/main/java/frc/robot/TalonFXConstants.java @@ -0,0 +1,7 @@ +package frc.robot; + +public class TalonFXConstants { + public static final boolean TALON_FUTURE_PROOF = true; + public static final String CANIVORE_NAME = "canivoreBus"; + +} diff --git a/src/main/java/frc/robot/commands/AutoAimCommand.java b/src/main/java/frc/robot/commands/AutoAimCommand.java new file mode 100644 index 0000000..2684b15 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoAimCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class AutoAimCommand extends SwerveRequest.FieldCentricFacingAngle { + + private Translation2d pointToFace; + + @Override + public StatusCode apply( + SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) { + this.TargetDirection = pointToFace.minus(parameters.currentPose.getTranslation()).getAngle(); + if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) { + // This is an angle from the frame of the reference of the field. Subtract + // the operator persepctive to counteract CTRE adding it later + this.TargetDirection = this.TargetDirection.minus(parameters.operatorForwardDirection); + } + // TODO: Adjust direction we're aiming based on current robot velocity in + // parameters.currentChassisSpeed + return super.apply(parameters, modulesToApply); + } + + @Override + public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) { + // Ignore this decorator, since we want to update this at 250hz in ::apply instead + // of 50hz in the main robot loop + return this; + } + + public void setPointToFace(Translation2d point) { + pointToFace = point; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java new file mode 100644 index 0000000..3942d62 --- /dev/null +++ b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java @@ -0,0 +1,63 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LimelightHelpers; + +import java.util.function.DoubleSupplier; + + +public class InboundingBoxAlignCommand extends Command { + private final CommandSwerveDrivetrain drivetrain; + private final DoubleSupplier xVelSupplier; + private final AutoAimCommand poseAimRequest; + private final DoubleSupplier yVelSupplier; + private double currentTag; + + public InboundingBoxAlignCommand( + CommandSwerveDrivetrain drivetrain, + DoubleSupplier xVelSupplier, + DoubleSupplier yVelSupplier) { + + this.drivetrain = drivetrain; + this.xVelSupplier = xVelSupplier; + this.yVelSupplier = yVelSupplier; + + addRequirements(drivetrain); + + poseAimRequest = new AutoAimCommand(); + poseAimRequest.HeadingController.setPID(5,0,0); + poseAimRequest.HeadingController.enableContinuousInput(-Math.PI,Math.PI); + } + + @Override + public void initialize() { + currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME); + + Translation2d speakerCenter = AllianceFlipUtil.apply( + Constants.FieldConstants.InboundingBox.inboundingBoxPosition.toTranslation2d() + ); + + poseAimRequest.setPointToFace(speakerCenter); + } + + @Override + public void execute() { + + } + + @Override + public boolean isFinished() { + + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/robot/subsystems/DumperSubsystem.java b/src/main/java/frc/robot/subsystems/DumperSubsystem.java index f3e6061..84d5e39 100644 --- a/src/main/java/frc/robot/subsystems/DumperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DumperSubsystem.java @@ -1,24 +1,43 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.TalonFXConstants; public class DumperSubsystem extends SubsystemBase { - - public class DumperConstants { - + public final TalonFX dumperMotor; + public final CANcoder dumperEncoder; + private final StatusSignal dumperPositionStatusSignal; + + public DumperSubsystem(StatusSignal dumperPositionStatusSignal, StatusSignal dumperPositionStatusSignal1) { + this.dumperPositionStatusSignal = dumperPositionStatusSignal1; + dumperMotor = new TalonFX(DumperConstants.DUMPER_ID, TalonFXConstants.CANIVORE_NAME); + dumperEncoder = new CANcoder(DumperConstants.DUMPER_ID, TalonFXConstants.CANIVORE_NAME); + dumperPositionStatusSignal = dumperMotor.getPosition(); } - private final static DumperSubsystem INSTANCE = new DumperSubsystem(); - - - public static DumperSubsystem getInstance() { - return INSTANCE; + public class DumperConstants { + public double dumpOpen = 0.0; + public double doubleClosed = 0.0; + public static int DUMPER_ID = 0; + public static final double DUMPER_P = 0; + public static final double DUMPER_I = 0; + public static final double DUMPER_D = 0.; + public static final double DUMPER_G = 0.0; + public static final double DUMPER_A = 0.00; + public static final double DUMPER_V = 0.0; + public static final double PROFILE_V = 0.0; + public static final double PROFILE_A = 0.0; } - - private DumperSubsystem() { - + @Override + public void periodic() { + dumperPositionStatusSignal.refresh(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e3d86e6..1d45f27 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -5,20 +5,17 @@ public class IntakeSubsystem extends SubsystemBase { - public class IntaleConstants { - - } - - private final static IntakeSubsystem INSTANCE = new IntakeSubsystem(); - - - public static IntakeSubsystem getInstance() { - return INSTANCE; - } - - - private IntakeSubsystem() { - + public class IntakeConstants { + + public int INTAKE_ID = 0; + public static final double INTAKE_P = 0; + public static final double INTAKE_I = 0; + public static final double INTAKE_D = 0.; + public static final double INTAKE_G = 0.0; + public static final double INTAKE_A = 0.00; + public static final double INTAKE_V = 0.0; + public static final double PROFILE_V = 0.0; + public static final double PROFILE_A = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 138712f..050660d 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -2,23 +2,24 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.LimelightHelpers; public class VisionSubsystem extends SubsystemBase { public class VisionConstants { - + public static final String LIMELIGHT_NAME = "limelight"; } - - private final static VisionSubsystem INSTANCE = new VisionSubsystem(); - - public static VisionSubsystem getInstance() { - return INSTANCE; + public double getX() { + return LimelightHelpers.getTX(VisionConstants.LIMELIGHT_NAME); } + public double getY() { + return LimelightHelpers.getTY(VisionConstants.LIMELIGHT_NAME); + } - private VisionSubsystem() { - + public LimelightHelpers.Results getTargetingResults() { + return LimelightHelpers.getLatestResults(VisionConstants.LIMELIGHT_NAME).targetingResults; } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index de708d6..3c1acbe 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -219,7 +219,6 @@ public void periodic() { SmartDashboard.putNumber("distance", distance); SmartDashboard.putNumber("gyro spin rate", getPigeon2().getRate()); } - } diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index 17e5a8b..32e081b 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -13,14 +13,14 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.constants.FieldConstants; +import frc.robot.Constants; /** Utility functions for flipping from the blue to red alliance. */ public class AllianceFlipUtil { /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ public static double apply(double xCoordinate) { if (shouldFlip()) { - return FieldConstants.FIELD_LENGTH - xCoordinate; + return Constants.FieldConstants.fieldLength - xCoordinate; } else { return xCoordinate; } diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java new file mode 100644 index 0000000..eb1855f --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -0,0 +1,780 @@ +//LimelightHelpers v1.2.1 (March 1, 2023) + +package frc.robot.util; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { +// System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + //System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + // System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java b/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java index f07b31c..49c1693 100644 --- a/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java +++ b/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java @@ -1,22 +1,21 @@ package frc.robot.util.controllerUtils; import edu.wpi.first.wpilibj.XboxController; - -import java.util.Map; +import frc.robot.Constants; public class ControllerContainer { private final Controller[] controllers; - public ControllerContainer(Map controllers) { - this.controllers = new Controller[controllers.size()]; + public ControllerContainer() { + controllers = new Controller[Constants.OIConstants.CONTROLLER_COUNT]; - controllers.forEach((port, type) -> { + Constants.OIConstants.CONTROLLERS.forEach((port, type) -> { switch (type) { case CUSTOM: - this.controllers[port] = new CustomController(port); + controllers[port] = new CustomController(port); break; case XBOX: - this.controllers[port] = new CustomXboxController(port); + controllers[port] = new CustomXboxController(port); break; } }); @@ -81,4 +80,4 @@ public double getRightX() { return super.getRawAxis(XboxController.Axis.kRightX.value); } } -} +} \ No newline at end of file From c69659c419905dfe3413d16a6e114a32dd1d3879 Mon Sep 17 00:00:00 2001 From: qwertycloudhub Date: Mon, 11 Nov 2024 22:50:55 -0500 Subject: [PATCH 2/5] fixed build issue --- src/main/java/frc/robot/Constants.java | 25 +++++------ .../commands/InboundingBoxAlignCommand.java | 2 +- .../frc/robot/subsystems/DumperSubsystem.java | 43 ------------------- .../robot/subsystems/ShooterSubsystem.java | 9 ++++ .../java/frc/robot/util/AllianceFlipUtil.java | 2 +- .../controllerUtils/ControllerContainer.java | 4 +- 6 files changed, 23 insertions(+), 62 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/DumperSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4f7df8c..8a45a35 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,24 +7,19 @@ public class Constants { - public class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(651.223); - public final class InboundingBox { - public static final Translation3d inboundingBoxPosition = new Translation3d(0, 0, 0); - } + public static final double fieldLength = Units.inchesToMeters(651.223); + public static final class InboundingBox { + public static final Translation3d inboundingBoxPosition = new Translation3d(0, 0, 0); } - public class OIConstants{ - public static final Map CONTROLLERS = Map.of( - 0, ControllerType.CUSTOM, 1, ControllerType.XBOX - ); + public static final Map CONTROLLERS = Map.of( + 0, ControllerType.CUSTOM, 1, ControllerType.XBOX + ); - public static final int CONTROLLER_COUNT = CONTROLLERS.size(); + public static final int CONTROLLER_COUNT = CONTROLLERS.size(); - - public enum ControllerType { - CUSTOM, - XBOX - } + public enum ControllerType { + CUSTOM, + XBOX } } diff --git a/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java index 3942d62..32a8f05 100644 --- a/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java +++ b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java @@ -39,7 +39,7 @@ public void initialize() { currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME); Translation2d speakerCenter = AllianceFlipUtil.apply( - Constants.FieldConstants.InboundingBox.inboundingBoxPosition.toTranslation2d() + Constants.InboundingBox.inboundingBoxPosition.toTranslation2d() ); poseAimRequest.setPointToFace(speakerCenter); diff --git a/src/main/java/frc/robot/subsystems/DumperSubsystem.java b/src/main/java/frc/robot/subsystems/DumperSubsystem.java deleted file mode 100644 index 84d5e39..0000000 --- a/src/main/java/frc/robot/subsystems/DumperSubsystem.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.subsystems; - - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.TalonFXConstants; - -public class DumperSubsystem extends SubsystemBase { - public final TalonFX dumperMotor; - public final CANcoder dumperEncoder; - private final StatusSignal dumperPositionStatusSignal; - - public DumperSubsystem(StatusSignal dumperPositionStatusSignal, StatusSignal dumperPositionStatusSignal1) { - this.dumperPositionStatusSignal = dumperPositionStatusSignal1; - dumperMotor = new TalonFX(DumperConstants.DUMPER_ID, TalonFXConstants.CANIVORE_NAME); - dumperEncoder = new CANcoder(DumperConstants.DUMPER_ID, TalonFXConstants.CANIVORE_NAME); - dumperPositionStatusSignal = dumperMotor.getPosition(); - } - - public class DumperConstants { - public double dumpOpen = 0.0; - public double doubleClosed = 0.0; - public static int DUMPER_ID = 0; - public static final double DUMPER_P = 0; - public static final double DUMPER_I = 0; - public static final double DUMPER_D = 0.; - public static final double DUMPER_G = 0.0; - public static final double DUMPER_A = 0.00; - public static final double DUMPER_V = 0.0; - public static final double PROFILE_V = 0.0; - public static final double PROFILE_A = 0.0; - } - - @Override - public void periodic() { - dumperPositionStatusSignal.refresh(); - } -} - diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..3b22b36 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + +} + diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index 32e081b..a569369 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -20,7 +20,7 @@ public class AllianceFlipUtil { /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ public static double apply(double xCoordinate) { if (shouldFlip()) { - return Constants.FieldConstants.fieldLength - xCoordinate; + return Constants.fieldLength - xCoordinate; } else { return xCoordinate; } diff --git a/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java b/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java index 49c1693..c60a593 100644 --- a/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java +++ b/src/main/java/frc/robot/util/controllerUtils/ControllerContainer.java @@ -7,9 +7,9 @@ public class ControllerContainer { private final Controller[] controllers; public ControllerContainer() { - controllers = new Controller[Constants.OIConstants.CONTROLLER_COUNT]; + controllers = new Controller[Constants.CONTROLLER_COUNT]; - Constants.OIConstants.CONTROLLERS.forEach((port, type) -> { + Constants.CONTROLLERS.forEach((port, type) -> { switch (type) { case CUSTOM: controllers[port] = new CustomController(port); From 5998830e7e84ed860d9eaadb77a7ca17a4431989 Mon Sep 17 00:00:00 2001 From: qwertycloudhub Date: Mon, 11 Nov 2024 23:20:59 -0500 Subject: [PATCH 3/5] 11/11/24 --- src/main/java/frc/robot/Constants.java | 9 ++- src/main/java/frc/robot/RobotContainer.java | 7 ++- .../commands/CenterDunkTankAlignCommand.java | 62 +++++++++++++++++++ .../robot/commands/DunkTankAlignCommand.java | 62 +++++++++++++++++++ .../commands/InboundingBoxAlignCommand.java | 21 +++---- .../frc/robot/subsystems/IntakeSubsystem.java | 10 +-- .../robot/subsystems/ShooterSubsystem.java | 20 +++++- 7 files changed, 171 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CenterDunkTankAlignCommand.java create mode 100644 src/main/java/frc/robot/commands/DunkTankAlignCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8a45a35..4430f6b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,13 +11,18 @@ public class Constants { public static final class InboundingBox { public static final Translation3d inboundingBoxPosition = new Translation3d(0, 0, 0); } + public static final class CenterTank { + public static final Translation3d centerTankPosition = new Translation3d(0,0,0); + } + public static final class DunkTank { + public static final Translation3d dunkTankPosition = new Translation3d(0,0,0); + } + public static final Map CONTROLLERS = Map.of( 0, ControllerType.CUSTOM, 1, ControllerType.XBOX ); - public static final int CONTROLLER_COUNT = CONTROLLERS.size(); - public enum ControllerType { CUSTOM, XBOX diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ff47566..f3a6611 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.CenterDunkTankAlignCommand; +import frc.robot.commands.DunkTankAlignCommand; import frc.robot.commands.InboundingBoxAlignCommand; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.drivetrain.generated.TunerConstants; @@ -47,7 +49,10 @@ private void configureBindings() { .withRotationalRate(-joystick.getRightX() * CommandSwerveDrivetrain.MaFxAngularRate) )); - joystick.leftTrigger().onTrue(new InboundingBoxAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); + joystick.leftBumper().whileTrue(new InboundingBoxAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); + joystick.rightBumper().whileTrue(new DunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); + joystick.a().whileTrue(new CenterDunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/CenterDunkTankAlignCommand.java b/src/main/java/frc/robot/commands/CenterDunkTankAlignCommand.java new file mode 100644 index 0000000..85754b9 --- /dev/null +++ b/src/main/java/frc/robot/commands/CenterDunkTankAlignCommand.java @@ -0,0 +1,62 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LimelightHelpers; + +import java.util.function.DoubleSupplier; + + +public class CenterDunkTankAlignCommand extends Command { + private final CommandSwerveDrivetrain drivetrain; + private final DoubleSupplier xVelSupplier; + private final AutoAimCommand poseAimRequest; + private final DoubleSupplier yVelSupplier; + private double currentTag; + + public CenterDunkTankAlignCommand( + CommandSwerveDrivetrain drivetrain, + DoubleSupplier xVelSupplier, + DoubleSupplier yVelSupplier) { + + this.drivetrain = drivetrain; + this.xVelSupplier = xVelSupplier; + this.yVelSupplier = yVelSupplier; + + addRequirements(drivetrain); + + poseAimRequest = new AutoAimCommand(); + poseAimRequest.HeadingController.setPID(5,0,0); + poseAimRequest.HeadingController.enableContinuousInput(-Math.PI,Math.PI); + } + + @Override + public void initialize() { + currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME); + + Translation2d centerTank = AllianceFlipUtil.apply( + Constants.CenterTank.centerTankPosition.toTranslation2d() + ); + + poseAimRequest.setPointToFace(centerTank); + } + + @Override + public void execute() { + if(LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME) == currentTag) { + drivetrain.setControl( + poseAimRequest.withVelocityX(xVelSupplier.getAsDouble() * 1.5).withVelocityY(yVelSupplier.getAsDouble() * 1.5) + ); + } else { + end(true); + } + } + + @Override + public boolean isFinished() {return false;} + +} diff --git a/src/main/java/frc/robot/commands/DunkTankAlignCommand.java b/src/main/java/frc/robot/commands/DunkTankAlignCommand.java new file mode 100644 index 0000000..cba7927 --- /dev/null +++ b/src/main/java/frc/robot/commands/DunkTankAlignCommand.java @@ -0,0 +1,62 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LimelightHelpers; + +import java.util.function.DoubleSupplier; + + +public class DunkTankAlignCommand extends Command { + private final CommandSwerveDrivetrain drivetrain; + private final DoubleSupplier xVelSupplier; + private final AutoAimCommand poseAimRequest; + private final DoubleSupplier yVelSupplier; + private double currentTag; + + public DunkTankAlignCommand( + CommandSwerveDrivetrain drivetrain, + DoubleSupplier xVelSupplier, + DoubleSupplier yVelSupplier) { + + this.drivetrain = drivetrain; + this.xVelSupplier = xVelSupplier; + this.yVelSupplier = yVelSupplier; + + addRequirements(drivetrain); + + poseAimRequest = new AutoAimCommand(); + poseAimRequest.HeadingController.setPID(5,0,0); + poseAimRequest.HeadingController.enableContinuousInput(-Math.PI,Math.PI); + } + + @Override + public void initialize() { + currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME); + + Translation2d dunkTank = AllianceFlipUtil.apply( + Constants.DunkTank.dunkTankPosition.toTranslation2d() + ); + + poseAimRequest.setPointToFace(dunkTank); + } + + @Override + public void execute() { + if(LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME) == currentTag) { + drivetrain.setControl( + poseAimRequest.withVelocityX(xVelSupplier.getAsDouble() * 1.5).withVelocityY(yVelSupplier.getAsDouble() * 1.5) + ); + } else { + end(true); + } + } + + @Override + public boolean isFinished() {return false;} + +} diff --git a/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java index 32a8f05..6681f23 100644 --- a/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java +++ b/src/main/java/frc/robot/commands/InboundingBoxAlignCommand.java @@ -38,26 +38,25 @@ public InboundingBoxAlignCommand( public void initialize() { currentTag = LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME); - Translation2d speakerCenter = AllianceFlipUtil.apply( + Translation2d inboundingBox = AllianceFlipUtil.apply( Constants.InboundingBox.inboundingBoxPosition.toTranslation2d() ); - poseAimRequest.setPointToFace(speakerCenter); + poseAimRequest.setPointToFace(inboundingBox); } @Override public void execute() { - - } - - @Override - public boolean isFinished() { - - return false; + if(LimelightHelpers.getFiducialID(VisionSubsystem.VisionConstants.LIMELIGHT_NAME) == currentTag) { + drivetrain.setControl( + poseAimRequest.withVelocityX(xVelSupplier.getAsDouble() * 1.5).withVelocityY(yVelSupplier.getAsDouble() * 1.5) + ); + } else { + end(true); + } } @Override - public void end(boolean interrupted) { + public boolean isFinished() {return false;} - } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1d45f27..137edc3 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -10,12 +10,12 @@ public class IntakeConstants { public int INTAKE_ID = 0; public static final double INTAKE_P = 0; public static final double INTAKE_I = 0; - public static final double INTAKE_D = 0.; + public static final double INTAKE_D = 0; public static final double INTAKE_G = 0.0; - public static final double INTAKE_A = 0.00; - public static final double INTAKE_V = 0.0; - public static final double PROFILE_V = 0.0; - public static final double PROFILE_A = 0.0; + public static final double INTAKE_A = 0; + public static final double INTAKE_V = 0; + public static final double PROFILE_V = 0; + public static final double PROFILE_A = 0; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3b22b36..2841841 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,9 +1,27 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.Slot0Configs; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - + + public class ShooterConstants{ + public int SHOOTER_ID = 0; + public static final double SHOOTER_P = 0; + public static final double SHOOTER_I = 0; + public static final double SHOOTER_D = 0; + public static final double SHOOTER_S = 0; + public static final double SHOOTER_V = 0; + public static final double MOTION_MAGIC_ACCELERATION = 0; + + public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs(). + withKS(SHOOTER_S). + withKP(SHOOTER_P). + withKI(SHOOTER_I). + withKD(SHOOTER_D). + withKA(MOTION_MAGIC_ACCELERATION). + withKV(SHOOTER_V); + } } From 8be7a56fed87e9971a13d352ab0c85a835469d22 Mon Sep 17 00:00:00 2001 From: qwertycloudhub Date: Tue, 12 Nov 2024 19:17:07 -0500 Subject: [PATCH 4/5] intake subsystem --- src/main/java/frc/robot/RobotContainer.java | 7 +++- .../frc/robot/subsystems/IntakeSubsystem.java | 32 +++++++++++++------ .../robot/subsystems/ShooterSubsystem.java | 2 +- 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f3a6611..31a47e1 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.CenterDunkTankAlignCommand; @@ -18,12 +19,16 @@ import frc.robot.subsystems.drivetrain.generated.TunerConstants; import frc.robot.util.controllerUtils.ButtonHelper; import frc.robot.util.controllerUtils.ControllerContainer; +import frc.robot.subsystems.IntakeSubsystem; public class RobotContainer { public ControllerContainer controllerContainer = new ControllerContainer(); public final CommandXboxController joystick = new CommandXboxController(1); ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers()); + final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; + public final IntakeSubsystem intake = new IntakeSubsystem(); + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(CommandSwerveDrivetrain.MAX_VELOCITY_METERS_PER_SECOND * 0.1) .withRotationalDeadband(CommandSwerveDrivetrain.MaFxAngularRate * 0.1) @@ -52,7 +57,7 @@ private void configureBindings() { joystick.leftBumper().whileTrue(new InboundingBoxAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); joystick.rightBumper().whileTrue(new DunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); joystick.a().whileTrue(new CenterDunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); - + joystick.leftTrigger().whileTrue(intake.rollIn(0)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 137edc3..ac1b95d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,21 +1,33 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { public class IntakeConstants { + private static int INTAKE_ID = 0; - public int INTAKE_ID = 0; - public static final double INTAKE_P = 0; - public static final double INTAKE_I = 0; - public static final double INTAKE_D = 0; - public static final double INTAKE_G = 0.0; - public static final double INTAKE_A = 0; - public static final double INTAKE_V = 0; - public static final double PROFILE_V = 0; - public static final double PROFILE_A = 0; + private static final TalonFX intakeMotor = new TalonFX(INTAKE_ID); + + private static final VoltageOut intakeVoltage = new VoltageOut(0); + + private static double ROLL_IN_POWER = 0; + } + private void stop() { + IntakeConstants.intakeMotor.stopMotor(); } -} + private void setIntakeSpeed(double speed) { + IntakeConstants.intakeMotor.set(speed); + } + + private Command roll(double vel){return startEnd(() -> setIntakeSpeed(vel), this::stop);} + + public Command rollIn(double vel){ + return roll(vel); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2841841..796425e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -6,7 +6,7 @@ public class ShooterSubsystem extends SubsystemBase { - public class ShooterConstants{ + private class ShooterConstants{ public int SHOOTER_ID = 0; public static final double SHOOTER_P = 0; public static final double SHOOTER_I = 0; From b7a81e421bfdcf71716d11b233e0619577273a4b Mon Sep 17 00:00:00 2001 From: qwertycloudhub Date: Tue, 12 Nov 2024 20:02:47 -0500 Subject: [PATCH 5/5] shooter subsystem and polish --- src/main/java/frc/robot/Robot.java | 12 ------ src/main/java/frc/robot/RobotContainer.java | 9 ++--- .../frc/robot/commands/AutoAimCommand.java | 6 --- .../frc/robot/subsystems/IntakeSubsystem.java | 14 +++---- .../robot/subsystems/ShooterSubsystem.java | 38 +++++++++++-------- .../drivetrain/CommandSwerveDrivetrain.java | 23 ++++------- 6 files changed, 39 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 06eb36e..2878e8f 100755 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,17 +12,11 @@ public class Robot extends TimedRobot { @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } @@ -41,7 +35,6 @@ public void disabledPeriodic() { public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) if (autonomousCommand != null) { autonomousCommand.schedule(); } @@ -55,10 +48,6 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); } @@ -72,7 +61,6 @@ public void teleopPeriodic() { @Override public void testInit() { - // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31a47e1..16bf34c 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,14 +7,12 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.CenterDunkTankAlignCommand; import frc.robot.commands.DunkTankAlignCommand; import frc.robot.commands.InboundingBoxAlignCommand; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.drivetrain.generated.TunerConstants; import frc.robot.util.controllerUtils.ButtonHelper; @@ -28,6 +26,7 @@ public class RobotContainer { final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; public final IntakeSubsystem intake = new IntakeSubsystem(); + public final ShooterSubsystem shooter = new ShooterSubsystem(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(CommandSwerveDrivetrain.MAX_VELOCITY_METERS_PER_SECOND * 0.1) @@ -44,13 +43,12 @@ public RobotContainer() { private void configureBindings() { - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.setDefaultCommand( drivetrain.applyRequest( () -> drive .withVelocityX(-joystick.getLeftY() * TunerConstants.kSpeedAt12VoltsMps) .withVelocityY(-joystick.getLeftX() * TunerConstants.kSpeedAt12VoltsMps) - // +rotational rate = counterclockwise (left), -X in joystick = left .withRotationalRate(-joystick.getRightX() * CommandSwerveDrivetrain.MaFxAngularRate) )); @@ -58,6 +56,7 @@ private void configureBindings() { joystick.rightBumper().whileTrue(new DunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); joystick.a().whileTrue(new CenterDunkTankAlignCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())); joystick.leftTrigger().whileTrue(intake.rollIn(0)); + joystick.rightTrigger().whileTrue(shooter.shootElectrolyte(0)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AutoAimCommand.java b/src/main/java/frc/robot/commands/AutoAimCommand.java index 2684b15..4c0be00 100644 --- a/src/main/java/frc/robot/commands/AutoAimCommand.java +++ b/src/main/java/frc/robot/commands/AutoAimCommand.java @@ -15,19 +15,13 @@ public StatusCode apply( SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) { this.TargetDirection = pointToFace.minus(parameters.currentPose.getTranslation()).getAngle(); if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) { - // This is an angle from the frame of the reference of the field. Subtract - // the operator persepctive to counteract CTRE adding it later this.TargetDirection = this.TargetDirection.minus(parameters.operatorForwardDirection); } - // TODO: Adjust direction we're aiming based on current robot velocity in - // parameters.currentChassisSpeed return super.apply(parameters, modulesToApply); } @Override public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) { - // Ignore this decorator, since we want to update this at 250hz in ::apply instead - // of 50hz in the main robot loop return this; } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ac1b95d..24b004a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -8,21 +8,19 @@ public class IntakeSubsystem extends SubsystemBase { - public class IntakeConstants { - private static int INTAKE_ID = 0; - private static final TalonFX intakeMotor = new TalonFX(INTAKE_ID); + private static int INTAKE_ID = 0; - private static final VoltageOut intakeVoltage = new VoltageOut(0); + private static final TalonFX intakeMotor = new TalonFX(INTAKE_ID); + + private static double ROLL_IN_POWER = 0; - private static double ROLL_IN_POWER = 0; - } private void stop() { - IntakeConstants.intakeMotor.stopMotor(); + intakeMotor.stopMotor(); } private void setIntakeSpeed(double speed) { - IntakeConstants.intakeMotor.set(speed); + intakeMotor.set(speed); } private Command roll(double vel){return startEnd(() -> setIntakeSpeed(vel), this::stop);} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 796425e..4927dfc 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -2,26 +2,32 @@ import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private class ShooterConstants{ - public int SHOOTER_ID = 0; - public static final double SHOOTER_P = 0; - public static final double SHOOTER_I = 0; - public static final double SHOOTER_D = 0; - public static final double SHOOTER_S = 0; - public static final double SHOOTER_V = 0; - public static final double MOTION_MAGIC_ACCELERATION = 0; - - public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs(). - withKS(SHOOTER_S). - withKP(SHOOTER_P). - withKI(SHOOTER_I). - withKD(SHOOTER_D). - withKA(MOTION_MAGIC_ACCELERATION). - withKV(SHOOTER_V); + + private static int SHOOTER_ID = 0; + + private static final TalonFX shooterMotor = new TalonFX(SHOOTER_ID); + + private static double SHOOTER_POWER = 0; + + + private void stop() { + shooterMotor.stopMotor(); + } + + private void setShooterSpeed(double speed) { + shooterMotor.set(speed); + } + + private Command roll(double vel){return startEnd(() -> setShooterSpeed(vel), this::stop);} + + public Command shootElectrolyte(double vel){ + return roll(Math.abs(vel)); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 3c1acbe..1b7b464 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -25,12 +25,9 @@ import java.util.function.Supplier; -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem - * so it can be used in command-based projects easily. - */ + public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms + private static final double kSimLoopPeriod = 0.005; private final Rotation2d BluePerspectiveRotation = Rotation2d.fromDegrees(0); private final Rotation2d RedPerspectiveRotation = Rotation2d.fromDegrees(180); private final SwerveRequest.ApplyChassisSpeeds AutoReq = new SwerveRequest.ApplyChassisSpeeds(); @@ -47,27 +44,23 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst public static final double PEAK_REVERSE_VOLTAGE = -12.0; public static final double SWERVE_X_REDUCTION = 1.0 / 6.75; - public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); //0.1016 + public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); - public static final double MaFxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + public static final double MaFxAngularRate = 1.5 * Math.PI; - public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 / 60.0 * SWERVE_X_REDUCTION * WHEEL_DIAMETER * Math.PI; //placeholder + public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 / 60.0 * SWERVE_X_REDUCTION * WHEEL_DIAMETER * Math.PI; - private static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(22.25); //PLACEHOLDER - private static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(22.25); //PLACEHOLDER + private static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(22.25); + private static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(22.25); public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics( - // Front left new Translation2d(DRIVETRAIN_WHEELBASE_METERS / 2.0, DRIVETRAIN_TRACKWIDTH_METERS / 2.0), - // Front right new Translation2d(DRIVETRAIN_WHEELBASE_METERS / 2.0, -DRIVETRAIN_TRACKWIDTH_METERS / 2.0), - // Back left new Translation2d(-DRIVETRAIN_WHEELBASE_METERS / 2.0, DRIVETRAIN_TRACKWIDTH_METERS / 2.0), - // Back right new Translation2d(-DRIVETRAIN_WHEELBASE_METERS / 2.0, -DRIVETRAIN_TRACKWIDTH_METERS / 2.0) ); @@ -122,13 +115,11 @@ public ChassisSpeeds getChassisSpeeds() { private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); - /* Run simulation at a faster rate so PID gains behave more reasonably */ m_simNotifier = new Notifier(() -> { final double currentTime = Utils.getCurrentTimeSeconds(); double deltaTime = currentTime - m_lastSimTime; m_lastSimTime = currentTime; - /* use the measured time delta, get battery voltage from WPILib */ updateSimState(deltaTime, RobotController.getBatteryVoltage()); }); m_simNotifier.startPeriodic(kSimLoopPeriod);