From 68d53c460e46a6918c921d8aece9cb303cc876de Mon Sep 17 00:00:00 2001 From: CJ Date: Sat, 1 Feb 2025 14:18:08 -0500 Subject: [PATCH 01/14] what??? --- src/main/java/frc/robot/RobotContainer.java | 150 +++--------------- src/main/java/frc/robot/RobotState.java | 94 ----------- .../frc/robot/commands/DriveCommands.java | 47 +++++- 3 files changed, 65 insertions(+), 226 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotState.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98d732f..f44e8a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.Vision.VisionConstants.*; - +import java.util.List; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; @@ -194,6 +194,24 @@ public RobotContainer() DriverStation.silenceJoystickConnectionWarning(true); } + + private static Pose2d getNearestReefFace(Pose2d currentPose) + { + return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); + } + + public enum Side { + Left, + Right + } + + private static Pose2d getNearestReefBranch(Pose2d currentPose, Side side) + { + return FieldConstants.Reef.branchPositions + .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose))) + .get(FieldConstants.ReefHeight.L1).toPose2d(); + } + /** Use this method to define your joystick and button -> command mappings. */ private void configureControllerBindings() { @@ -216,131 +234,11 @@ private void configureControllerBindings() .onTrue( Commands.runOnce(setPose).ignoringDisable(true)); - // Driver Start Button: Run Homing Sequence - m_driver - .start() - .onTrue( - m_profiledElevator - .setStateCommand(Elevator.State.HOMING) - .until(m_profiledElevator.getHomedTrigger()) - .andThen(m_profiledElevator.zeroSensorCommand())); - m_profiledElevator.getHomedTrigger().onTrue(m_profiledElevator.homedAlertCommand()); - - // Driver Left Trigger: Bring Arm and Elevator to INTAKE position - m_driver - .leftTrigger() - .onTrue( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.INTAKE), - m_profiledElevator.setStateCommand(Elevator.State.INTAKE))); - - // Driver A Button: Send Arm and Elevator to LEVEL_1 - m_driver - .a() - .onTrue( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.LEVEL_1), - Commands.waitUntil(() -> m_profiledArm.atPosition(0)) - .andThen(m_profiledElevator.setStateCommand(Elevator.State.LEVEL_1)))); - - // Driver X Button: Send Arm and Elevator to LEVEL_2 - m_driver - .x() - .onTrue( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.LEVEL_2), - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_2))); - - // Driver B Button: Send Arm and Elevator to LEVEL_3 - m_driver - .b() - .onTrue( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.LEVEL_3), - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_3))); - - // Driver Y Button: Send Arm and Elevator to LEVEL_4 - m_driver - .y() - .onTrue( - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_4), - Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) - .andThen(m_profiledArm.setStateCommand(Arm.State.LEVEL_4)))); - - - // Driver uses Left Bumper for orbitting and targeting maneuvers - m_driver - .leftBumper() - .whileTrue( - DriveCommands.joystickOrbitAtAngle( - m_drive, - () -> m_driver.getLeftY(), - () -> m_driver.getLeftX(), - () -> RobotState.getInstance() - .getAngleToTarget(m_drive.getPose().getTranslation()))); - - m_driver - .leftBumper() - .and(m_driver.axisGreaterThan(XboxController.Axis.kRightX.value, 0.8)) - .onTrue( - Commands.runOnce( - () -> RobotState.getInstance() - .setTarget(RobotState.TARGET.RIGHT_CORAL_STATION))); - - m_driver - .leftBumper() - .and(m_driver.axisLessThan(XboxController.Axis.kRightX.value, -0.8)) - .onTrue( - Commands.runOnce( - () -> RobotState.getInstance() - .setTarget(RobotState.TARGET.LEFT_CORAL_STATION))); - - m_driver - .leftBumper() - .and(m_driver.axisLessThan(XboxController.Axis.kRightY.value, -0.8)) - .onTrue( - Commands.runOnce(() -> RobotState.getInstance().setTarget(RobotState.TARGET.REEF))); - - m_driver - .povDown() - .whileTrue( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.GROUND), - m_profiledElevator.setStateCommand(Elevator.State.HOME))); - - m_driver - .povLeft() - .whileTrue( - Commands.runOnce( - () -> SimulatedArena.getInstance() - .addGamePiece(new ReefscapeAlgaeOnField(new Translation2d(2, 2))))); - - - m_driver - .rightBumper() - .onTrue( - Commands.runOnce( - () -> SimulatedArena.getInstance() - .addGamePieceProjectile( - new ReefscapeCoralOnFly( - // Obtain robot position from drive simulation - m_driveSimulation.getSimulatedDriveTrainPose().getTranslation(), - // The scoring mechanism is installed at (0.46, 0) (meters) on the - // robot - new Translation2d(0.48, 0), - // Obtain robot speed from drive simulation - m_driveSimulation - .getDriveTrainSimulatedChassisSpeedsFieldRelative(), - // Obtain robot facing from drive simulation - m_driveSimulation.getSimulatedDriveTrainPose().getRotation(), - // The height at which the coral is ejected - Meters.of(2.3), - // The initial speed of the coral - MetersPerSecond.of(1), - // The coral is ejected vertically downwards - Degrees.of(-75))))); - + m_driver.leftBumper().whileTrue( + DriveCommands.joystickApproach( + m_drive, + () -> -m_driver.getLeftY(), + () -> FieldConstants.Reef.centerFaces[3])); } /** diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index efe6254..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import lombok.Getter; -import lombok.RequiredArgsConstructor; -import lombok.Setter; - -/** Add your docs here. */ -public class RobotState { - private static RobotState instance; - - @Getter - @Setter - private Pose2d robotPose = new Pose2d(); - - @Getter - @Setter - private ChassisSpeeds robotSpeeds = new ChassisSpeeds(); - - @RequiredArgsConstructor - @Getter - public enum TARGET { - LEFT_CORAL_STATION(FieldConstants.CoralStation.leftCenterFace.getTranslation()), - RIGHT_CORAL_STATION(FieldConstants.CoralStation.rightCenterFace.getTranslation()), - REEF(FieldConstants.Reef.center); - - private final Translation2d targetTranslation; - } - - @Getter - @Setter - private TARGET target = TARGET.LEFT_CORAL_STATION; - - private double deltaT = .15; - - public static RobotState getInstance() - { - if (instance == null) - instance = new RobotState(); - return instance; - } - - public Rotation2d getAngleToTarget(Translation2d currentTranslation) - { - return target.targetTranslation.minus(currentTranslation).getAngle(); - } - - private Translation2d getFuturePose() - { - // If magnitude of velocity is low enough, use current pose - if (Math.hypot(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond) < .25) { - return robotPose.getTranslation(); - } else { - // Add translation based on current speed and time in the future deltaT - return robotPose - .getTranslation() - .plus( - new Translation2d( - deltaT * robotSpeeds.vxMetersPerSecond, - deltaT * robotSpeeds.vyMetersPerSecond)); - } - } - - private static final InterpolatingDoubleTreeMap speakerArmAngleMap = - new InterpolatingDoubleTreeMap(); - - static { - speakerArmAngleMap.put(1.5, 12.71); - speakerArmAngleMap.put(2.0, 21.00); - speakerArmAngleMap.put(2.5, 24.89); - speakerArmAngleMap.put(3.0, 29.00); - speakerArmAngleMap.put(3.5, 31.20); - speakerArmAngleMap.put(4.0, 32.50); - speakerArmAngleMap.put(4.5, 34.00); - speakerArmAngleMap.put(5.0, 35.00); - } - - private static final InterpolatingDoubleTreeMap feedArmAngleMap = - new InterpolatingDoubleTreeMap(); - - static { - feedArmAngleMap.put(5.0, 0.0); - feedArmAngleMap.put(6.0, -10.0); - feedArmAngleMap.put(7.0, -19.0); - } -} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 25546f9..1e81ea5 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -23,11 +23,14 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.FieldConstants; import frc.robot.subsystems.drive.Drive; import java.text.DecimalFormat; import java.text.NumberFormat; @@ -35,6 +38,7 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -162,11 +166,10 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - public static Command joystickOrbitAtAngle( + public static Command joystickApproach( Drive drive, - DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) + Supplier approachSupplier) { // Create PID controller @@ -178,18 +181,50 @@ public static Command joystickOrbitAtAngle( new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); angleController.enableContinuousInput(-Math.PI, Math.PI); + ProfiledPIDController alignController = + new ProfiledPIDController( + 100, + 0.0, + 0, + new TrapezoidProfile.Constraints(20, 8)); + // Construct command return Commands.run( () -> { + + Translation2d currentTranslation = drive.getPose().getTranslation(); + + Rotation2d angleToTarget = + currentTranslation.minus(approachSupplier.get().getTranslation()).getAngle(); + Rotation2d theta = Rotation2d.kCCW_90deg.minus(angleToTarget); + + double distanceFromGoal = + currentTranslation.getDistance(approachSupplier.get().getTranslation()) + * theta.getCos(); + + Translation2d offsetVector = new Translation2d( + alignController.calculate(distanceFromGoal * -1, 0), 0) + .rotateBy(approachSupplier.get().getRotation()) + .rotateBy(Rotation2d.kCCW_90deg); + // Get linear velocity Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()) - .rotateBy(rotationSupplier.get()); + getLinearVelocityFromJoysticks(0, + ySupplier.getAsDouble()).rotateBy( + approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) + .plus(offsetVector); + + SmartDashboard.putData(alignController); + Logger.recordOutput("test/linearVelocity", linearVelocity); + Logger.recordOutput("test/distanceFromGoal", distanceFromGoal); + Logger.recordOutput("test/PIDOutput", offsetVector.getY()); + Logger.recordOutput("test/goal", approachSupplier.get()); // Calculate angular speed double omega = angleController.calculate( - drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + drive.getRotation().getRadians(), approachSupplier.get().getRotation() + .rotateBy(Rotation2d.k180deg).getRadians()); // Convert to field relative speeds & send command ChassisSpeeds speeds = From e27ffa587ec54efd679941800e50e4c94da12c3e Mon Sep 17 00:00:00 2001 From: spybh66 Date: Sun, 2 Feb 2025 11:46:13 -0500 Subject: [PATCH 02/14] init --- .../robot/util/drivers/LaserCANSensor.java | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/main/java/frc/robot/util/drivers/LaserCANSensor.java diff --git a/src/main/java/frc/robot/util/drivers/LaserCANSensor.java b/src/main/java/frc/robot/util/drivers/LaserCANSensor.java new file mode 100644 index 0000000..291cbff --- /dev/null +++ b/src/main/java/frc/robot/util/drivers/LaserCANSensor.java @@ -0,0 +1,40 @@ +// 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.util.drivers; + +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; + +/** Add your docs here. */ +public class LaserCANSensor { + private LaserCan lc; + private double closeCut = 200; + + public LaserCANSensor(int ID, double closeCut) + { + lc = new LaserCan(ID); + this.closeCut = closeCut; + try { + lc.setRangingMode(LaserCan.RangingMode.SHORT); + lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 6, 6)); + lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } + } + + public boolean isClose() + { + Measurement measurement = lc.getMeasurement(); + if (measurement != null + && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + return (measurement.distance_mm < closeCut); + } else { + return false; + } + + } +} From 48e08edc1aa9f46a80f4d00e350980cdc68022aa Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 17:48:22 -0500 Subject: [PATCH 03/14] joystickApproach working !!! --- .vscode/settings.json | 3 +- src/main/java/frc/robot/Robot.java | 3 ++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/DriveCommands.java | 35 +++++++++++-------- 4 files changed, 26 insertions(+), 17 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index bd19617..df8b92e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -65,5 +65,6 @@ "[java]": { "editor.detectIndentation": false }, - "workbench.iconTheme": "vs-seti" + "workbench.iconTheme": "vs-seti", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0708aee..4fcb11e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -159,6 +159,9 @@ public void teleopInit() if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + Logger.recordOutput("test/faces", FieldConstants.Reef.centerFaces); + } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f44e8a1..a4c346b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -238,7 +238,7 @@ private void configureControllerBindings() DriveCommands.joystickApproach( m_drive, () -> -m_driver.getLeftY(), - () -> FieldConstants.Reef.centerFaces[3])); + () -> getNearestReefFace(m_drive.getPose()))); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1e81ea5..a3a33fb 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -183,29 +183,34 @@ public static Command joystickApproach( ProfiledPIDController alignController = new ProfiledPIDController( - 100, + 1, 0.0, 0, new TrapezoidProfile.Constraints(20, 8)); + alignController.setGoal(0); // Construct command return Commands.run( () -> { Translation2d currentTranslation = drive.getPose().getTranslation(); + Translation2d targetTranslation = approachSupplier.get().getTranslation(); + double distanceToTarget = currentTranslation.getDistance(targetTranslation); - Rotation2d angleToTarget = - currentTranslation.minus(approachSupplier.get().getTranslation()).getAngle(); - Rotation2d theta = Rotation2d.kCCW_90deg.minus(angleToTarget); + Rotation2d perpendicularLine = approachSupplier.get().getRotation(); - double distanceFromGoal = - currentTranslation.getDistance(approachSupplier.get().getTranslation()) - * theta.getCos(); + Translation2d goalTranslation = new Translation2d( + perpendicularLine.getCos() * distanceToTarget + targetTranslation.getX(), + perpendicularLine.getSin() * distanceToTarget + targetTranslation.getY()); - Translation2d offsetVector = new Translation2d( - alignController.calculate(distanceFromGoal * -1, 0), 0) - .rotateBy(approachSupplier.get().getRotation()) - .rotateBy(Rotation2d.kCCW_90deg); + Translation2d distanceToGoal = currentTranslation.minus(goalTranslation); + double distanceToGoalValue = + Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()) + * (distanceToGoal.getX() < 0 ? -1 : 1); + + Translation2d offsetVector = + new Translation2d(alignController.calculate(distanceToGoalValue), 0) + .rotateBy(perpendicularLine.plus(Rotation2d.kCCW_90deg)); // Get linear velocity Translation2d linearVelocity = @@ -215,10 +220,10 @@ public static Command joystickApproach( .plus(offsetVector); SmartDashboard.putData(alignController); - Logger.recordOutput("test/linearVelocity", linearVelocity); - Logger.recordOutput("test/distanceFromGoal", distanceFromGoal); - Logger.recordOutput("test/PIDOutput", offsetVector.getY()); - Logger.recordOutput("test/goal", approachSupplier.get()); + Logger.recordOutput("test/face", approachSupplier.get()); + Logger.recordOutput("test/zero", Pose2d.kZero); + Logger.recordOutput("test/goalTranslation", goalTranslation); + Logger.recordOutput("test/offsetVector", offsetVector); // Calculate angular speed double omega = From ff059d3f67b84d8871f0ff1dca1f617a1f78ced6 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 18:06:46 -0500 Subject: [PATCH 04/14] fix sign error and add logging --- src/main/java/frc/robot/Robot.java | 10 +++++++++- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4fcb11e..ce8e337 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,10 +8,13 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.generated.TunerConstants; +import java.util.ArrayList; +import java.util.List; import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -161,7 +164,12 @@ public void teleopInit() } Logger.recordOutput("test/faces", FieldConstants.Reef.centerFaces); - + List branchPositions = new ArrayList<>(); + for (int i = 0; i < FieldConstants.Reef.branchPositions.size(); i++) { + branchPositions.add(FieldConstants.Reef.branchPositions.get(i) + .get(FieldConstants.ReefHeight.L1).toPose2d()); + } + Logger.recordOutput("test/branches", branchPositions.toArray(new Pose2d[0])); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a3a33fb..a8cd393 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -206,7 +206,7 @@ public static Command joystickApproach( Translation2d distanceToGoal = currentTranslation.minus(goalTranslation); double distanceToGoalValue = Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()) - * (distanceToGoal.getX() < 0 ? -1 : 1); + * (distanceToGoal.getX() * distanceToGoal.getY() > 0 ? -1 : 1); Translation2d offsetVector = new Translation2d(alignController.calculate(distanceToGoalValue), 0) From 7a5fe0da59ae9337fb74b7a1c4fae4bb99c04c49 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 18:39:34 -0500 Subject: [PATCH 05/14] Branch Alignment Done! --- src/main/java/frc/robot/RobotContainer.java | 21 +++++++++++++++++-- .../frc/robot/commands/DriveCommands.java | 5 ++--- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a4c346b..c0379b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,6 +59,8 @@ */ public class RobotContainer { + private final int RIGHT_STICK_X = 4; + // Controllers private final CommandXboxController m_driver = new CommandXboxController(0); // private final CommandXboxController m_operator = new CommandXboxController(1); @@ -208,7 +210,8 @@ public enum Side { private static Pose2d getNearestReefBranch(Pose2d currentPose, Side side) { return FieldConstants.Reef.branchPositions - .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose))) + .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) + * 2 + (side == Side.Left ? 1 : 0)) .get(FieldConstants.ReefHeight.L1).toPose2d(); } @@ -235,10 +238,24 @@ private void configureControllerBindings() Commands.runOnce(setPose).ignoringDisable(true)); m_driver.leftBumper().whileTrue( + DriveCommands.joystickDriveAtAngle( + m_drive, + () -> -m_driver.getLeftY(), + () -> -m_driver.getLeftX(), + () -> getNearestReefFace(m_drive.getPose()).getRotation() + .rotateBy(Rotation2d.k180deg))); + + m_driver.leftBumper().and(m_driver.axisGreaterThan(RIGHT_STICK_X, 0.8)).whileTrue( + DriveCommands.joystickApproach( + m_drive, + () -> -m_driver.getLeftY(), + () -> getNearestReefBranch(m_drive.getPose(), Side.Right))); + + m_driver.leftBumper().and(m_driver.axisLessThan(RIGHT_STICK_X, -0.8)).whileTrue( DriveCommands.joystickApproach( m_drive, () -> -m_driver.getLeftY(), - () -> getNearestReefFace(m_drive.getPose()))); + () -> getNearestReefBranch(m_drive.getPose(), Side.Left))); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a8cd393..56c18aa 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -205,12 +205,11 @@ public static Command joystickApproach( Translation2d distanceToGoal = currentTranslation.minus(goalTranslation); double distanceToGoalValue = - Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()) - * (distanceToGoal.getX() * distanceToGoal.getY() > 0 ? -1 : 1); + Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()); Translation2d offsetVector = new Translation2d(alignController.calculate(distanceToGoalValue), 0) - .rotateBy(perpendicularLine.plus(Rotation2d.kCCW_90deg)); + .rotateBy(distanceToGoal.getAngle()); // Get linear velocity Translation2d linearVelocity = From 0366261f1170aeff3a793fbbf1797e484f40f3da Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 19:21:35 -0500 Subject: [PATCH 06/14] Change controls and remove test logging --- src/main/java/frc/robot/Robot.java | 8 ----- src/main/java/frc/robot/RobotContainer.java | 34 +++++++++++++++++++ .../frc/robot/commands/DriveCommands.java | 6 ---- 3 files changed, 34 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce8e337..190f539 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -162,14 +162,6 @@ public void teleopInit() if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - - Logger.recordOutput("test/faces", FieldConstants.Reef.centerFaces); - List branchPositions = new ArrayList<>(); - for (int i = 0; i < FieldConstants.Reef.branchPositions.size(); i++) { - branchPositions.add(FieldConstants.Reef.branchPositions.get(i) - .get(FieldConstants.ReefHeight.L1).toPose2d()); - } - Logger.recordOutput("test/branches", branchPositions.toArray(new Pose2d[0])); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c0379b2..e3286ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -256,6 +256,40 @@ private void configureControllerBindings() m_drive, () -> -m_driver.getLeftY(), () -> getNearestReefBranch(m_drive.getPose(), Side.Left))); + + // Driver A Button: Send Arm and Elevator to LEVEL_1 + m_driver + .a() + .onTrue( + Commands.parallel( + m_profiledArm.setStateCommand(Arm.State.LEVEL_1), + Commands.waitUntil(() -> m_profiledArm.atPosition(0)) + .andThen(m_profiledElevator.setStateCommand(Elevator.State.LEVEL_1)))); + + // Driver X Button: Send Arm and Elevator to LEVEL_2 + m_driver + .x() + .onTrue( + Commands.parallel( + m_profiledArm.setStateCommand(Arm.State.LEVEL_2), + m_profiledElevator.setStateCommand(Elevator.State.LEVEL_2))); + + // Driver B Button: Send Arm and Elevator to LEVEL_3 + m_driver + .b() + .onTrue( + Commands.parallel( + m_profiledArm.setStateCommand(Arm.State.LEVEL_3), + m_profiledElevator.setStateCommand(Elevator.State.LEVEL_3))); + + // Driver Y Button: Send Arm and Elevator to LEVEL_4 + m_driver + .y() + .onTrue( + Commands.parallel( + m_profiledElevator.setStateCommand(Elevator.State.LEVEL_4), + Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) + .andThen(m_profiledArm.setStateCommand(Arm.State.LEVEL_4)))); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 56c18aa..82ca7cb 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -218,12 +218,6 @@ public static Command joystickApproach( approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) .plus(offsetVector); - SmartDashboard.putData(alignController); - Logger.recordOutput("test/face", approachSupplier.get()); - Logger.recordOutput("test/zero", Pose2d.kZero); - Logger.recordOutput("test/goalTranslation", goalTranslation); - Logger.recordOutput("test/offsetVector", offsetVector); - // Calculate angular speed double omega = angleController.calculate( From 5b87cc8c544fe02c74069bf806cadd9e88d16538 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 22:04:08 -0500 Subject: [PATCH 07/14] add tunable pid --- src/main/java/frc/robot/commands/DriveCommands.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 82ca7cb..7be6b6e 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -32,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.util.TuneableProfiledPID; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -181,12 +182,14 @@ public static Command joystickApproach( new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); angleController.enableContinuousInput(-Math.PI, Math.PI); - ProfiledPIDController alignController = - new ProfiledPIDController( + TuneableProfiledPID alignController = + new TuneableProfiledPID( + "alignController", 1, 0.0, 0, - new TrapezoidProfile.Constraints(20, 8)); + 20, + 8); alignController.setGoal(0); // Construct command @@ -218,6 +221,8 @@ public static Command joystickApproach( approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) .plus(offsetVector); + SmartDashboard.putData(alignController); + // Calculate angular speed double omega = angleController.calculate( From f84255c674add89f07126d44207ee4331958f2b5 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 17:44:07 -0500 Subject: [PATCH 08/14] remove unnecessary imports and document --- src/main/java/frc/robot/Robot.java | 3 --- src/main/java/frc/robot/RobotContainer.java | 4 ++- .../frc/robot/commands/DriveCommands.java | 25 +++++++++++-------- .../robot/subsystems/drive/ModuleIOSim.java | 4 +-- 4 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 190f539..0708aee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,13 +8,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.generated.TunerConstants; -import java.util.ArrayList; -import java.util.List; import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfcd58a..df47042 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -240,6 +239,7 @@ private void configureControllerBindings() .onTrue( Commands.runOnce(setPose).ignoringDisable(true)); + // Driver Left Button: Face Nearest Reef Face m_driver.leftBumper().whileTrue( DriveCommands.joystickDriveAtAngle( m_drive, @@ -248,12 +248,14 @@ private void configureControllerBindings() () -> getNearestReefFace(m_drive.getPose()).getRotation() .rotateBy(Rotation2d.k180deg))); + // Driver Left Button + Right Stick Right: Approach Nearest Right-Side Reef Branch m_driver.leftBumper().and(m_driver.axisGreaterThan(RIGHT_STICK_X, 0.8)).whileTrue( DriveCommands.joystickApproach( m_drive, () -> -m_driver.getLeftY(), () -> getNearestReefBranch(m_drive.getPose(), Side.Right))); + // Driver Left Button + Right Stick Left: Approach Nearest Left-Side Reef Branch m_driver.leftBumper().and(m_driver.axisLessThan(RIGHT_STICK_X, -0.8)).whileTrue( DriveCommands.joystickApproach( m_drive, diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7be6b6e..0f9d071 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -23,14 +23,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.FieldConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.util.TuneableProfiledPID; import java.text.DecimalFormat; @@ -39,7 +37,6 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -167,6 +164,12 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Robot relative drive command using joystick for linear control towards the approach target, + * PID for aligning with the target laterally, and PID for angular control. Used for approaching + * a known target, usually from a short distance. The approachSupplier must supply a Pose2d with + * a rotation facing away from the target + */ public static Command joystickApproach( Drive drive, DoubleSupplier ySupplier, @@ -195,33 +198,35 @@ public static Command joystickApproach( // Construct command return Commands.run( () -> { - + // Name constants Translation2d currentTranslation = drive.getPose().getTranslation(); - Translation2d targetTranslation = approachSupplier.get().getTranslation(); - double distanceToTarget = currentTranslation.getDistance(targetTranslation); + Translation2d approachTranslation = approachSupplier.get().getTranslation(); + double distanceToApproach = currentTranslation.getDistance(approachTranslation); Rotation2d perpendicularLine = approachSupplier.get().getRotation(); + // Find lateral distance from goal Translation2d goalTranslation = new Translation2d( - perpendicularLine.getCos() * distanceToTarget + targetTranslation.getX(), - perpendicularLine.getSin() * distanceToTarget + targetTranslation.getY()); + perpendicularLine.getCos() * distanceToApproach + approachTranslation.getX(), + perpendicularLine.getSin() * distanceToApproach + approachTranslation.getY()); Translation2d distanceToGoal = currentTranslation.minus(goalTranslation); double distanceToGoalValue = Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()); + // Calculate lateral linear velocity Translation2d offsetVector = new Translation2d(alignController.calculate(distanceToGoalValue), 0) .rotateBy(distanceToGoal.getAngle()); - // Get linear velocity + // Calculate total linear velocity Translation2d linearVelocity = getLinearVelocityFromJoysticks(0, ySupplier.getAsDouble()).rotateBy( approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) .plus(offsetVector); - SmartDashboard.putData(alignController); + SmartDashboard.putData(alignController); // TODO: Calibrate PID // Calculate angular speed double omega = diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index ec76764..f7cce31 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -34,13 +34,13 @@ public class ModuleIOSim implements ModuleIO { // TunerConstants doesn't support separate sim constants, so they are declared // locally - private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KP = 100.0; private static final double DRIVE_KD = 0.0; private static final double DRIVE_KS = 0.0; private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); - private static final double TURN_KP = 8.0; + private static final double TURN_KP = 100.0; private static final double TURN_KD = 0.0; // private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); // private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); From d7145d106c8d5b1cb2a618e0c5e33f93c56353ec Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 18:04:19 -0500 Subject: [PATCH 09/14] Remove SampleRollers --- .../SampleRollers/SampleRollers.java | 48 ------------------- .../SampleRollers/SampleRollersConstants.java | 38 --------------- .../SampleRollers/SampleRollersIO.java | 6 --- .../SampleRollers/SampleRollersIOSim.java | 11 ----- .../SampleRollers/SampleRollersIOTalonFX.java | 12 ----- 5 files changed, 115 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/SampleRollers/SampleRollers.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIO.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollers.java b/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollers.java deleted file mode 100644 index 89ad466..0000000 --- a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollers.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.SampleRollers; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystem; -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystem.VoltageState; -import lombok.Getter; -import lombok.RequiredArgsConstructor; -import lombok.Setter; - -@Setter -@Getter -public class SampleRollers extends GenericRollerSubsystem { - - // This is the number of independent roller motors in the subsystem - // It must equal the size of the voltage arrays in the State enum - // It also must equal the length of the kMotorIds List in the supplied Constants file - public static int numRollers = 2; - - @RequiredArgsConstructor - @Getter - public enum State implements VoltageState { - OFF(new double[] {0.0, 0.0}), - INTAKE(new double[] {10.0, -5.0}), - EJECT(new double[] {-8.0, 4.0}); - - private final double[] output; - - public double getOutput(int index) - { - return this.output[index]; - } - } - - @Getter - @Setter - private State state = State.OFF; - - /** Constructor */ - public SampleRollers(SampleRollersIO io) - { - super("SampleRollers", numRollers, io); - } - - public Command setStateCommand(State state) - { - return startEnd(() -> this.state = state, () -> this.state = State.OFF); - } -} diff --git a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersConstants.java b/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersConstants.java deleted file mode 100644 index d152d65..0000000 --- a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersConstants.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.subsystems.SampleRollers; - -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.system.plant.DCMotor; -import frc.robot.Ports; -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystemConstants; -import java.util.List; - -/** Add your docs here. */ -public final class SampleRollersConstants { - - public static final GenericRollerSubsystemConstants kSubSysConstants = - new GenericRollerSubsystemConstants(); - - static { - kSubSysConstants.kName = "SampleRollers"; - - // kMotorConstants.kMotorIDs = List.of(Ports.SAMPLE_ROLLER); - kSubSysConstants.kMotorIDs = List.of(Ports.TWO_ROLLER_1, Ports.TWO_ROLLER_2); - - kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - kSubSysConstants.kMotorConfig.MotorOutput.Inverted = - InvertedValue.CounterClockwise_Positive; - kSubSysConstants.kMotorConfig.Voltage.PeakForwardVoltage = 12.0; - kSubSysConstants.kMotorConfig.Voltage.PeakReverseVoltage = -12.0; - - kSubSysConstants.kMotorConfig.CurrentLimits.SupplyCurrentLimit = 20; - kSubSysConstants.kMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimit = 70; - kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; - - // Motor simulation - kSubSysConstants.simMotorModelSupplier = () -> DCMotor.getKrakenX60Foc(1); - kSubSysConstants.simReduction = (18.0 / 12.0); - kSubSysConstants.simMOI = 0.001; - } -} diff --git a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIO.java b/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIO.java deleted file mode 100644 index 817f4e4..0000000 --- a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIO.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.subsystems.SampleRollers; - -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystemIO; - -public interface SampleRollersIO extends GenericRollerSubsystemIO { -} diff --git a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOSim.java b/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOSim.java deleted file mode 100644 index 5b1cda3..0000000 --- a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOSim.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.SampleRollers; - -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystemIOImpl; - -public class SampleRollersIOSim extends GenericRollerSubsystemIOImpl implements SampleRollersIO { - - public SampleRollersIOSim() - { - super(SampleRollersConstants.kSubSysConstants, true); - } -} diff --git a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOTalonFX.java b/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOTalonFX.java deleted file mode 100644 index b8c26d4..0000000 --- a/src/main/java/frc/robot/subsystems/SampleRollers/SampleRollersIOTalonFX.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems.SampleRollers; - -import frc.robot.subsystems.GenericRollerSubsystem.GenericRollerSubsystemIOImpl; - -public class SampleRollersIOTalonFX extends GenericRollerSubsystemIOImpl - implements SampleRollersIO { - - public SampleRollersIOTalonFX() - { - super(SampleRollersConstants.kSubSysConstants, false); - } -} From e648f3cfba54a9474029d21060f9799208026d68 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 18:37:17 -0500 Subject: [PATCH 10/14] Basic rollers for the claw --- src/main/java/frc/robot/Ports.java | 8 +------ .../ClawRoller.java} | 14 +++++------ .../ClawRollerConstants.java} | 24 ++++--------------- .../subsystems/ClawRoller/ClawRollerIO.java | 6 +++++ .../ClawRoller/ClawRollerIOSim.java | 12 ++++++++++ .../ClawRoller/ClawRollerIOTalonFX.java | 12 ++++++++++ .../SampleProfiledRollerIO.java | 6 ----- .../SampleProfiledRollerIOSim.java | 12 ---------- .../SampleProfiledRollerIOTalonFX.java | 12 ---------- 9 files changed, 42 insertions(+), 64 deletions(-) rename src/main/java/frc/robot/subsystems/{SampleProfiledRoller/SampleProfiledRoller.java => ClawRoller/ClawRoller.java} (69%) rename src/main/java/frc/robot/subsystems/{SampleProfiledRoller/SampleProfiledRollerConstants.java => ClawRoller/ClawRollerConstants.java} (76%) create mode 100644 src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIO.java create mode 100644 src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOTalonFX.java diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index 991a94e..4803042 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -17,10 +17,7 @@ public class Ports { // "canivore1"); /* SUBSYSTEM CAN DEVICE IDS */ - public static final CanDeviceId SAMPLE_ROLLER = new CanDeviceId(15, "rio"); - - public static final CanDeviceId TWO_ROLLER_1 = new CanDeviceId(16, "rio"); - public static final CanDeviceId TWO_ROLLER_2 = new CanDeviceId(17, "rio"); + public static final CanDeviceId CLAW_ROLLER = new CanDeviceId(16, "rio"); public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(18, "rio"); public static final CanDeviceId ELEVATOR_FOLLOWER = new CanDeviceId(19, "rio"); @@ -30,9 +27,6 @@ public class Ports { public static final CanDeviceId ARM_FOLLOWER = new CanDeviceId(22, "rio"); public static final CanDeviceId ARM_CANCODER = new CanDeviceId(23, "rio"); - public static final CanDeviceId PROFROLLER_MAIN = new CanDeviceId(24, "rio"); - public static final CanDeviceId PROFROLLER_FOLLOWER = new CanDeviceId(25, "rio"); - public static final CanDeviceId CLIMBER = new CanDeviceId(26, "rio"); /* diff --git a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRoller.java b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRoller.java similarity index 69% rename from src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRoller.java rename to src/main/java/frc/robot/subsystems/ClawRoller/ClawRoller.java index 473db08..7878c46 100644 --- a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRoller.java +++ b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRoller.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.SampleProfiledRoller; +package frc.robot.subsystems.ClawRoller; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem; @@ -9,13 +9,13 @@ @Setter @Getter -public class SampleProfiledRoller - extends GenericMotionProfiledSubsystem { +public class ClawRoller + extends GenericMotionProfiledSubsystem { @RequiredArgsConstructor @Getter public enum State implements TargetState { - OFF(0.0, 0.0, ProfileType.OPEN_VOLTAGE), + OFF(0.0, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot INTAKE(6.0, 0.0, ProfileType.OPEN_VOLTAGE), EJECT(-6.0, 0.0, ProfileType.OPEN_VOLTAGE), POSITION(20.0, 0.0, ProfileType.MM_POSITION); @@ -29,12 +29,10 @@ public enum State implements TargetState { @Setter private State state = State.OFF; - private final boolean debug = true; - /** Constructor */ - public SampleProfiledRoller(SampleProfiledRollerIO io, boolean isSim) + public ClawRoller(ClawRollerIO io, boolean isSim) { - super(ProfileType.OPEN_VOLTAGE, SampleProfiledRollerConstants.kSubSysConstants, io, isSim); + super(ProfileType.OPEN_VOLTAGE, ClawRollerConstants.kSubSysConstants, io, isSim); } public Command setStateCommand(State state) diff --git a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerConstants.java b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerConstants.java similarity index 76% rename from src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerConstants.java rename to src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerConstants.java index 315bdc4..8610d67 100644 --- a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerConstants.java +++ b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerConstants.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.SampleProfiledRoller; +package frc.robot.subsystems.ClawRoller; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; @@ -9,15 +9,15 @@ import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemConstants.simType; /** Add your docs here. */ -public final class SampleProfiledRollerConstants { +public final class ClawRollerConstants { public static final GenericMotionProfiledSubsystemConstants kSubSysConstants = new GenericMotionProfiledSubsystemConstants(); static { - kSubSysConstants.kName = "SampleProfiledRoller"; + kSubSysConstants.kName = "ClawRoller"; - kSubSysConstants.kLeaderMotor = Ports.PROFROLLER_MAIN; + kSubSysConstants.kLeaderMotor = Ports.CLAW_ROLLER; kSubSysConstants.kFollowMotor = null; kSubSysConstants.kFollowerOpposesMain = true; @@ -29,20 +29,6 @@ public final class SampleProfiledRollerConstants { kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 1.0; kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 1.0; - // Using a remote CANcoder - /* - * kSubSysConstants.kCANcoder = Ports.ARM_CANCODER; - * kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = - * FeedbackSensorSourceValue.FusedCANcoder; - * kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 7.04; - * kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 54.4/7.04; - * kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.3467; - * kSubSysConstants.kEncoderConfig.MagnetSensor.SensorDirection = - * SensorDirectionValue.Clockwise_Positive; - * kSubSysConstants.kEncoderConfig.MagnetSensor.AbsoluteSensorRange = - * AbsoluteSensorRangeValue.Unsigned_0To1; - */ - kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; kSubSysConstants.kMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; @@ -55,7 +41,7 @@ public final class SampleProfiledRollerConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 0; + kSubSysConstants.kMotorConfig.Slot0.kP = 0; // TODO: tune on real robot kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.kG = 0; diff --git a/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIO.java b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIO.java new file mode 100644 index 0000000..b0304e8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIO.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.ClawRoller; + +import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIO; + +public interface ClawRollerIO extends GenericMotionProfiledSubsystemIO { +} diff --git a/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOSim.java b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOSim.java new file mode 100644 index 0000000..b059920 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOSim.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.ClawRoller; + +import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIOImpl; + +public class ClawRollerIOSim extends GenericMotionProfiledSubsystemIOImpl + implements ClawRollerIO { + + public ClawRollerIOSim() + { + super(ClawRollerConstants.kSubSysConstants, true); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOTalonFX.java new file mode 100644 index 0000000..0d39182 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClawRoller/ClawRollerIOTalonFX.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.ClawRoller; + +import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIOImpl; + +public class ClawRollerIOTalonFX extends GenericMotionProfiledSubsystemIOImpl + implements ClawRollerIO { + + public ClawRollerIOTalonFX() + { + super(ClawRollerConstants.kSubSysConstants, false); + } +} diff --git a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIO.java b/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIO.java deleted file mode 100644 index 6e88bb9..0000000 --- a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIO.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.subsystems.SampleProfiledRoller; - -import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIO; - -public interface SampleProfiledRollerIO extends GenericMotionProfiledSubsystemIO { -} diff --git a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOSim.java b/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOSim.java deleted file mode 100644 index 80e7bb4..0000000 --- a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOSim.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems.SampleProfiledRoller; - -import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIOImpl; - -public class SampleProfiledRollerIOSim extends GenericMotionProfiledSubsystemIOImpl - implements SampleProfiledRollerIO { - - public SampleProfiledRollerIOSim() - { - super(SampleProfiledRollerConstants.kSubSysConstants, true); - } -} diff --git a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOTalonFX.java deleted file mode 100644 index 0ebeade..0000000 --- a/src/main/java/frc/robot/subsystems/SampleProfiledRoller/SampleProfiledRollerIOTalonFX.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems.SampleProfiledRoller; - -import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemIOImpl; - -public class SampleProfiledRollerIOTalonFX extends GenericMotionProfiledSubsystemIOImpl - implements SampleProfiledRollerIO { - - public SampleProfiledRollerIOTalonFX() - { - super(SampleProfiledRollerConstants.kSubSysConstants, false); - } -} From 0d90c96c723d35f17d6d3c19438c78000e6f37ca Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 19:17:54 -0500 Subject: [PATCH 11/14] rename variables --- src/main/java/frc/robot/commands/DriveCommands.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 0f9d071..6970957 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -210,14 +210,14 @@ public static Command joystickApproach( perpendicularLine.getCos() * distanceToApproach + approachTranslation.getX(), perpendicularLine.getSin() * distanceToApproach + approachTranslation.getY()); - Translation2d distanceToGoal = currentTranslation.minus(goalTranslation); - double distanceToGoalValue = - Math.hypot(distanceToGoal.getX(), distanceToGoal.getY()); + Translation2d robotToGoal = currentTranslation.minus(goalTranslation); + double distanceToGoal = + Math.hypot(robotToGoal.getX(), robotToGoal.getY()); // Calculate lateral linear velocity Translation2d offsetVector = - new Translation2d(alignController.calculate(distanceToGoalValue), 0) - .rotateBy(distanceToGoal.getAngle()); + new Translation2d(alignController.calculate(distanceToGoal), 0) + .rotateBy(robotToGoal.getAngle()); // Calculate total linear velocity Translation2d linearVelocity = From a4da670682befe4fbc1315727a567c648ad3b227 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 19:21:09 -0500 Subject: [PATCH 12/14] rename another variable --- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 6970957..296d007 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -203,7 +203,7 @@ public static Command joystickApproach( Translation2d approachTranslation = approachSupplier.get().getTranslation(); double distanceToApproach = currentTranslation.getDistance(approachTranslation); - Rotation2d perpendicularLine = approachSupplier.get().getRotation(); + Rotation2d alignmentDirection = approachSupplier.get().getRotation(); // Find lateral distance from goal Translation2d goalTranslation = new Translation2d( From 063c7788806b1833ae0f30cf34067d0ca2662916 Mon Sep 17 00:00:00 2001 From: CJ Date: Sun, 2 Feb 2025 19:21:23 -0500 Subject: [PATCH 13/14] fix oversight --- src/main/java/frc/robot/commands/DriveCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 296d007..7970295 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -207,8 +207,8 @@ public static Command joystickApproach( // Find lateral distance from goal Translation2d goalTranslation = new Translation2d( - perpendicularLine.getCos() * distanceToApproach + approachTranslation.getX(), - perpendicularLine.getSin() * distanceToApproach + approachTranslation.getY()); + alignmentDirection.getCos() * distanceToApproach + approachTranslation.getX(), + alignmentDirection.getSin() * distanceToApproach + approachTranslation.getY()); Translation2d robotToGoal = currentTranslation.minus(goalTranslation); double distanceToGoal = From 9dd24ae3e632b7b16fd2606665497fabcd55fd37 Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Sun, 2 Feb 2025 20:01:12 -0500 Subject: [PATCH 14/14] refactored methods for lasercan, added generic distance type --- src/main/java/frc/robot/Constants.java | 72 ------------------- src/main/java/frc/robot/Ports.java | 3 + src/main/java/frc/robot/RobotContainer.java | 25 +++---- .../robot/util/drivers/LaserCANSensor.java | 58 ++++++++++++--- 4 files changed, 60 insertions(+), 98 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62474bb..80b1895 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,11 +4,6 @@ package frc.robot; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; public class Constants { @@ -36,73 +31,6 @@ public static enum Mode { REPLAY } - public static final class ExampleSimpleSubsystemConstants { - public static final int ID_Motor = 10; - - public static TalonFXConfiguration motorConfig() - { - TalonFXConfiguration m_configuration = new TalonFXConfiguration(); - - m_configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_configuration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - m_configuration.Voltage.PeakForwardVoltage = 12.0; - m_configuration.Voltage.PeakReverseVoltage = -12.0; - - m_configuration.CurrentLimits.SupplyCurrentLimit = 20; - m_configuration.CurrentLimits.SupplyCurrentLimitEnable = true; - m_configuration.CurrentLimits.StatorCurrentLimit = 70; - m_configuration.CurrentLimits.StatorCurrentLimitEnable = true; - - return m_configuration; - } - } - - public static final class ExampleComplexSubsystemConstants { - public static final int ID_Motor = 11; - public static final double upperLimit = Units.degreesToRadians(180); - public static final double lowerLimit = Units.degreesToRadians(0); - public static final double tolerance = Units.degreesToRadians(1); - - public static TalonFXConfiguration motorConfig() - { - TalonFXConfiguration m_configuration = new TalonFXConfiguration(); - - m_configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_configuration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - m_configuration.Voltage.PeakForwardVoltage = 12.0; - m_configuration.Voltage.PeakReverseVoltage = -12.0; - - m_configuration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - m_configuration.Feedback.SensorToMechanismRatio = 1; - - m_configuration.Slot0.kP = 1; // output per unit of error in position (output/rotation) - m_configuration.Slot0.kI = 0; // output per unit of integrated error in position - // (output/(rotation*s)) - m_configuration.Slot0.kD = 0; // output per unit of error derivative in position - // (output/rps) - - m_configuration.Slot1.kG = 0; // output to overcome gravity (output) - m_configuration.Slot1.kS = 0; // output to overcome static friction (output) - m_configuration.Slot1.kV = 0; // output per unit of requested velocity (output/rps) - m_configuration.Slot1.kA = 0; // unused, as there is no target acceleration - m_configuration.Slot1.kP = 1; // output per unit of error in position (output/rotation) - m_configuration.Slot1.kI = 0; // output per unit of integrated error in position - // (output/(rotation*s)) - m_configuration.Slot1.kD = 0; // output per unit of error derivative in position - // (output/rps) - - m_configuration.MotionMagic.MotionMagicCruiseVelocity = 10; - m_configuration.MotionMagic.MotionMagicAcceleration = 10; - m_configuration.MotionMagic.MotionMagicJerk = 10; - - m_configuration.CurrentLimits.SupplyCurrentLimit = 20; - m_configuration.CurrentLimits.SupplyCurrentLimitEnable = true; - m_configuration.CurrentLimits.StatorCurrentLimit = 70; - m_configuration.CurrentLimits.StatorCurrentLimitEnable = true; - - return m_configuration; - } - } } diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index a807b0e..4befad3 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -33,6 +33,9 @@ public class Ports { public static final CanDeviceId PROFROLLER_MAIN = new CanDeviceId(24, "rio"); public static final CanDeviceId PROFROLLER_FOLLOWER = new CanDeviceId(25, "rio"); + public static final CanDeviceId CLAW_LASERCAN = new CanDeviceId(33, "rio"); + public static final CanDeviceId RAMP_LASERCAN = new CanDeviceId(34, "rio"); + /* * public static final CanDeviceId INTAKE_PIVOT = new CanDeviceId(8, "canivore1"); public static * final CanDeviceId INTAKE_ROLLER = new CanDeviceId(15, "rio"); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a2985fb..cbaeb39 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,22 +20,11 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.Arm.Arm; -import frc.robot.subsystems.Arm.ArmIO; -import frc.robot.subsystems.Arm.ArmIOSim; -import frc.robot.subsystems.Arm.ArmIOTalonFX; +import frc.robot.subsystems.Arm.*; import frc.robot.subsystems.Elevator.*; -import frc.robot.subsystems.Vision.Vision; -import frc.robot.subsystems.Vision.VisionIO; -import frc.robot.subsystems.Vision.VisionIOPhotonVision; -import frc.robot.subsystems.Vision.VisionIOPhotonVisionSim; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.GyroIO; -import frc.robot.subsystems.drive.GyroIOPigeon2; -import frc.robot.subsystems.drive.GyroIOSim; -import frc.robot.subsystems.drive.ModuleIO; -import frc.robot.subsystems.drive.ModuleIOTalonFXReal; -import frc.robot.subsystems.drive.ModuleIOTalonFXSim; +import frc.robot.subsystems.Vision.*; +import frc.robot.subsystems.drive.*; +import frc.robot.util.drivers.LaserCANSensor; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnField; @@ -53,7 +42,6 @@ public class RobotContainer { // Controllers private final CommandXboxController m_driver = new CommandXboxController(0); - // private final CommandXboxController m_operator = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser m_autoChooser; @@ -68,6 +56,11 @@ public class RobotContainer { public final Vision m_vision; + private final LaserCANSensor m_clawLaserCAN = + new LaserCANSensor(Ports.CLAW_LASERCAN.getDeviceNumber(), Inches.of(6)); + private final LaserCANSensor m_rampLaserCAN = + new LaserCANSensor(Ports.RAMP_LASERCAN.getDeviceNumber(), Inches.of(6)); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { diff --git a/src/main/java/frc/robot/util/drivers/LaserCANSensor.java b/src/main/java/frc/robot/util/drivers/LaserCANSensor.java index 291cbff..5df2c97 100644 --- a/src/main/java/frc/robot/util/drivers/LaserCANSensor.java +++ b/src/main/java/frc/robot/util/drivers/LaserCANSensor.java @@ -7,34 +7,72 @@ import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import lombok.Getter; +import edu.wpi.first.wpilibj.Alert; +import static edu.wpi.first.units.Units.*; + -/** Add your docs here. */ public class LaserCANSensor { private LaserCan lc; - private double closeCut = 200; + private int CAN_ID; + private Distance triggerDistance = Inches.zero(); + + @Getter + public Trigger nearTrigger = new Trigger(() -> getMeasurement().lte(triggerDistance)); + + private final Alert failedConfig = + new Alert("Failed to configure LaserCAN!", AlertType.kError); + private final Alert sensorAlert = + new Alert("Failed to get LaserCAN measurement", Alert.AlertType.kWarning); - public LaserCANSensor(int ID, double closeCut) + /** + * Constructs a new LaserCAN sensor + * + * @param ID CAN ID assigned to the sensor + * @param triggerDistance distance at which to trigger the sensor + */ + public LaserCANSensor(int ID, Distance distance) { - lc = new LaserCan(ID); - this.closeCut = closeCut; + CAN_ID = ID; + lc = new LaserCan(CAN_ID); + triggerDistance = distance; try { lc.setRangingMode(LaserCan.RangingMode.SHORT); lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 6, 6)); - lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_20MS); + failedConfig.set(false); } catch (ConfigurationFailedException e) { System.out.println("Configuration failed! " + e); + failedConfig.setText("Failed to configure LaserCAN ID: " + ID + "!"); + failedConfig.set(true); } + } - public boolean isClose() + /** + * Returns distance reading from LaserCAN sensor + */ + public Distance getMeasurement() { Measurement measurement = lc.getMeasurement(); if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return (measurement.distance_mm < closeCut); + sensorAlert.set(false); + return Millimeters.of(measurement.distance_mm); + } else if (measurement != null + && measurement.status != LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + sensorAlert + .setText("Failed to get LaserCAN ID: " + this.CAN_ID + ", no valid measurement"); + sensorAlert.set(true); + return Millimeters.of(Double.POSITIVE_INFINITY); } else { - return false; + sensorAlert.setText("Failed to get LaserCAN ID: " + this.CAN_ID + ", measurement null"); + sensorAlert.set(true); + return Millimeters.of(Double.POSITIVE_INFINITY); } - } + }