diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4a6f4d8..dfbd494 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,10 +53,10 @@ import frc4388.robot.constants.Constants.SimConstants.Mode; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Elevator.CoordinationState; +import frc4388.robot.subsystems.elevator.Elevator; +import frc4388.robot.subsystems.elevator.Elevator.CoordinationState; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; @@ -80,7 +80,7 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 45eb62a..01cdb68 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -21,14 +21,16 @@ import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.elevator.ElevatorIO; +import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.lidar.LidarIO; -import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.lidar.LidarReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; import frc4388.robot.subsystems.swerve.SwerveIO; -import frc4388.robot.subsystems.swerve.SwervePhoenix; +import frc4388.robot.subsystems.swerve.SwerveReal; import frc4388.robot.subsystems.vision.VisionIO; -import frc4388.robot.subsystems.vision.VisionPhotonvision; +import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; @@ -58,13 +60,7 @@ public class RobotMap { public final SwerveIO swerveDrivetrain; /* Elevator Subsystem */ - public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); - - - public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); - public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); - public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + public final ElevatorIO elevatorIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { @@ -73,28 +69,40 @@ public class RobotMap { PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; - rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // Configure LiDAR - reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); - reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); // Configure swerve drive train - SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, SwerveDriveConstants.DrivetrainConstants, SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT ); - swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal); + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + + // Configure elevator + + TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); + TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + + + DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); + DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + + elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); + // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); FaultTalonFX.addDevice(elevator, "Elevator"); @@ -122,6 +130,7 @@ public class RobotMap { reefLidar = new LidarIO() {}; reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; + elevatorIO = new ElevatorIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java index 7108de5..7d2ec77 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitElevatorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java index 73fd893..1ff48cd 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitEndefectorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java index 2f66710..992879a 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java +++ b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitFeedCoral extends Command { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f1e17b4..5aeda6b 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 172; - public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874"; - public static final String GIT_DATE = "2025-07-15 13:42:25 MDT"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; + public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT"; - public static final long BUILD_UNIX_TIME = 1752704433011L; + public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; + public static final long BUILD_UNIX_TIME = 1752774931371L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 4aff045..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,107 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.swerve.SwerveDriveConstants; -// import frc4388.utility.RobotGyro; -import frc4388.utility.compute.RobotTime; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase implements Queryable { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private TalonFX m_leftFrontMotor; - private TalonFX m_rightFrontMotor; - private TalonFX m_leftBackMotor; - private TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - // private Pigeon2 m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) { - - FaultReporter.register(this); - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); - m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); - m_driveTrain = driveTrain; - // m_gyro = gyro; - } - - @Override - public void periodic() { - if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - // SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - // SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - // SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - //SmartDashboard.putData(m_gyro); - } - - - @Override - public String getName() { - return "Diff Drive"; - } - - // @Override - // public void queryStatus() { - // // TODO: Add Stuff - // } - - @Override - public Status diagnosticStatus() { - // Log("Diagnostic info for this has not been inplemented!"); //TODO - return new Status(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java similarity index 53% rename from src/main/java/frc4388/robot/subsystems/Elevator.java rename to src/main/java/frc4388/robot/subsystems/elevator/Elevator.java index 7ac9418..a054425 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java @@ -2,7 +2,10 @@ // 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 frc4388.robot.subsystems; +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,38 +17,31 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState; import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; import frc4388.utility.status.Status.ReportLevel; public class Elevator extends SubsystemBase implements Queryable { + ElevatorIO io; + ElevatorStateAutoLogged state = new ElevatorStateAutoLogged(); + /** Creates a new Elevator. */ - private TalonFX elevatorMotor; - private TalonFX endeffectorMotor; private LED led; - // @AutoLog - // private class ElevatorState { - @SuppressWarnings("unused") - public long wait = 0; - public long maxWait = 1000; + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; - public double elevatorRefrence = 0; - public double endeffectorRefrence = 0; + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; - public boolean elevatorManualStop = true; - public boolean endefectorManualStop = true; + public boolean disableAutoIntake = false; - public boolean disableAutoIntake = false; - - public boolean seededZeroEndefector = false; - public boolean seededZeroElevator = false; - - public DigitalInput basinBeamBreak; - public DigitalInput endeffectorLimitSwitch; - public DigitalInput intakeIR; - // } + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; // private ElevatorState state = new ElevatorState(); @@ -69,43 +65,15 @@ public class Elevator extends SubsystemBase implements Queryable { private CoordinationState currentState; // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { - elevatorMotor = elevatorTalonFX; - endeffectorMotor = endeffectorTalonFX; + public Elevator(ElevatorIO io, LED led) { + this.io = io; this.led = led; - this.basinBeamBreak = basinLimitSwitch; - this.endeffectorLimitSwitch = endeffectorLimitSwitch; - this.intakeIR = intakeDigitalInput; - - elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); - - elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); - endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); currentState = CoordinationState.Ready; FaultReporter.register(this); } - //PID methods - - private void PIDPosition(TalonFX motor, double position) { - if (motor == elevatorMotor) elevatorRefrence = position; - else endeffectorRefrence = position; - - var request = new PositionDutyCycle(position); - motor.setControl(request); - } - - public void elevatorStop() { - elevatorMotor.set(0); - } - - public void endeffectorStop() { - endeffectorMotor.set(0); - } - public void transitionState(CoordinationState state) { // elevatorMotor.enable(); @@ -115,98 +83,98 @@ public class Elevator extends SubsystemBase implements Queryable { switch (currentState) { case Waiting: { wait = System.currentTimeMillis() + maxWait; - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); led.setMode(LEDConstants.WAITING_PATTERN); break; } case WatingBeamTripped: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; } case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; } case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case L2ScoreLeave: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL3Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); break; } case BallRemoverL3Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } @@ -223,27 +191,25 @@ public class Elevator extends SubsystemBase implements Queryable { } public boolean elevatorAtReference() { - // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); - double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); - double diffrence = elevatorRefrence - elevatorPosition; + double diffrence = state.elevatorRefrence - state.elevatorPosition; boolean headedUp = diffrence < 0; - boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.elevatorForwardLimit && !headedUp) + ); } public boolean endeffectorAtReference() { - // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); - double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); - double diffrence = endeffectorRefrence - endeffectorPosition; + double diffrence = state.endeffectorRefrence - state.endeffectorPosition; boolean headedUp = diffrence < 0; - boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.endeffectorForwardLimit && !headedUp) + ); } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -252,11 +218,11 @@ public class Elevator extends SubsystemBase implements Queryable { // } public boolean getEndeffectorLimit() { - return endeffectorLimitSwitch.get(); + return state.endeffectorLimitSwitch; } private void periodicWaiting() { - if (!basinBeamBreak.get()) + if (!state.basinBeamBreak) transitionState(CoordinationState.Ready); // if(!endeffectorLimitSwitch.get()) // transitionState(CoordinationState.Hovering); @@ -268,39 +234,39 @@ public class Elevator extends SubsystemBase implements Queryable { // } private void periodicReady() { - if (elevatorAtReference() && !endeffectorLimitSwitch.get()) + if (elevatorAtReference() && !state.endeffectorLimitSwitch) transitionState(CoordinationState.Hovering); - if(elevatorAtReference() && endeffectorLimitSwitch.get()) + if(elevatorAtReference() && state.endeffectorLimitSwitch) transitionState(CoordinationState.Hovering); } @SuppressWarnings("unused") private void periodicScoring() { - if (!endeffectorLimitSwitch.get()) + if (!state.endeffectorLimitSwitch) transitionState(CoordinationState.Waiting); } public void manualElevatorVel(double velocity) { if (Math.abs(velocity) > 0.1) { - elevatorMotor.set(velocity); + io.elevatorToVelocity(velocity); elevatorManualStop = false; return; } if (!elevatorManualStop) { elevatorManualStop = true; - elevatorMotor.set(0); + io.elevatorToVelocity(0); } } public void manualEndeffectorVel(double velocity) { if (Math.abs(velocity) > 0.1) { - endeffectorMotor.set(velocity); + io.endeffectorToVelocity(velocity); endefectorManualStop = false; return; } if (!endefectorManualStop) { endefectorManualStop = true; - endeffectorMotor.set(0); + io.endeffectorToVelocity(0); } } @@ -320,18 +286,21 @@ public class Elevator extends SubsystemBase implements Queryable { // This method will be called once per scheduler run // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); - SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); - SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); - SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); - SmartDashboard.putString("State", currentState.toString()); + // SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); + // SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + // SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); + // SmartDashboard.putString("State", currentState.toString()); - if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { - endeffectorMotor.setPosition(0); + io.updateInputs(state); + Logger.processInputs("Elevator", state); + + if (!seededZeroEndefector && state.endeffectorForwardLimit) { + io.endeffectorToPosition(0); seededZeroEndefector = !seededZeroEndefector; } - if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { - elevatorMotor.setPosition(0); + if (!seededZeroElevator && state.elevatorReverseLimit) { + io.endeffectorToPosition(0); seededZeroElevator = !seededZeroElevator; } @@ -345,7 +314,7 @@ public class Elevator extends SubsystemBase implements Queryable { periodicReady(); } - if(!intakeIR.get()){ + if(!state.intakeIR){ led.setMode(LEDConstants.DOWN_PATTERN); } @@ -355,6 +324,11 @@ public class Elevator extends SubsystemBase implements Queryable { // } } + @AutoLogOutput(key="Elevator/state") + public String getState() { + return currentState.toString(); + } + public boolean isL4Primed() { return currentState == CoordinationState.PrimedFour; } @@ -364,16 +338,24 @@ public class Elevator extends SubsystemBase implements Queryable { } public boolean hasCoral() { - return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); + return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch; + } + + public void elevatorStop() { + io.elevatorToVelocity(0); + } + + public void endeffectorStop() { + io.endeffectorToVelocity(0); } public boolean readyToMove() { - return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch; // return hasCoral(); } public void armShuffle(){ - if(!basinBeamBreak.get()){ + if(!state.basinBeamBreak){ //shuffle the coral with the arm until coral hits beam break } } diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..a48278c --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,34 @@ +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.subsystems.lidar.LidarIO.LidarState; + +public interface ElevatorIO { + @AutoLog + public class ElevatorState { + public double elevatorRefrence; + public double elevatorPosition; + public boolean elevatorForwardLimit; + public boolean elevatorReverseLimit; + + public double endeffectorRefrence; + public double endeffectorPosition; + public boolean endeffectorForwardLimit; + public boolean endeffectorReverseLimit; + + + public boolean basinBeamBreak; + public boolean endeffectorLimitSwitch; + public boolean intakeIR; + + } + + public default void elevatorToPosition(double position) {} + public default void endeffectorToPosition(double position) {} + public default void elevatorToVelocity(double velocity) {} + public default void endeffectorToVelocity(double velocity) {} + + public default void updateInputs(ElevatorState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java new file mode 100644 index 0000000..89d48aa --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.elevator; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.constants.Constants.ElevatorConstants; + +public class ElevatorReal implements ElevatorIO { + TalonFX elevatorMotor; + TalonFX endeffectorMotor; + + DigitalInput basinLimitSwitch; + DigitalInput endeffectorLimitSwitch; + DigitalInput intakeIR; + + + double elevatorRefrence = 0; + double endeffectorRefrence = 0; + + public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) { + this.elevatorMotor = elevatorTalonFX; + this.endeffectorMotor = endeffectorTalonFX; + + this.basinLimitSwitch = basinLimitSwitch; + this.endeffectorLimitSwitch = endeffectorLimitSwitch; + this.intakeIR = intakeIR; + + + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); + + elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); + endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); + } + + @Override + public void updateInputs(ElevatorState state) { + state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + state.elevatorRefrence = elevatorRefrence; + state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); + state.endeffectorRefrence = endeffectorRefrence; + state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; + state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; + + + state.basinBeamBreak = basinLimitSwitch.get(); + state.endeffectorLimitSwitch = endeffectorLimitSwitch.get(); + state.intakeIR = intakeIR.get(); + } + + @Override + public void elevatorToPosition(double position) { + elevatorRefrence = position; + var request = new PositionDutyCycle(position); + elevatorMotor.setControl(request); + } + + @Override + public void endeffectorToPosition(double position) { + endeffectorRefrence = position; + var request = new PositionDutyCycle(position); + endeffectorMotor.setControl(request); + } + + @Override + public void elevatorToVelocity(double velocity) { + elevatorMotor.set(velocity); + } + + + @Override + public void endeffectorToVelocity(double velocity) { + endeffectorMotor.set(velocity); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java similarity index 91% rename from src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java rename to src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java index 3851050..3bf33d5 100644 --- a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java @@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter; import frc4388.robot.constants.Constants.LiDARConstants; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class LidarLiteV2 implements LidarIO { +public class LidarReal implements LidarIO { private Counter LidarPWM; - public LidarLiteV2(int port) { + public LidarReal(int port) { LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 4a5f4cc..bbc61e4 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -4,16 +4,9 @@ package frc4388.robot.subsystems.swerve; -import java.util.Optional; - +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -56,7 +48,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double lastOdomSpeed; - public Pose2d initalPose2d = null; + public Pose2d initalPose2d = new Pose2d(); public double rotTarget = 0.0; @@ -353,9 +345,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SmartDashboard.putNumber("RotTartget", rotTarget); io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); vision.setLastOdomPose(state.currentPose); - setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -448,6 +441,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; } } + + @AutoLogOutput(key="SwerveDrive/speed ") + public double getOdometrySpeed() { + return lastOdomSpeed; + } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index 5debd5a..fa7de1a 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -16,7 +16,7 @@ public interface SwerveIO { public Pose2d currentPose = null; public Pose2d lastPose = null; public ChassisSpeeds speeds = null; - public double frequency = 1; + public double odometryRate = 1; } public default void setControl(SwerveRequest ctrl) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java similarity index 85% rename from src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index 84bbcd9..05d9e8f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -10,15 +10,13 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Pose2d; -import frc4388.robot.subsystems.lidar.LidarIO.LidarState; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; -public class SwervePhoenix implements SwerveIO { +public class SwerveReal implements SwerveIO { SwerveDrivetrain swerveDriveTrain; - public SwervePhoenix(SwerveDrivetrain swerveDriveTrain) { + public SwerveReal(SwerveDrivetrain swerveDriveTrain) { this.swerveDriveTrain = swerveDriveTrain; swerveDriveTrain.getOdometryFrequency(); } @@ -26,9 +24,9 @@ public class SwervePhoenix implements SwerveIO { @Override public void updateInputs(SwerveState state) { double time = Vision.getTime(); - state.frequency = swerveDriveTrain.getOdometryFrequency(); + state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency(); state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); - state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null); state.speeds = swerveDriveTrain.getState().Speeds; } diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java similarity index 97% rename from src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java rename to src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 888dc12..1c12437 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult; import frc4388.robot.constants.Constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; -public class VisionPhotonvision implements VisionIO { +public class VisionReal implements VisionIO { // private PhotonCamera[] cameras; // private PhotonPoseEstimator[] estimators; @@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO { // public List poses = new ArrayList<>(); - public VisionPhotonvision(PhotonCamera camera, Transform3d position){ + public VisionReal(PhotonCamera camera, Transform3d position){ this.camera = camera; estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);