diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 1afcacc1..d5749cbf 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -15,11 +15,10 @@ jobs: # This workflow contains a single job called "build" build: # The type of runner that the job will run on - runs-on: ubuntu-latest + runs-on: ubuntu-24.04 # This grabs the WPILib docker container container: wpilib/roborio-cross-ubuntu:2025-24.04 - # Steps represent a sequence of tasks that will be executed as part of the job steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it @@ -29,10 +28,6 @@ jobs: - name: Add repository to git safe directories run: git config --global --add safe.directory $GITHUB_WORKSPACE - # Grant execute permission for gradlew - - name: Grant execute permission for gradlew - run: chmod +x gradlew - # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build diff --git a/build.gradle b/build.gradle index a1afcada..fb50480c 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.diffplug.spotless" version "6.20.0" - id "com.gorylenko.gradle-git-properties" version '2.5.0' + id "com.gorylenko.gradle-git-properties" version '2.5.1' } gitProperties { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 8e975a5f..366f8335 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.2.1-bin.zip +distributionSha256Sum=72f44c9f8ebcb1af43838f45ee5c4aa9c5444898b3468ab3f4af7b6076c5bc3f networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/src/main/java/com/team6962/lib/digitalsensor/BeamBreak.java b/src/main/java/com/team6962/lib/digitalsensor/BeamBreak.java index 5e0f8521..584ebef2 100644 --- a/src/main/java/com/team6962/lib/digitalsensor/BeamBreak.java +++ b/src/main/java/com/team6962/lib/digitalsensor/BeamBreak.java @@ -6,21 +6,22 @@ import edu.wpi.first.wpilibj.Timer; public class BeamBreak extends DigitalSensor { - private double simulatedDetectionTime; - private double lastDetectionTimestamp; + private double simulatedDetectionTime; + private double lastDetectionTimestamp; - public BeamBreak(int channel, DigitalSensor.Wiring wiring, Time simulatedDetectionTime) { - super(channel, wiring); + public BeamBreak(int channel, DigitalSensor.Wiring wiring, Time simulatedDetectionTime) { + super(channel, wiring); - this.simulatedDetectionTime = simulatedDetectionTime.in(Seconds); - } + this.simulatedDetectionTime = simulatedDetectionTime.in(Seconds); + } - @Override - public void simulationPeriodic() { - setTriggeredInSimulation(Timer.getFPGATimestamp() - lastDetectionTimestamp < simulatedDetectionTime); - } + @Override + public void simulationPeriodic() { + setTriggeredInSimulation( + Timer.getFPGATimestamp() - lastDetectionTimestamp < simulatedDetectionTime); + } - public void simulateDetection() { - lastDetectionTimestamp = Timer.getFPGATimestamp(); - } + public void simulateDetection() { + lastDetectionTimestamp = Timer.getFPGATimestamp(); + } } diff --git a/src/main/java/com/team6962/lib/digitalsensor/DigitalSensor.java b/src/main/java/com/team6962/lib/digitalsensor/DigitalSensor.java index 55c055f6..e19d5b03 100644 --- a/src/main/java/com/team6962/lib/digitalsensor/DigitalSensor.java +++ b/src/main/java/com/team6962/lib/digitalsensor/DigitalSensor.java @@ -5,43 +5,43 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DigitalSensor extends SubsystemBase { - private DigitalInput input; - private Wiring wiring; - private boolean triggeredSim; - - public DigitalSensor(int channel, Wiring wiring) { - input = new DigitalInput(channel); - this.wiring = wiring; + private DigitalInput input; + private Wiring wiring; + private boolean triggeredSim; + + public DigitalSensor(int channel, Wiring wiring) { + input = new DigitalInput(channel); + this.wiring = wiring; + } + + public boolean isTriggered() { + if (RobotBase.isSimulation()) { + return triggeredSim; } - public boolean isTriggered() { - if (RobotBase.isSimulation()) { - return triggeredSim; - } - - return input.get() == wiring.triggeredValue; - } + return input.get() == wiring.triggeredValue; + } - public boolean notTriggered() { - return !isTriggered(); - } + public boolean notTriggered() { + return !isTriggered(); + } - public void setTriggeredInSimulation(boolean triggered) { - this.triggeredSim = triggered; - } + public void setTriggeredInSimulation(boolean triggered) { + this.triggeredSim = triggered; + } - public static enum Wiring { - NormallyOpen(false), - NormallyClosed(true); + public static enum Wiring { + NormallyOpen(false), + NormallyClosed(true); - private boolean triggeredValue; + private boolean triggeredValue; - private Wiring(boolean triggeredValue) { - this.triggeredValue = triggeredValue; - } + private Wiring(boolean triggeredValue) { + this.triggeredValue = triggeredValue; + } - public boolean getValueWhenDetecting() { - return triggeredValue; - } + public boolean getValueWhenDetecting() { + return triggeredValue; } + } } diff --git a/src/main/java/com/team6962/lib/swerve/SwerveConfig.java b/src/main/java/com/team6962/lib/swerve/SwerveConfig.java index 7a7758c4..737025e9 100644 --- a/src/main/java/com/team6962/lib/swerve/SwerveConfig.java +++ b/src/main/java/com/team6962/lib/swerve/SwerveConfig.java @@ -28,7 +28,6 @@ import com.team6962.lib.swerve.module.SwerveModule.Corner; import com.team6962.lib.utils.KinematicsUtils; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/com/team6962/lib/swerve/SwerveCore.java b/src/main/java/com/team6962/lib/swerve/SwerveCore.java index 165706bc..c8a769c1 100644 --- a/src/main/java/com/team6962/lib/swerve/SwerveCore.java +++ b/src/main/java/com/team6962/lib/swerve/SwerveCore.java @@ -62,7 +62,8 @@ public SwerveCore(SwerveConfig constants) { kinematics = KinematicsUtils.kinematicsFromChassis(constants.chassis()); poseEstimator = - new PoseEstimator(kinematics, () -> getModulePositions(), () -> getModuleStates(), constants); + new PoseEstimator( + kinematics, () -> getModulePositions(), () -> getModuleStates(), constants); currentMovement = new SpeedsMovement(); diff --git a/src/main/java/com/team6962/lib/swerve/SwerveDrive.java b/src/main/java/com/team6962/lib/swerve/SwerveDrive.java index 78e2819e..cc1eb9ef 100644 --- a/src/main/java/com/team6962/lib/swerve/SwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/SwerveDrive.java @@ -11,10 +11,6 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; -import java.util.List; -import java.util.Set; -import java.util.function.Supplier; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.config.PIDConstants; @@ -34,7 +30,6 @@ import com.team6962.lib.utils.CommandUtils; import com.team6962.lib.utils.KinematicsUtils; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -57,6 +52,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; /** * The main class for the swerve drive system. This class extends {@link SwerveCore} to provide the @@ -561,9 +559,11 @@ public Command driveTwist(Supplier twist) { public Command drivePreciselyTo(Pose2d targetPose) { return driveTwist(() -> getEstimatedPose().log(targetPose)) - .deadlineFor(Commands.run(() -> { - Logger.getField().getObject("Target Pose").setPose(targetPose); - })); + .deadlineFor( + Commands.run( + () -> { + Logger.getField().getObject("Target Pose").setPose(targetPose); + })); } private class ProfiledDriveCommand extends Command { @@ -573,9 +573,9 @@ private class ProfiledDriveCommand extends Command { private double startTime; public ProfiledDriveCommand( - HolonomicPositionController.State targetState, // TODO: Convert to supplier or make wrapper methods use Commands.defer - HolonomicPositionController controller - ) { + HolonomicPositionController.State + targetState, // TODO: Convert to supplier or make wrapper methods use Commands.defer + HolonomicPositionController controller) { this.targetState = targetState; this.controller = controller; @@ -611,30 +611,34 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - controller.close(); + controller.close(); } } - public Command driveQuicklyTo(HolonomicPositionController.State targetState, LinearVelocity maxVelocity) { - return new ProfiledDriveCommand(targetState, new HolonomicPositionController( - new TrapezoidProfile.Constraints( - maxVelocity.in(MetersPerSecond), - getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2), - new TrapezoidProfile.Constraints( - getConstants().maxRotationSpeed().in(RadiansPerSecond), - getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2) - )) - .deadlineFor(Commands.run(() -> { - Logger.getField().getObject("Target Pose").setPose(targetState.position); - })); + public Command driveQuicklyTo( + HolonomicPositionController.State targetState, LinearVelocity maxVelocity) { + return new ProfiledDriveCommand( + targetState, + new HolonomicPositionController( + new TrapezoidProfile.Constraints( + maxVelocity.in(MetersPerSecond), + getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2), + new TrapezoidProfile.Constraints( + getConstants().maxRotationSpeed().in(RadiansPerSecond), + getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2))) + .deadlineFor( + Commands.run( + () -> { + Logger.getField().getObject("Target Pose").setPose(targetState.position); + })); } - public Command driveQuicklyTo(Pose2d targetPose, ChassisSpeeds targetSpeeds, LinearVelocity maxVelocity) { - return driveQuicklyTo(new HolonomicPositionController.State(targetPose, targetSpeeds), maxVelocity); + public Command driveQuicklyTo( + Pose2d targetPose, ChassisSpeeds targetSpeeds, LinearVelocity maxVelocity) { + return driveQuicklyTo( + new HolonomicPositionController.State(targetPose, targetSpeeds), maxVelocity); } public Command driveQuicklyTo(Pose2d targetPose, LinearVelocity maxVelocity) { @@ -642,21 +646,22 @@ public Command driveQuicklyTo(Pose2d targetPose, LinearVelocity maxVelocity) { } public Command driveQuicklyTo(HolonomicPositionController.State targetState) { - return new ProfiledDriveCommand(targetState, new HolonomicPositionController( - new TrapezoidProfile.Constraints( - getConstants().maxDriveSpeed().in(MetersPerSecond), - getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2), - new TrapezoidProfile.Constraints( - getConstants().maxRotationSpeed().in(RadiansPerSecond), - getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2) - )) - .deadlineFor(Commands.run(() -> { - Logger.getField().getObject("Target Pose").setPose(targetState.position); - })); + return new ProfiledDriveCommand( + targetState, + new HolonomicPositionController( + new TrapezoidProfile.Constraints( + getConstants().maxDriveSpeed().in(MetersPerSecond), + getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2), + new TrapezoidProfile.Constraints( + getConstants().maxRotationSpeed().in(RadiansPerSecond), + getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2))) + .deadlineFor( + Commands.run( + () -> { + Logger.getField().getObject("Target Pose").setPose(targetState.position); + })); } public Command driveQuicklyTo(Pose2d targetPose, ChassisSpeeds targetSpeeds) { @@ -668,40 +673,42 @@ public Command driveQuicklyTo(Pose2d targetPose) { } public Command driveQuicklyToState(Supplier targetState) { - ProfiledDriveCommand profiledDriveCommand = new ProfiledDriveCommand(targetState.get(), new HolonomicPositionController( - new TrapezoidProfile.Constraints( - getConstants().maxDriveSpeed().in(MetersPerSecond), - getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2), - new TrapezoidProfile.Constraints( - getConstants().maxRotationSpeed().in(RadiansPerSecond), - getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond) - ), - new PIDConstraints(1.0, 0.0, 0.2) - )); - - return profiledDriveCommand.deadlineFor(Commands.run(() -> { - profiledDriveCommand.setTarget(targetState.get()); - Logger.getField().getObject("Target Pose").setPose(targetState.get().position); - })); + ProfiledDriveCommand profiledDriveCommand = + new ProfiledDriveCommand( + targetState.get(), + new HolonomicPositionController( + new TrapezoidProfile.Constraints( + getConstants().maxDriveSpeed().in(MetersPerSecond), + getConstants().maxLinearAcceleration().in(MetersPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2), + new TrapezoidProfile.Constraints( + getConstants().maxRotationSpeed().in(RadiansPerSecond), + getConstants().maxAngularAcceleration().in(RadiansPerSecondPerSecond)), + new PIDConstraints(1.0, 0.0, 0.2))); + + return profiledDriveCommand.deadlineFor( + Commands.run( + () -> { + profiledDriveCommand.setTarget(targetState.get()); + Logger.getField().getObject("Target Pose").setPose(targetState.get().position); + })); } public Command driveQuicklyTo(Supplier targetPose, Supplier targetSpeeds) { - return driveQuicklyToState(() -> new HolonomicPositionController.State(targetPose.get(), targetSpeeds.get())); + return driveQuicklyToState( + () -> new HolonomicPositionController.State(targetPose.get(), targetSpeeds.get())); } public Command driveQuicklyTo(Supplier targetPose) { - return driveQuicklyToState(() -> new HolonomicPositionController.State(targetPose.get(), new ChassisSpeeds())); + return driveQuicklyToState( + () -> new HolonomicPositionController.State(targetPose.get(), new ChassisSpeeds())); } public Command driveTo(Pose2d targetPose, ChassisSpeeds targetSpeeds) { return Commands.either( - drivePreciselyTo(targetPose), - driveQuicklyTo(targetPose, targetSpeeds) - .andThen(drivePreciselyTo(targetPose)), - () -> isWithinToleranceOf(targetPose, Inches.of(15), Degrees.of(30)) - ); + drivePreciselyTo(targetPose), + driveQuicklyTo(targetPose, targetSpeeds).andThen(drivePreciselyTo(targetPose)), + () -> isWithinToleranceOf(targetPose, Inches.of(15), Degrees.of(30))); } public Command driveTo(Pose2d targetPose) { diff --git a/src/main/java/com/team6962/lib/swerve/auto/PoseEstimator.java b/src/main/java/com/team6962/lib/swerve/auto/PoseEstimator.java index d16442d4..c6cf8eb7 100644 --- a/src/main/java/com/team6962/lib/swerve/auto/PoseEstimator.java +++ b/src/main/java/com/team6962/lib/swerve/auto/PoseEstimator.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants.LIMELIGHT; import frc.robot.vision.AprilTags; - import java.util.function.Supplier; /** diff --git a/src/main/java/com/team6962/lib/swerve/auto/SwerveGyroscope.java b/src/main/java/com/team6962/lib/swerve/auto/SwerveGyroscope.java index 2555db17..5427c077 100644 --- a/src/main/java/com/team6962/lib/swerve/auto/SwerveGyroscope.java +++ b/src/main/java/com/team6962/lib/swerve/auto/SwerveGyroscope.java @@ -2,85 +2,87 @@ import static edu.wpi.first.units.Units.Radians; -import java.util.function.Supplier; - import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import com.team6962.lib.swerve.SwerveConfig; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.CTREUtils; - import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.Supplier; public abstract class SwerveGyroscope extends SubsystemBase { - public SwerveGyroscope() { - setName("Swerve Drive/Gyroscope"); + public SwerveGyroscope() { + setName("Swerve Drive/Gyroscope"); - Logger.logMeasure(getName() + "/heading", this::getHeading); - } + Logger.logMeasure(getName() + "/heading", this::getHeading); + } - public abstract Angle getHeading(); + public abstract Angle getHeading(); - public static SwerveGyroscope get(Supplier moduleDeltasSupplier, SwerveDriveKinematics kinematics, SwerveConfig config) { - if (RobotBase.isReal()) return new Pigeon(config); - else return new Simulated(moduleDeltasSupplier, kinematics); - } + public static SwerveGyroscope get( + Supplier moduleDeltasSupplier, + SwerveDriveKinematics kinematics, + SwerveConfig config) { + if (RobotBase.isReal()) return new Pigeon(config); + else return new Simulated(moduleDeltasSupplier, kinematics); + } - private static class Pigeon extends SwerveGyroscope { - private final Pigeon2 pigeon; - private final StatusSignal headingSignal; + private static class Pigeon extends SwerveGyroscope { + private final Pigeon2 pigeon; + private final StatusSignal headingSignal; - public Pigeon(SwerveConfig config) { - pigeon = new Pigeon2(config.gyroscope().canId(), config.canBus()); + public Pigeon(SwerveConfig config) { + pigeon = new Pigeon2(config.gyroscope().canId(), config.canBus()); - Pigeon2Configuration pigeonConfig = new Pigeon2Configuration(); - pigeonConfig.MountPose = config.gyroscope().mountPose(); - CTREUtils.check(pigeon.getConfigurator().apply(pigeonConfig)); + Pigeon2Configuration pigeonConfig = new Pigeon2Configuration(); + pigeonConfig.MountPose = config.gyroscope().mountPose(); + CTREUtils.check(pigeon.getConfigurator().apply(pigeonConfig)); - headingSignal = pigeon.getYaw(); - } + headingSignal = pigeon.getYaw(); + } - @Override - public void periodic() { - headingSignal.refresh(); - } + @Override + public void periodic() { + headingSignal.refresh(); + } - @Override - public Angle getHeading() { - return CTREUtils.unwrap(headingSignal); - } + @Override + public Angle getHeading() { + return CTREUtils.unwrap(headingSignal); } + } - private static class Simulated extends SwerveGyroscope { - private Angle heading = Radians.of(0); + private static class Simulated extends SwerveGyroscope { + private Angle heading = Radians.of(0); - private final Supplier moduleDeltasSupplier; - private final SwerveDriveKinematics kinematics; + private final Supplier moduleDeltasSupplier; + private final SwerveDriveKinematics kinematics; - public Simulated(Supplier moduleDeltasSupplier, SwerveDriveKinematics kinematics) { - this.moduleDeltasSupplier = moduleDeltasSupplier; - this.kinematics = kinematics; - } + public Simulated( + Supplier moduleDeltasSupplier, SwerveDriveKinematics kinematics) { + this.moduleDeltasSupplier = moduleDeltasSupplier; + this.kinematics = kinematics; + } - @Override - public Angle getHeading() { - return heading; - } + @Override + public Angle getHeading() { + return heading; + } - @Override - public void periodic() { - Angle headingChange = Radians.of(kinematics.toTwist2d(moduleDeltasSupplier.get()).dtheta); + @Override + public void periodic() { + Angle headingChange = Radians.of(kinematics.toTwist2d(moduleDeltasSupplier.get()).dtheta); - if (Math.abs(headingChange.in(Radians)) < 0.01) { - headingChange = Radians.of(0); - } + if (Math.abs(headingChange.in(Radians)) < 0.01) { + headingChange = Radians.of(0); + } - heading = heading.plus(headingChange); - } + heading = heading.plus(headingChange); } + } } diff --git a/src/main/java/com/team6962/lib/swerve/controller/HolonomicPositionController.java b/src/main/java/com/team6962/lib/swerve/controller/HolonomicPositionController.java index 9a01da3e..ced595e8 100644 --- a/src/main/java/com/team6962/lib/swerve/controller/HolonomicPositionController.java +++ b/src/main/java/com/team6962/lib/swerve/controller/HolonomicPositionController.java @@ -4,100 +4,115 @@ import com.team6962.lib.swerve.controller.LineFollowing.Vector2; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; public class HolonomicPositionController implements AutoCloseable { - private PositionController xController; - private PositionController yController; - private PositionController thetaController; + private PositionController xController; + private PositionController yController; + private PositionController thetaController; - @SuppressWarnings("resource") - public HolonomicPositionController( - TrapezoidProfile.Constraints linearProfile, - PIDConstraints linearFeedback, - TrapezoidProfile.Constraints angularProfile, - PIDConstraints angularFeedback - ) { - this.xController = new PositionController(linearProfile, linearFeedback).withLogging("HolonomicPositionController/XMeters"); - this.yController = new PositionController(linearProfile, linearFeedback).withLogging("HolonomicPositionController/YMeters"); - this.thetaController = new PositionController(angularProfile, angularFeedback).withLogging("HolonomicPositionController/ThetaRadians"); - } + @SuppressWarnings("resource") + public HolonomicPositionController( + TrapezoidProfile.Constraints linearProfile, + PIDConstraints linearFeedback, + TrapezoidProfile.Constraints angularProfile, + PIDConstraints angularFeedback) { + this.xController = + new PositionController(linearProfile, linearFeedback) + .withLogging("HolonomicPositionController/XMeters"); + this.yController = + new PositionController(linearProfile, linearFeedback) + .withLogging("HolonomicPositionController/YMeters"); + this.thetaController = + new PositionController(angularProfile, angularFeedback) + .withLogging("HolonomicPositionController/ThetaRadians"); + } - public static class State { - public Pose2d position; - public ChassisSpeeds velocity; + public static class State { + public Pose2d position; + public ChassisSpeeds velocity; - public State(Pose2d position, ChassisSpeeds velocity) { - this.position = position; - this.velocity = velocity; - } + public State(Pose2d position, ChassisSpeeds velocity) { + this.position = position; + this.velocity = velocity; + } - public boolean equals(State other) { - return this.position.equals(other.position) && - Math.abs(this.velocity.vxMetersPerSecond - other.velocity.vxMetersPerSecond) < 1e-6 && - Math.abs(this.velocity.vyMetersPerSecond - other.velocity.vyMetersPerSecond) < 1e-6 && - Math.abs(this.velocity.omegaRadiansPerSecond - other.velocity.omegaRadiansPerSecond) < 1e-6; - } + public boolean equals(State other) { + return this.position.equals(other.position) + && Math.abs(this.velocity.vxMetersPerSecond - other.velocity.vxMetersPerSecond) < 1e-6 + && Math.abs(this.velocity.vyMetersPerSecond - other.velocity.vyMetersPerSecond) < 1e-6 + && Math.abs(this.velocity.omegaRadiansPerSecond - other.velocity.omegaRadiansPerSecond) + < 1e-6; } + } - public ChassisSpeeds calculate(double time, State start, State end, State current) { - Angle startAngle = MeasureMath.minDifference(start.position.getRotation().getMeasure(), end.position.getRotation().getMeasure()); - Angle currentAngle = MeasureMath.minDifference(current.position.getRotation().getMeasure(), end.position.getRotation().getMeasure()); - Angle endAngle = Radians.of(0); + public ChassisSpeeds calculate(double time, State start, State end, State current) { + Angle startAngle = + MeasureMath.minDifference( + start.position.getRotation().getMeasure(), end.position.getRotation().getMeasure()); + Angle currentAngle = + MeasureMath.minDifference( + current.position.getRotation().getMeasure(), end.position.getRotation().getMeasure()); + Angle endAngle = Radians.of(0); - TrapezoidProfile.State optimizedAngularEndState = new TrapezoidProfile.State( - endAngle.in(Radians), - end.velocity.omegaRadiansPerSecond - ); + TrapezoidProfile.State optimizedAngularEndState = + new TrapezoidProfile.State(endAngle.in(Radians), end.velocity.omegaRadiansPerSecond); - Vector2 pathDirection = LineFollowing.directionBetweenPoints(new Vector2(start.position), new Vector2(end.position)); - Vector2 startPosition = new Vector2(0, 0); - Vector2 endPosition = new Vector2(end.position.getTranslation().minus(start.position.getTranslation()).getNorm(), 0); - Vector2 startVelocity = LineFollowing.fieldToPathDisplacement(new Vector2(start.velocity), pathDirection); - Vector2 endVelocity = LineFollowing.fieldToPathDisplacement(new Vector2(end.velocity), pathDirection); - Vector2 currentVelocity = LineFollowing.fieldToPathDisplacement(new Vector2(current.velocity), pathDirection); - Vector2 currentPosition = LineFollowing.fieldToPathPosition(new Vector2(current.position), new Vector2(start.position), pathDirection); + Vector2 pathDirection = + LineFollowing.directionBetweenPoints( + new Vector2(start.position), new Vector2(end.position)); + Vector2 startPosition = new Vector2(0, 0); + Vector2 endPosition = + new Vector2( + end.position.getTranslation().minus(start.position.getTranslation()).getNorm(), 0); + Vector2 startVelocity = + LineFollowing.fieldToPathDisplacement(new Vector2(start.velocity), pathDirection); + Vector2 endVelocity = + LineFollowing.fieldToPathDisplacement(new Vector2(end.velocity), pathDirection); + Vector2 currentVelocity = + LineFollowing.fieldToPathDisplacement(new Vector2(current.velocity), pathDirection); + Vector2 currentPosition = + LineFollowing.fieldToPathPosition( + new Vector2(current.position), new Vector2(start.position), pathDirection); - Vector2 velocity = LineFollowing.pathToFieldDisplacement(new Vector2( - xController.calculateVelocity( - time, - new TrapezoidProfile.State(startPosition.x, startVelocity.x), - new TrapezoidProfile.State(endPosition.x, endVelocity.x), - new TrapezoidProfile.State(currentPosition.x, currentVelocity.x) - ), - yController.calculateVelocity( - time, - new TrapezoidProfile.State(startPosition.y, startVelocity.y), - new TrapezoidProfile.State(endPosition.y, endVelocity.y), - new TrapezoidProfile.State(currentPosition.y, currentVelocity.y) - ) - ), pathDirection); + Vector2 velocity = + LineFollowing.pathToFieldDisplacement( + new Vector2( + xController.calculateVelocity( + time, + new TrapezoidProfile.State(startPosition.x, startVelocity.x), + new TrapezoidProfile.State(endPosition.x, endVelocity.x), + new TrapezoidProfile.State(currentPosition.x, currentVelocity.x)), + yController.calculateVelocity( + time, + new TrapezoidProfile.State(startPosition.y, startVelocity.y), + new TrapezoidProfile.State(endPosition.y, endVelocity.y), + new TrapezoidProfile.State(currentPosition.y, currentVelocity.y))), + pathDirection); - return new ChassisSpeeds( - velocity.x, - velocity.y, - thetaController.calculateVelocity( - time, - new TrapezoidProfile.State(startAngle.in(Radians), start.velocity.omegaRadiansPerSecond), - optimizedAngularEndState, - new TrapezoidProfile.State(currentAngle.in(Radians), current.velocity.omegaRadiansPerSecond) - ) - ); - } + return new ChassisSpeeds( + velocity.x, + velocity.y, + thetaController.calculateVelocity( + time, + new TrapezoidProfile.State( + startAngle.in(Radians), start.velocity.omegaRadiansPerSecond), + optimizedAngularEndState, + new TrapezoidProfile.State( + currentAngle.in(Radians), current.velocity.omegaRadiansPerSecond))); + } - public boolean isFinished() { - return xController.isFinished() && - thetaController.isFinished(); - } + public boolean isFinished() { + return xController.isFinished() && thetaController.isFinished(); + } - @Override - public void close() { - xController.close(); - yController.close(); - thetaController.close(); - } + @Override + public void close() { + xController.close(); + yController.close(); + thetaController.close(); + } } diff --git a/src/main/java/com/team6962/lib/swerve/controller/LineFollowing.java b/src/main/java/com/team6962/lib/swerve/controller/LineFollowing.java index d7a065b9..fcf16c76 100644 --- a/src/main/java/com/team6962/lib/swerve/controller/LineFollowing.java +++ b/src/main/java/com/team6962/lib/swerve/controller/LineFollowing.java @@ -6,127 +6,129 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; public final class LineFollowing { - private LineFollowing() { - } - - public static class Vector2 { - public final double x; - public final double y; - - public Vector2(double x, double y) { - this.x = x; - this.y = y; - } - - public Vector2() { - this(0, 0); - } - - public Vector2(Translation2d translation) { - this(translation.getX(), translation.getY()); - } - - public Vector2(Pose2d pose) { - this(pose.getTranslation()); - } + private LineFollowing() {} - public Vector2(ChassisSpeeds speeds) { - this(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - } + public static class Vector2 { + public final double x; + public final double y; - public Vector2(Rotation2d rotation) { - this(rotation.getCos(), rotation.getSin()); - } - - public double magnitude() { - return Math.hypot(x, y); - } - - public Vector2 plus(Vector2 other) { - return new Vector2(this.x + other.x, this.y + other.y); - } - - public Vector2 minus(Vector2 other) { - return new Vector2(this.x - other.x, this.y - other.y); - } + public Vector2(double x, double y) { + this.x = x; + this.y = y; + } - public Vector2 times(double scalar) { - return new Vector2(this.x * scalar, this.y * scalar); - } + public Vector2() { + this(0, 0); + } - public Vector2 div(double scalar) { - return new Vector2(this.x / scalar, this.y / scalar); - } + public Vector2(Translation2d translation) { + this(translation.getX(), translation.getY()); + } - public Vector2 times(Vector2 other) { - return new Vector2(this.x * other.x, this.y * other.y); - } + public Vector2(Pose2d pose) { + this(pose.getTranslation()); + } - public Vector2 div(Vector2 other) { - return new Vector2(this.x / other.x, this.y / other.y); - } + public Vector2(ChassisSpeeds speeds) { + this(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + } - public Vector2 negate() { - return new Vector2(-this.x, -this.y); - } + public Vector2(Rotation2d rotation) { + this(rotation.getCos(), rotation.getSin()); + } - public double dot(Vector2 other) { - return this.x * other.x + this.y * other.y; - } + public double magnitude() { + return Math.hypot(x, y); + } - public Vector2 rotate90CCW() { - return new Vector2(-this.y, this.x); - } + public Vector2 plus(Vector2 other) { + return new Vector2(this.x + other.x, this.y + other.y); + } - public Vector2 rotate90CW() { - return new Vector2(this.y, -this.x); - } + public Vector2 minus(Vector2 other) { + return new Vector2(this.x - other.x, this.y - other.y); + } - public Vector2 unit() { - double mag = magnitude(); + public Vector2 times(double scalar) { + return new Vector2(this.x * scalar, this.y * scalar); + } - if (mag < 1e-9) { - return new Vector2(0, 0); - } + public Vector2 div(double scalar) { + return new Vector2(this.x / scalar, this.y / scalar); + } - return new Vector2(this.x / mag, this.y / mag); - } + public Vector2 times(Vector2 other) { + return new Vector2(this.x * other.x, this.y * other.y); + } - public Translation2d toTranslation2d() { - return new Translation2d(this.x, this.y); - } + public Vector2 div(Vector2 other) { + return new Vector2(this.x / other.x, this.y / other.y); + } - public Pose2d toPose2d(Rotation2d rotation) { - return new Pose2d(this.x, this.y, rotation); - } + public Vector2 negate() { + return new Vector2(-this.x, -this.y); } - public static Vector2 directionBetweenPoints(Vector2 start, Vector2 end) { - return end.minus(start).unit(); + public double dot(Vector2 other) { + return this.x * other.x + this.y * other.y; } - public static Vector2 fieldToPathPosition(Vector2 robotPositionFieldCoords, Vector2 start, Vector2 pathDirection) { - return fieldToPathDisplacement(robotPositionFieldCoords.minus(start), pathDirection); + public Vector2 rotate90CCW() { + return new Vector2(-this.y, this.x); } - public static Vector2 pathToFieldPosition(Vector2 robotPositionPathCoords, Vector2 start, Vector2 pathDirection) { - return start.plus(pathToFieldDisplacement(start, pathDirection)); + public Vector2 rotate90CW() { + return new Vector2(this.y, -this.x); } - public static Vector2 fieldToPathDisplacement(Vector2 robotDisplacementFieldCoords, Vector2 pathDirection) { - Vector2 pathNormal = pathDirection.rotate90CCW(); + public Vector2 unit() { + double mag = magnitude(); + + if (mag < 1e-9) { + return new Vector2(0, 0); + } - return new Vector2( - robotDisplacementFieldCoords.dot(pathDirection), - robotDisplacementFieldCoords.dot(pathNormal) - ); + return new Vector2(this.x / mag, this.y / mag); } - public static Vector2 pathToFieldDisplacement(Vector2 robotDisplacementPathCoords, Vector2 pathDirection) { - Vector2 pathNormal = pathDirection.rotate90CCW(); + public Translation2d toTranslation2d() { + return new Translation2d(this.x, this.y); + } - return pathDirection.times(robotDisplacementPathCoords.x).plus( - pathNormal.times(robotDisplacementPathCoords.y) - ); + public Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(this.x, this.y, rotation); } + } + + public static Vector2 directionBetweenPoints(Vector2 start, Vector2 end) { + return end.minus(start).unit(); + } + + public static Vector2 fieldToPathPosition( + Vector2 robotPositionFieldCoords, Vector2 start, Vector2 pathDirection) { + return fieldToPathDisplacement(robotPositionFieldCoords.minus(start), pathDirection); + } + + public static Vector2 pathToFieldPosition( + Vector2 robotPositionPathCoords, Vector2 start, Vector2 pathDirection) { + return start.plus(pathToFieldDisplacement(start, pathDirection)); + } + + public static Vector2 fieldToPathDisplacement( + Vector2 robotDisplacementFieldCoords, Vector2 pathDirection) { + Vector2 pathNormal = pathDirection.rotate90CCW(); + + return new Vector2( + robotDisplacementFieldCoords.dot(pathDirection), + robotDisplacementFieldCoords.dot(pathNormal)); + } + + public static Vector2 pathToFieldDisplacement( + Vector2 robotDisplacementPathCoords, Vector2 pathDirection) { + Vector2 pathNormal = pathDirection.rotate90CCW(); + + return pathDirection + .times(robotDisplacementPathCoords.x) + .plus(pathNormal.times(robotDisplacementPathCoords.y)); + } } diff --git a/src/main/java/com/team6962/lib/swerve/controller/PIDConstraints.java b/src/main/java/com/team6962/lib/swerve/controller/PIDConstraints.java index 18d01b6f..d8e7a06c 100644 --- a/src/main/java/com/team6962/lib/swerve/controller/PIDConstraints.java +++ b/src/main/java/com/team6962/lib/swerve/controller/PIDConstraints.java @@ -3,84 +3,83 @@ import edu.wpi.first.math.controller.PIDController; public class PIDConstraints { - public double kP = 0.0; - public double kI = 0.0; - public double kD = 0.0; - public double iZone = Double.POSITIVE_INFINITY; - public double iMax = 1.0; - public double iMin = -1.0; - public double tolerance = 0.05; - public double derivativeTolerance = Double.POSITIVE_INFINITY; - public double period = 0.02; - - public PIDConstraints() { - } - - public PIDConstraints(double kP, double kI, double kD) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - } - - public PIDConstraints(double kP, double kI, double kD, double period) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.period = period; - } - - public PIDConstraints withKP(double kP) { - this.kP = kP; - return this; - } - - public PIDConstraints withKI(double kI) { - this.kI = kI; - return this; - } - - public PIDConstraints withKD(double kD) { - this.kD = kD; - return this; - } - - public PIDConstraints withIZone(double iZone) { - this.iZone = iZone; - return this; - } - - public PIDConstraints withIMin(double iMin) { - this.iMin = iMin; - return this; - } - - public PIDConstraints withIMax(double iMax) { - this.iMax = iMax; - return this; - } - - public PIDConstraints withTolerance(double tolerance) { - this.tolerance = tolerance; - return this; - } - - public PIDConstraints withDerivativeTolerance(double derivativeTolerance) { - this.derivativeTolerance = derivativeTolerance; - return this; - } - - public PIDConstraints withPeriod(double period) { - this.period = period; - return this; - } - - public PIDController createController() { - var controller = new PIDController(kP, kI, kD, period); - - controller.setIZone(iZone); - controller.setIntegratorRange(iMin, iMax); - controller.setTolerance(tolerance, derivativeTolerance); - - return controller; - } + public double kP = 0.0; + public double kI = 0.0; + public double kD = 0.0; + public double iZone = Double.POSITIVE_INFINITY; + public double iMax = 1.0; + public double iMin = -1.0; + public double tolerance = 0.05; + public double derivativeTolerance = Double.POSITIVE_INFINITY; + public double period = 0.02; + + public PIDConstraints() {} + + public PIDConstraints(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + public PIDConstraints(double kP, double kI, double kD, double period) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.period = period; + } + + public PIDConstraints withKP(double kP) { + this.kP = kP; + return this; + } + + public PIDConstraints withKI(double kI) { + this.kI = kI; + return this; + } + + public PIDConstraints withKD(double kD) { + this.kD = kD; + return this; + } + + public PIDConstraints withIZone(double iZone) { + this.iZone = iZone; + return this; + } + + public PIDConstraints withIMin(double iMin) { + this.iMin = iMin; + return this; + } + + public PIDConstraints withIMax(double iMax) { + this.iMax = iMax; + return this; + } + + public PIDConstraints withTolerance(double tolerance) { + this.tolerance = tolerance; + return this; + } + + public PIDConstraints withDerivativeTolerance(double derivativeTolerance) { + this.derivativeTolerance = derivativeTolerance; + return this; + } + + public PIDConstraints withPeriod(double period) { + this.period = period; + return this; + } + + public PIDController createController() { + var controller = new PIDController(kP, kI, kD, period); + + controller.setIZone(iZone); + controller.setIntegratorRange(iMin, iMax); + controller.setTolerance(tolerance, derivativeTolerance); + + return controller; + } } diff --git a/src/main/java/com/team6962/lib/swerve/controller/PositionController.java b/src/main/java/com/team6962/lib/swerve/controller/PositionController.java index a4f4dd0d..ab400c66 100644 --- a/src/main/java/com/team6962/lib/swerve/controller/PositionController.java +++ b/src/main/java/com/team6962/lib/swerve/controller/PositionController.java @@ -1,74 +1,82 @@ package com.team6962.lib.swerve.controller; import com.team6962.lib.telemetry.Logger; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; public class PositionController implements AutoCloseable { - private TrapezoidProfile motionProfile; - private PIDController pidController; - private boolean finished; - - private double lastTime, pidOutput, profilePosition, profileVelocity; - private TrapezoidProfile.State lastStart, lastEnd, lastCurrent; - - private Runnable closeLogging; - - public PositionController(TrapezoidProfile.Constraints profileConstraints, PIDConstraints pidConstraints) { - this.motionProfile = new TrapezoidProfile(profileConstraints); - this.pidController = pidConstraints.createController(); - } - - public PositionController withLogging(String name) { - closeLogging = Logger.addUpdate(name, () -> { - if (lastStart == null || lastEnd == null || lastCurrent == null) return; - - Logger.log(name + "/timeSeconds", lastTime); - Logger.log(name + "/start/position", lastStart.position); - Logger.log(name + "/start/velocity", lastStart.velocity); - Logger.log(name + "/end/position", lastEnd.position); - Logger.log(name + "/end/velocity", lastEnd.velocity); - Logger.log(name + "/current/position", lastCurrent.position); - Logger.log(name + "/current/velocity", lastCurrent.velocity); - Logger.log(name + "/profile/position", profilePosition); - Logger.log(name + "/profile/velocity", profileVelocity); - Logger.log(name + "/velocityOutput/pid", pidOutput); - Logger.log(name + "/velocityOutput/profile", profileVelocity); - Logger.log(name + "/velocityOutput/total", profileVelocity + pidOutput); - Logger.log(name + "/isFinished", isFinished()); - }); - - return this; - } - - public double calculateVelocity(double time, TrapezoidProfile.State start, TrapezoidProfile.State end, TrapezoidProfile.State current) { - this.lastTime = time; - this.lastStart = start; - this.lastEnd = end; - this.lastCurrent = current; - - TrapezoidProfile.State target = motionProfile.calculate(time, start, end); - double pidOutput = pidController.calculate(current.position, target.position); - - this.pidOutput = pidOutput; - this.profileVelocity = target.velocity; - - profilePosition = target.position; - - finished = /*Math.abs(target.position - end.position) < tolerance && */motionProfile.isFinished(time); - - return target.velocity + pidOutput; - } - - public boolean isFinished() { - return finished; - } - - @Override - public void close() { - if (closeLogging != null) { - closeLogging.run(); - } + private TrapezoidProfile motionProfile; + private PIDController pidController; + private boolean finished; + + private double lastTime, pidOutput, profilePosition, profileVelocity; + private TrapezoidProfile.State lastStart, lastEnd, lastCurrent; + + private Runnable closeLogging; + + public PositionController( + TrapezoidProfile.Constraints profileConstraints, PIDConstraints pidConstraints) { + this.motionProfile = new TrapezoidProfile(profileConstraints); + this.pidController = pidConstraints.createController(); + } + + public PositionController withLogging(String name) { + closeLogging = + Logger.addUpdate( + name, + () -> { + if (lastStart == null || lastEnd == null || lastCurrent == null) return; + + Logger.log(name + "/timeSeconds", lastTime); + Logger.log(name + "/start/position", lastStart.position); + Logger.log(name + "/start/velocity", lastStart.velocity); + Logger.log(name + "/end/position", lastEnd.position); + Logger.log(name + "/end/velocity", lastEnd.velocity); + Logger.log(name + "/current/position", lastCurrent.position); + Logger.log(name + "/current/velocity", lastCurrent.velocity); + Logger.log(name + "/profile/position", profilePosition); + Logger.log(name + "/profile/velocity", profileVelocity); + Logger.log(name + "/velocityOutput/pid", pidOutput); + Logger.log(name + "/velocityOutput/profile", profileVelocity); + Logger.log(name + "/velocityOutput/total", profileVelocity + pidOutput); + Logger.log(name + "/isFinished", isFinished()); + }); + + return this; + } + + public double calculateVelocity( + double time, + TrapezoidProfile.State start, + TrapezoidProfile.State end, + TrapezoidProfile.State current) { + this.lastTime = time; + this.lastStart = start; + this.lastEnd = end; + this.lastCurrent = current; + + TrapezoidProfile.State target = motionProfile.calculate(time, start, end); + double pidOutput = pidController.calculate(current.position, target.position); + + this.pidOutput = pidOutput; + this.profileVelocity = target.velocity; + + profilePosition = target.position; + + finished = /*Math.abs(target.position - end.position) < tolerance && */ + motionProfile.isFinished(time); + + return target.velocity + pidOutput; + } + + public boolean isFinished() { + return finished; + } + + @Override + public void close() { + if (closeLogging != null) { + closeLogging.run(); } + } } diff --git a/src/main/java/com/team6962/lib/swerve/controller/TrapezoidalConstraints.java b/src/main/java/com/team6962/lib/swerve/controller/TrapezoidalConstraints.java index e20dc083..23d1a11f 100644 --- a/src/main/java/com/team6962/lib/swerve/controller/TrapezoidalConstraints.java +++ b/src/main/java/com/team6962/lib/swerve/controller/TrapezoidalConstraints.java @@ -1,83 +1,96 @@ package com.team6962.lib.swerve.controller; public class TrapezoidalConstraints { - private final double maxVelocity; - private final double maxAcceleration; - - public TrapezoidalConstraints(double maxVelocity, double maxAcceleration) { - this.maxVelocity = maxVelocity; - this.maxAcceleration = maxAcceleration; + private final double maxVelocity; + private final double maxAcceleration; + + public TrapezoidalConstraints(double maxVelocity, double maxAcceleration) { + this.maxVelocity = maxVelocity; + this.maxAcceleration = maxAcceleration; + } + + @Override + public String toString() { + return "Trapezoidal{maxVelocity=" + maxVelocity + ", maxAcceleration=" + maxAcceleration + "}"; + } + + public static double computeTrapezoidalProfileTime( + TrapezoidalConstraints constraints, double distance) { + double minimumTrapezoidalProfileDistance = + constraints.maxVelocity / constraints.maxAcceleration; + + if (distance >= minimumTrapezoidalProfileDistance) { + return constraints.maxVelocity / constraints.maxAcceleration + + distance / constraints.maxVelocity; + } else { + return 2 * Math.sqrt(distance / constraints.maxAcceleration); } + } + public static record TrapezoidProfileAndDistance( + TrapezoidalConstraints constraints, double distance) { @Override - public String toString() { - return "Trapezoidal{maxVelocity=" + maxVelocity + ", maxAcceleration=" + maxAcceleration + "}"; + public final String toString() { + return "TrapezoidProfileAndDistance{" + + "constraints=" + + constraints + + ", distance=" + + distance + + '}'; } + } - public static double computeTrapezoidalProfileTime(TrapezoidalConstraints constraints, double distance) { - double minimumTrapezoidalProfileDistance = constraints.maxVelocity / constraints.maxAcceleration; + public static TrapezoidalConstraints scaleTrapezoidalConstraintsToTime( + TrapezoidalConstraints constraints, double time, double distance) { + double trapezoidalMaxVelocity = + distance / (time - constraints.maxVelocity / constraints.maxAcceleration); + double trapezoidalMaxAcceleration = + constraints.maxAcceleration / constraints.maxVelocity * trapezoidalMaxVelocity; - if (distance >= minimumTrapezoidalProfileDistance) { - return constraints.maxVelocity / constraints.maxAcceleration + distance / constraints.maxVelocity; - } else { - return 2 * Math.sqrt(distance / constraints.maxAcceleration); - } - } + double minimumTrapezoidalProfileDistance = trapezoidalMaxVelocity / trapezoidalMaxAcceleration; - public static record TrapezoidProfileAndDistance(TrapezoidalConstraints constraints, double distance) { - @Override - public final String toString() { - return "TrapezoidProfileAndDistance{" + - "constraints=" + constraints + - ", distance=" + distance + - '}'; - } + if (distance >= minimumTrapezoidalProfileDistance) { + return new TrapezoidalConstraints(trapezoidalMaxVelocity, trapezoidalMaxAcceleration); } - public static TrapezoidalConstraints scaleTrapezoidalConstraintsToTime(TrapezoidalConstraints constraints, double time, double distance) { - double trapezoidalMaxVelocity = distance / (time - constraints.maxVelocity / constraints.maxAcceleration); - double trapezoidalMaxAcceleration = constraints.maxAcceleration / constraints.maxVelocity * trapezoidalMaxVelocity; - - double minimumTrapezoidalProfileDistance = trapezoidalMaxVelocity / trapezoidalMaxAcceleration; + double triangularMaxAcceleration = distance / Math.pow(time / 2, 2); + double triangularMaxVelocity = + constraints.maxVelocity / constraints.maxAcceleration * triangularMaxAcceleration; - if (distance >= minimumTrapezoidalProfileDistance) { - return new TrapezoidalConstraints(trapezoidalMaxVelocity, trapezoidalMaxAcceleration); - } + return new TrapezoidalConstraints(triangularMaxVelocity, triangularMaxAcceleration); + } - double triangularMaxAcceleration = distance / Math.pow(time / 2, 2); - double triangularMaxVelocity = constraints.maxVelocity / constraints.maxAcceleration * triangularMaxAcceleration; - - return new TrapezoidalConstraints(triangularMaxVelocity, triangularMaxAcceleration); - } + // TODO: Support initial and exit velocities + public static TrapezoidalConstraints[] matchProfileDurations( + TrapezoidProfileAndDistance... profiles) { + double[] times = new double[profiles.length]; + double targetTime = 0.0; - // TODO: Support initial and exit velocities - public static TrapezoidalConstraints[] matchProfileDurations(TrapezoidProfileAndDistance ...profiles) { - double[] times = new double[profiles.length]; - double targetTime = 0.0; + for (int i = 0; i < profiles.length; i++) { + TrapezoidProfileAndDistance profile = profiles[i]; - for (int i = 0; i < profiles.length; i++) { - TrapezoidProfileAndDistance profile = profiles[i]; + times[i] = computeTrapezoidalProfileTime(profile.constraints, profile.distance); - times[i] = computeTrapezoidalProfileTime(profile.constraints, profile.distance); - - if (targetTime < times[i]) { - targetTime = times[i]; - } - } - - TrapezoidalConstraints[] matchedProfiles = new TrapezoidalConstraints[profiles.length]; + if (targetTime < times[i]) { + targetTime = times[i]; + } + } - for (int i = 0; i < profiles.length; i++) { - double currentTime = times[i]; + TrapezoidalConstraints[] matchedProfiles = new TrapezoidalConstraints[profiles.length]; - if (currentTime < 1e-6) { - matchedProfiles[i] = profiles[i].constraints; - continue; - } + for (int i = 0; i < profiles.length; i++) { + double currentTime = times[i]; - matchedProfiles[i] = scaleTrapezoidalConstraintsToTime(profiles[i].constraints, targetTime, profiles[i].distance); - } + if (currentTime < 1e-6) { + matchedProfiles[i] = profiles[i].constraints; + continue; + } - return matchedProfiles; + matchedProfiles[i] = + scaleTrapezoidalConstraintsToTime( + profiles[i].constraints, targetTime, profiles[i].distance); } + + return matchedProfiles; + } } diff --git a/src/main/java/com/team6962/lib/swerve/movement/PreciseDrivePositionMovement.java b/src/main/java/com/team6962/lib/swerve/movement/PreciseDrivePositionMovement.java index 4135ccee..1530528b 100644 --- a/src/main/java/com/team6962/lib/swerve/movement/PreciseDrivePositionMovement.java +++ b/src/main/java/com/team6962/lib/swerve/movement/PreciseDrivePositionMovement.java @@ -8,7 +8,6 @@ import com.team6962.lib.swerve.SwerveCore; import com.team6962.lib.swerve.module.SwerveModule; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.units.measure.Angle; @@ -120,8 +119,8 @@ public void execute(SwerveCore drivetrain) { Angle targetAngle = targets[i].angle.getMeasure(); module.drive( - SwerveMovement.motionMagicVelocityVoltage.withVelocity(0), - SwerveMovement.motionMagicExpoVoltage.withPosition(targetAngle)); + SwerveMovement.motionMagicVelocityVoltage.withVelocity(0), + SwerveMovement.motionMagicExpoVoltage.withPosition(targetAngle)); } return; diff --git a/src/main/java/com/team6962/lib/swerve/movement/SpeedsMovement.java b/src/main/java/com/team6962/lib/swerve/movement/SpeedsMovement.java index b3178fe0..ccdc55ba 100644 --- a/src/main/java/com/team6962/lib/swerve/movement/SpeedsMovement.java +++ b/src/main/java/com/team6962/lib/swerve/movement/SpeedsMovement.java @@ -2,12 +2,10 @@ import static edu.wpi.first.units.Units.MetersPerSecond; -import com.ctre.phoenix6.controls.NeutralOut; import com.team6962.lib.swerve.SwerveCore; import com.team6962.lib.swerve.module.SwerveModule; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.KinematicsUtils; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -83,14 +81,16 @@ public void setRotation(Rotation2d rotation) { private SwerveModuleState[] getStates(SwerveCore drivetrain) { if (speeds != null) { ChassisSpeeds outputSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - + Translation2d translation = KinematicsUtils.getTranslation(outputSpeeds); - + // TODO: Use data from testing to improve this - translation = translation.rotateBy(Rotation2d.fromRadians(-outputSpeeds.omegaRadiansPerSecond * 0.142)); + translation = + translation.rotateBy(Rotation2d.fromRadians(-outputSpeeds.omegaRadiansPerSecond * 0.142)); outputSpeeds = - new ChassisSpeeds(translation.getX(), translation.getY(), outputSpeeds.omegaRadiansPerSecond); + new ChassisSpeeds( + translation.getX(), translation.getY(), outputSpeeds.omegaRadiansPerSecond); states = drivetrain.getKinematics().toSwerveModuleStates(outputSpeeds); } @@ -117,10 +117,7 @@ public void execute(SwerveCore drivetrain) { for (int i = 0; i < 4; i++) { SwerveModule module = modules[i]; - module.drive( - SwerveMovement.neutralOut, - SwerveMovement.neutralOut - ); + module.drive(SwerveMovement.neutralOut, SwerveMovement.neutralOut); } return; diff --git a/src/main/java/com/team6962/lib/telemetry/Logger.java b/src/main/java/com/team6962/lib/telemetry/Logger.java index 85bee878..9d4ce602 100644 --- a/src/main/java/com/team6962/lib/telemetry/Logger.java +++ b/src/main/java/com/team6962/lib/telemetry/Logger.java @@ -8,18 +8,10 @@ import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Seconds; -import java.util.Arrays; -import java.util.Collection; -import java.util.LinkedList; -import java.util.List; -import java.util.Map; -import java.util.function.Supplier; - import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import com.studica.frc.AHRS; import com.team6962.lib.digitalsensor.DigitalSensor; - import edu.wpi.first.hal.PowerDistributionFaults; import edu.wpi.first.hal.can.CANStatus; import edu.wpi.first.math.geometry.Pose2d; @@ -50,6 +42,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants.ENABLED_SYSTEMS; +import java.util.Arrays; +import java.util.Collection; +import java.util.LinkedList; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; public class Logger extends SubsystemBase { private static NetworkTable table = NetworkTableInstance.getDefault().getTable("Logs"); diff --git a/src/main/java/com/team6962/lib/telemetry/MechanismLogger.java b/src/main/java/com/team6962/lib/telemetry/MechanismLogger.java index b255203f..b29e5b6a 100644 --- a/src/main/java/com/team6962/lib/telemetry/MechanismLogger.java +++ b/src/main/java/com/team6962/lib/telemetry/MechanismLogger.java @@ -3,13 +3,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; -import java.util.HashMap; -import java.util.HashSet; -import java.util.Map; -import java.util.Set; -import java.util.function.Consumer; -import java.util.function.Supplier; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -17,87 +10,102 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Map; +import java.util.Set; +import java.util.function.Consumer; +import java.util.function.Supplier; public final class MechanismLogger { - private static final double ORIGIN_X = 50; - private static final double ORIGIN_Y = 10; - private static final double MAX_Y = 100; - - private static final Mechanism2d mechanism2d = new Mechanism2d(ORIGIN_X * 2, MAX_Y); - private static final Map ligaments = new HashMap<>(); - private static final Set roots = new HashSet<>(); - - public static void start() { - SmartDashboard.putData("Mechanisms", mechanism2d); - } - - public static MechanismRoot2d getRoot(String name, double x, double y) { - MechanismRoot2d root; - if (roots.contains(name)) { - root = mechanism2d.getRoot(name, ORIGIN_X, ORIGIN_Y); // x and y are unused - root.setPosition(x, y); - return root; - } - - return mechanism2d.getRoot(name, x + ORIGIN_X, y + ORIGIN_Y); - } - - public static MechanismRoot2d getRoot(String name) { - return mechanism2d.getRoot(name, ORIGIN_X, ORIGIN_Y); // if root already exists, x and y are unused. Otherwise, - // they will be overriden later. - } - - private static MechanismLigament2d getLigament(String name, Supplier generator, Consumer modifier) { - if (!ligaments.containsKey(name)) { - ligaments.put(name, generator.get()); - } - - return ligaments.get(name); - } - - public static MechanismLigament2d getLigament(String name, double length, double angle, double lineWidth, Color8Bit color) { - return getLigament( - name, - () -> new MechanismLigament2d(name, length, angle, lineWidth, color), - ligament -> { - ligament.setLength(length); - ligament.setAngle(angle); - ligament.setLineWeight(lineWidth); - ligament.setColor(color); - } - ); - } - - public static MechanismLigament2d getLigament(String name, double length, double angle) { - return getLigament( - name, - () -> new MechanismLigament2d(name, length, angle), - ligament -> { - ligament.setLength(length); - ligament.setAngle(angle); - } - ); - } - - public static MechanismLigament2d getLigament(String name) { - return getLigament( - name, - () -> new MechanismLigament2d(name, 0, 0), - ligament -> {} - ); - } - - public static void addDynamicLength(MechanismLigament2d ligament, Supplier lengthSupplier) { - Logger.addUpdate("MechanismLogger/" + ligament.getName(), () -> ligament.setLength(lengthSupplier.get().in(Inches))); - } - - public static void addDynamicAngle(MechanismLigament2d ligament, Supplier angleSupplier) { - Logger.addUpdate("MechanismLogger/" + ligament.getName(), () -> ligament.setAngle(angleSupplier.get().in(Degrees))); + private static final double ORIGIN_X = 50; + private static final double ORIGIN_Y = 10; + private static final double MAX_Y = 100; + + private static final Mechanism2d mechanism2d = new Mechanism2d(ORIGIN_X * 2, MAX_Y); + private static final Map ligaments = new HashMap<>(); + private static final Set roots = new HashSet<>(); + + public static void start() { + SmartDashboard.putData("Mechanisms", mechanism2d); + } + + public static MechanismRoot2d getRoot(String name, double x, double y) { + MechanismRoot2d root; + if (roots.contains(name)) { + root = mechanism2d.getRoot(name, ORIGIN_X, ORIGIN_Y); // x and y are unused + root.setPosition(x, y); + return root; } - public static void addDyanmicPosition(MechanismRoot2d root, Supplier xSupplier, Supplier ySupplier) { - Logger.addUpdate("MechanismLogger/" + root.getName(), () -> root.setPosition(xSupplier.get().in(Inches) + ORIGIN_X, ySupplier.get().in(Inches) + ORIGIN_Y)); + return mechanism2d.getRoot(name, x + ORIGIN_X, y + ORIGIN_Y); + } + + public static MechanismRoot2d getRoot(String name) { + return mechanism2d.getRoot( + name, ORIGIN_X, ORIGIN_Y); // if root already exists, x and y are unused. Otherwise, + // they will be overriden later. + } + + private static MechanismLigament2d getLigament( + String name, + Supplier generator, + Consumer modifier) { + if (!ligaments.containsKey(name)) { + ligaments.put(name, generator.get()); } - private MechanismLogger() {} + return ligaments.get(name); + } + + public static MechanismLigament2d getLigament( + String name, double length, double angle, double lineWidth, Color8Bit color) { + return getLigament( + name, + () -> new MechanismLigament2d(name, length, angle, lineWidth, color), + ligament -> { + ligament.setLength(length); + ligament.setAngle(angle); + ligament.setLineWeight(lineWidth); + ligament.setColor(color); + }); + } + + public static MechanismLigament2d getLigament(String name, double length, double angle) { + return getLigament( + name, + () -> new MechanismLigament2d(name, length, angle), + ligament -> { + ligament.setLength(length); + ligament.setAngle(angle); + }); + } + + public static MechanismLigament2d getLigament(String name) { + return getLigament(name, () -> new MechanismLigament2d(name, 0, 0), ligament -> {}); + } + + public static void addDynamicLength( + MechanismLigament2d ligament, Supplier lengthSupplier) { + Logger.addUpdate( + "MechanismLogger/" + ligament.getName(), + () -> ligament.setLength(lengthSupplier.get().in(Inches))); + } + + public static void addDynamicAngle(MechanismLigament2d ligament, Supplier angleSupplier) { + Logger.addUpdate( + "MechanismLogger/" + ligament.getName(), + () -> ligament.setAngle(angleSupplier.get().in(Degrees))); + } + + public static void addDyanmicPosition( + MechanismRoot2d root, Supplier xSupplier, Supplier ySupplier) { + Logger.addUpdate( + "MechanismLogger/" + root.getName(), + () -> + root.setPosition( + xSupplier.get().in(Inches) + ORIGIN_X, ySupplier.get().in(Inches) + ORIGIN_Y)); + } + + private MechanismLogger() {} } diff --git a/src/main/java/com/team6962/lib/utils/CommandUtils.java b/src/main/java/com/team6962/lib/utils/CommandUtils.java index 67d18d59..ab84256e 100644 --- a/src/main/java/com/team6962/lib/utils/CommandUtils.java +++ b/src/main/java/com/team6962/lib/utils/CommandUtils.java @@ -22,10 +22,12 @@ public static Command noneWithRequirements(Subsystem... requirements) { } public static Command annotate(String name, Command command) { - return command.deadlineFor(Commands.startEnd( - () -> System.out.println("start: " + name), - () -> System.out.println("end: " + name) - )).withName(name); + return command + .deadlineFor( + Commands.startEnd( + () -> System.out.println("start: " + name), + () -> System.out.println("end: " + name))) + .withName(name); } public static Command selectByMode(Command realCommand, Command simCommand) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02e2d37f..5f584861 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,21 +3,12 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Milliseconds; -import java.io.InputStream; -import java.util.List; -import java.util.Properties; - import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.swerve.module.SwerveModule; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.telemetry.StatusChecks; - -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -50,7 +41,9 @@ import frc.robot.util.CachedRobotState; import frc.robot.util.RobotEvent; import frc.robot.vision.Algae; -import frc.robot.vision.CoralDetection; +import java.io.InputStream; +import java.util.List; +import java.util.Properties; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -142,20 +135,22 @@ public RobotContainer() { refreshButtonEntry.setBoolean(false); - autoChooser = new AutoChooser(List.of( - new Auto("Nothing", Commands.none()), - new Auto(groundAuto.lollipopAuto(CoralStation.RIGHT, true)), - new Auto(groundAuto.lollipopAuto(CoralStation.LEFT, true)), - new Auto(groundAuto.sideAutonomous(CoralStation.RIGHT)), - new Auto(groundAuto.sideAutonomous(CoralStation.LEFT)), - new Auto(groundAuto.lollipopAuto(CoralStation.RIGHT, false)), - new Auto(groundAuto.lollipopAuto(CoralStation.LEFT, false)), - new Auto(groundAuto.middleAuto(0)), - new Auto(groundAuto.middleAuto(1)), - new Auto(groundAuto.middleAuto(2)), - new Auto("Drive Forward", swerveDrive.drive(new ChassisSpeeds(0.5, 0, 0))), - new Auto("Wheel Size Calibration", swerveDrive.calibrateWheelSize()) - ), "Nothing"); + autoChooser = + new AutoChooser( + List.of( + new Auto("Nothing", Commands.none()), + new Auto(groundAuto.lollipopAuto(CoralStation.RIGHT, true)), + new Auto(groundAuto.lollipopAuto(CoralStation.LEFT, true)), + new Auto(groundAuto.sideAutonomous(CoralStation.RIGHT)), + new Auto(groundAuto.sideAutonomous(CoralStation.LEFT)), + new Auto(groundAuto.lollipopAuto(CoralStation.RIGHT, false)), + new Auto(groundAuto.lollipopAuto(CoralStation.LEFT, false)), + new Auto(groundAuto.middleAuto(0)), + new Auto(groundAuto.middleAuto(1)), + new Auto(groundAuto.middleAuto(2)), + new Auto("Drive Forward", swerveDrive.drive(new ChassisSpeeds(0.5, 0, 0))), + new Auto("Wheel Size Calibration", swerveDrive.calibrateWheelSize())), + "Nothing"); Logger.start(Milliseconds.of(20)); } diff --git a/src/main/java/frc/robot/auto/AutoAlign.java b/src/main/java/frc/robot/auto/AutoAlign.java index 80a4cc83..b46267ba 100644 --- a/src/main/java/frc/robot/auto/AutoAlign.java +++ b/src/main/java/frc/robot/auto/AutoAlign.java @@ -3,13 +3,8 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; -import java.util.Set; -import java.util.function.Function; -import java.util.function.Supplier; - import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,6 +14,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.field.ReefPositioning; +import java.util.Set; +import java.util.function.Function; +import java.util.function.Supplier; public class AutoAlign { private SwerveDrive swerveDrive; @@ -57,7 +55,8 @@ private int getClosestReefPole(Pose2d pose, PolePattern pattern) { return closestPole; } - private int getClosestReefPole(Pose2d pose, Function poseSupplier, PolePattern pattern) { + private int getClosestReefPole( + Pose2d pose, Function poseSupplier, PolePattern pattern) { int closestPole = 0; double closestDistance = Double.MAX_VALUE; @@ -91,14 +90,12 @@ public int getClosestReefFace(Pose2d pose) { public Command alignPole(int pole, boolean endWithinTolerance) { return swerveDrive - .driveTo(ReefPositioning.getCoralPlacePose(pole)) - .until( - () -> - endWithinTolerance - && swerveDrive.isWithinToleranceOf( - ReefPositioning.getCoralPlacePose(pole), - Inches.of(1), - Degrees.of(3))); + .driveTo(ReefPositioning.getCoralPlacePose(pole)) + .until( + () -> + endWithinTolerance + && swerveDrive.isWithinToleranceOf( + ReefPositioning.getCoralPlacePose(pole), Inches.of(1), Degrees.of(3))); } public Command alignL1(int pole, boolean endWithinTolerance) { @@ -118,14 +115,12 @@ public Command alignL1(int pole, boolean endWithinTolerance) { public Command alignFace(int face, boolean endWithinTolerance) { return swerveDrive - .driveTo(ReefPositioning.getAlgaePickupPose(face)) - .until( - () -> - endWithinTolerance - && swerveDrive.isWithinToleranceOf( - ReefPositioning.getAlgaePickupPose(face), - Inches.of(1), - Degrees.of(3))); + .driveTo(ReefPositioning.getAlgaePickupPose(face)) + .until( + () -> + endWithinTolerance + && swerveDrive.isWithinToleranceOf( + ReefPositioning.getAlgaePickupPose(face), Inches.of(1), Degrees.of(3))); } public Command alignToClosestPoleTeleop(PolePattern pattern, Supplier rumble) { @@ -148,7 +143,9 @@ public Command alignToClosestPoleTeleop(PolePattern pattern, Supplier r public Command alignToClosestL1Teleop(PolePattern pattern, Supplier rumble) { return Commands.defer( () -> { - int pole = getClosestReefPole(swerveDrive.getEstimatedPose(), ReefPositioning::getL1PlacePose, pattern); + int pole = + getClosestReefPole( + swerveDrive.getEstimatedPose(), ReefPositioning::getL1PlacePose, pattern); Pose2d polePose = ReefPositioning.getL1PlacePose(pole); return alignL1(pole, false) @@ -173,38 +170,84 @@ public Command alignToClosestFaceTeleop() { private static double MAX_BARGE_Y = 7.5; public Command autoAlignBarge() { - return Commands.defer(() -> { - Pose2d closestBargePose = new Pose2d(BARGE_X, MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), Rotation2d.fromDegrees(0)); - - return swerveDrive.driveTo(closestBargePose).until(() -> swerveDrive.isWithinToleranceOf(closestBargePose, Inches.of(4), Degrees.of(6))); - }, Set.of(swerveDrive.useMotion())); + return Commands.defer( + () -> { + Pose2d closestBargePose = + new Pose2d( + BARGE_X, + MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), + Rotation2d.fromDegrees(0)); + + return swerveDrive + .driveTo(closestBargePose) + .until( + () -> + swerveDrive.isWithinToleranceOf( + closestBargePose, Inches.of(4), Degrees.of(6))); + }, + Set.of(swerveDrive.useMotion())); } public Command autoAlignSetupBarge() { - return Commands.defer(() -> { - Angle currentRotation = swerveDrive.getEstimatedPose().getRotation().getMeasure(); - Angle rotationDifference = MeasureMath.minDifference(Degrees.of(0), currentRotation); - Angle clampedDifference = MeasureMath.clamp(rotationDifference, Degrees.of(-60), Degrees.of(60)); - Rotation2d targetRotation = new Rotation2d(currentRotation.plus(clampedDifference)); - - Pose2d closestBargePose = new Pose2d(BARGE_X - 0.5, MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), targetRotation); - - return swerveDrive.driveQuicklyTo(closestBargePose).until(() -> swerveDrive.isWithinToleranceOf(closestBargePose, Inches.of(4), Degrees.of(6))); - }, Set.of(swerveDrive.useMotion())) - .andThen( - Commands.defer(() -> { - Pose2d closestBargePose = new Pose2d(BARGE_X - 0.5, MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), Rotation2d.fromDegrees(0)); - - return swerveDrive.driveQuicklyTo(closestBargePose).until(() -> swerveDrive.isWithinToleranceOf(closestBargePose, Inches.of(4), Degrees.of(6))); - }, Set.of(swerveDrive.useMotion())) - ); + return Commands.defer( + () -> { + Angle currentRotation = swerveDrive.getEstimatedPose().getRotation().getMeasure(); + Angle rotationDifference = MeasureMath.minDifference(Degrees.of(0), currentRotation); + Angle clampedDifference = + MeasureMath.clamp(rotationDifference, Degrees.of(-60), Degrees.of(60)); + Rotation2d targetRotation = new Rotation2d(currentRotation.plus(clampedDifference)); + + Pose2d closestBargePose = + new Pose2d( + BARGE_X - 0.5, + MathUtil.clamp( + swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), + targetRotation); + + return swerveDrive + .driveQuicklyTo(closestBargePose) + .until( + () -> + swerveDrive.isWithinToleranceOf( + closestBargePose, Inches.of(4), Degrees.of(6))); + }, + Set.of(swerveDrive.useMotion())) + .andThen( + Commands.defer( + () -> { + Pose2d closestBargePose = + new Pose2d( + BARGE_X - 0.5, + MathUtil.clamp( + swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), + Rotation2d.fromDegrees(0)); + + return swerveDrive + .driveQuicklyTo(closestBargePose) + .until( + () -> + swerveDrive.isWithinToleranceOf( + closestBargePose, Inches.of(4), Degrees.of(6))); + }, + Set.of(swerveDrive.useMotion()))); } public Command autoAlignBargeFast() { - return Commands.defer(() -> { - Pose2d closestBargePose = new Pose2d(BARGE_X, MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), Rotation2d.fromDegrees(0)); - - return swerveDrive.driveQuicklyTo(closestBargePose).until(() -> swerveDrive.isWithinToleranceOf(closestBargePose, Inches.of(8), Degrees.of(10))); - }, Set.of(swerveDrive.useMotion())); + return Commands.defer( + () -> { + Pose2d closestBargePose = + new Pose2d( + BARGE_X, + MathUtil.clamp(swerveDrive.getEstimatedPose().getY(), MIN_BARGE_Y, MAX_BARGE_Y), + Rotation2d.fromDegrees(0)); + + return swerveDrive + .driveQuicklyTo(closestBargePose) + .until( + () -> + swerveDrive.isWithinToleranceOf( + closestBargePose, Inches.of(8), Degrees.of(10))); + }, + Set.of(swerveDrive.useMotion())); } } diff --git a/src/main/java/frc/robot/auto/AutoChooser.java b/src/main/java/frc/robot/auto/AutoChooser.java index 81358254..7db1192e 100644 --- a/src/main/java/frc/robot/auto/AutoChooser.java +++ b/src/main/java/frc/robot/auto/AutoChooser.java @@ -1,102 +1,105 @@ package frc.robot.auto; -import java.util.List; -import java.util.Map; -import java.util.function.Supplier; -import java.util.stream.Collectors; - import com.team6962.lib.telemetry.Logger; - import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; +import java.util.stream.Collectors; public class AutoChooser extends SubsystemBase { - private Map autos; - private SendableChooser sendableChooser; - private String defaultAuto; + private Map autos; + private SendableChooser sendableChooser; + private String defaultAuto; - private static Auto BACKUP_AUTO = new Auto("Backup Auto", Commands.print("No autonomous available. Selected and default autos do not exist.")); + private static Auto BACKUP_AUTO = + new Auto( + "Backup Auto", + Commands.print("No autonomous available. Selected and default autos do not exist.")); - private Auto preparedAuto; + private Auto preparedAuto; - public AutoChooser(List autosList, String defaultAuto) { - this.autos = autosList.stream().collect(Collectors.toMap(Auto::getName, auto -> auto)); - this.defaultAuto = defaultAuto; + public AutoChooser(List autosList, String defaultAuto) { + this.autos = autosList.stream().collect(Collectors.toMap(Auto::getName, auto -> auto)); + this.defaultAuto = defaultAuto; - sendableChooser = new SendableChooser<>(); - sendableChooser.setDefaultOption(defaultAuto, defaultAuto); + sendableChooser = new SendableChooser<>(); + sendableChooser.setDefaultOption(defaultAuto, defaultAuto); - for (String autoName : autos.keySet()) { - sendableChooser.addOption(autoName, autoName); - } + for (String autoName : autos.keySet()) { + sendableChooser.addOption(autoName, autoName); + } - SmartDashboard.putData("Autonomous", sendableChooser); + SmartDashboard.putData("Autonomous", sendableChooser); - Logger.logString("AutoChooser/selectedAuto", () -> sendableChooser.getSelected()); - Logger.logString("AutoChooser/defaultAuto", () -> defaultAuto); - Logger.logBoolean("AutoChooser/selectedAutoExists", () -> autos.containsKey(sendableChooser.getSelected())); - Logger.logBoolean("AutoChooser/defaultAutoExists", () -> autos.containsKey(defaultAuto)); + Logger.logString("AutoChooser/selectedAuto", () -> sendableChooser.getSelected()); + Logger.logString("AutoChooser/defaultAuto", () -> defaultAuto); + Logger.logBoolean( + "AutoChooser/selectedAutoExists", () -> autos.containsKey(sendableChooser.getSelected())); + Logger.logBoolean("AutoChooser/defaultAutoExists", () -> autos.containsKey(defaultAuto)); - prepareAuto(); + prepareAuto(); + } + + @Override + public void periodic() { + prepareAuto(); + } + + private void prepareAuto() { + if (preparedAuto != null && preparedAuto.getName().equals(sendableChooser.getSelected())) + return; + + String autoName = sendableChooser.getSelected(); + Auto auto = autos.getOrDefault(autoName, autos.getOrDefault(defaultAuto, BACKUP_AUTO)); + + preparedAuto = auto.prepare(); + } + + public Command getAutonomousCommand() { + prepareAuto(); + + return preparedAuto.getCommand(); + } + + public static class Auto { + private String name; + private Supplier commandSupplier; + private boolean allowPrepare = true; + + public Auto(String name, Supplier commandSupplier) { + this.name = name; + this.commandSupplier = commandSupplier; } - @Override - public void periodic() { - prepareAuto(); + public Auto(String name, Command command) { + this(name, () -> command); } - private void prepareAuto() { - if (preparedAuto != null && preparedAuto.getName().equals(sendableChooser.getSelected())) return; + public Auto(Command command) { + this(command.getName(), () -> command); + } - String autoName = sendableChooser.getSelected(); - Auto auto = autos.getOrDefault(autoName, autos.getOrDefault(defaultAuto, BACKUP_AUTO)); + public String getName() { + return name; + } - preparedAuto = auto.prepare(); + public Command getCommand() { + return commandSupplier.get(); } - public Command getAutonomousCommand() { - prepareAuto(); - - return preparedAuto.getCommand(); + public Auto disallowPrepare() { + allowPrepare = false; + return this; } - public static class Auto { - private String name; - private Supplier commandSupplier; - private boolean allowPrepare = true; - - public Auto(String name, Supplier commandSupplier) { - this.name = name; - this.commandSupplier = commandSupplier; - } - - public Auto(String name, Command command) { - this(name, () -> command); - } - - public Auto(Command command) { - this(command.getName(), () -> command); - } - - public String getName() { - return name; - } - - public Command getCommand() { - return commandSupplier.get(); - } - - public Auto disallowPrepare() { - allowPrepare = false; - return this; - } - - public Auto prepare() { - if (allowPrepare) return new Auto(name, commandSupplier.get()); - else return this; - } + public Auto prepare() { + if (allowPrepare) return new Auto(name, commandSupplier.get()); + else return this; } + } } diff --git a/src/main/java/frc/robot/auto/AutoPickup.java b/src/main/java/frc/robot/auto/AutoPickup.java index d0f534d7..5cc29c24 100644 --- a/src/main/java/frc/robot/auto/AutoPickup.java +++ b/src/main/java/frc/robot/auto/AutoPickup.java @@ -3,12 +3,9 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; -import java.util.function.Supplier; - import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.swerve.controller.LineFollowing.Vector2; import com.team6962.lib.utils.KinematicsUtils; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -17,104 +14,115 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.XBoxSwerve; import frc.robot.vision.CoralDetection; +import java.util.function.Supplier; public class AutoPickup { - private SwerveDrive swerveDrive; - private XBoxSwerve swerveController; - private CoralDetection coralDetection; - - public AutoPickup(SwerveDrive swerveDrive, XBoxSwerve swerveController, CoralDetection coralDetection) { - this.swerveDrive = swerveDrive; - this.swerveController = swerveController; - this.coralDetection = coralDetection; - } - - public Command driftToCoral() { - return driftToTarget( - swerveDrive, swerveController, - () -> { - Translation2d coralTranslation = coralDetection.getCoralLocation(); - - return new Pose2d(coralTranslation, coralTranslation.minus(swerveDrive.getEstimatedPose().getTranslation()).getAngle()); - }, - 1.0 - ); - } - - private boolean isTargetReachable(Translation2d target) { - return isTargetReachable(swerveDrive.getEstimatedPose(), swerveDrive.getEstimatedSpeeds(), target); - } - - private Command driftToTarget(SwerveDrive drive, XBoxSwerve controller, Supplier target, double strength) { - return Commands.parallel( + private SwerveDrive swerveDrive; + private XBoxSwerve swerveController; + private CoralDetection coralDetection; + + public AutoPickup( + SwerveDrive swerveDrive, XBoxSwerve swerveController, CoralDetection coralDetection) { + this.swerveDrive = swerveDrive; + this.swerveController = swerveController; + this.coralDetection = coralDetection; + } + + public Command driftToCoral() { + return driftToTarget( + swerveDrive, + swerveController, + () -> { + Translation2d coralTranslation = coralDetection.getCoralLocation(); + + return new Pose2d( + coralTranslation, + coralTranslation.minus(swerveDrive.getEstimatedPose().getTranslation()).getAngle()); + }, + 1.0); + } + + private boolean isTargetReachable(Translation2d target) { + return isTargetReachable( + swerveDrive.getEstimatedPose(), swerveDrive.getEstimatedSpeeds(), target); + } + + private Command driftToTarget( + SwerveDrive drive, XBoxSwerve controller, Supplier target, double strength) { + return Commands.parallel( drive.driveHeading(() -> target.get().getRotation()), - controller.modifySpeeds(speeds -> alterSpeedsToReachTarget( - speeds, - drive.getEstimatedPose().getTranslation(), - target.get().getTranslation(), - strength - )) - ) - .onlyIf(() -> isTargetReachable(target.get().getTranslation())) - .onlyWhile(() -> isTargetReachable(target.get().getTranslation())) - .repeatedly(); - } - - public static Pose2d getApproachWaypoint(Pose2d robotPose, Translation2d finalTarget, double approachDistance) { - Vector2 targetToRobot = new Vector2(robotPose.getTranslation().minus(finalTarget)).unit(); - Vector2 targetToApproachWaypoint = targetToRobot.times(approachDistance); - Vector2 approachWaypoint = new Vector2(finalTarget).plus(targetToApproachWaypoint); - - return approachWaypoint.toPose2d(targetToRobot.negate().toTranslation2d().getAngle()); - } - - public static Pose2d getPickupWaypoint(Pose2d robotPose, Translation2d finalTarget) { - return getApproachWaypoint(robotPose, finalTarget, 0); - } - - public static Translation2d alterTranslationalVelocityToReachTarget(Translation2d translationalVelocity, Translation2d relativeTargetPosition, double strength) { - Vector2 robotVelocity = new Vector2(translationalVelocity); - Vector2 target = new Vector2(relativeTargetPosition); - Vector2 robotVelocityUnit = robotVelocity.unit(); - Vector2 robotVelocityUnit90CCW = robotVelocity.rotate90CCW(); - - Vector2 targetDirectionInMovingDirection = new Vector2( - target.dot(robotVelocityUnit), - target.dot(robotVelocityUnit90CCW) - ).unit(); - - Vector2 velocityToTargetInMovingDirection = targetDirectionInMovingDirection.times(robotVelocity.magnitude() / targetDirectionInMovingDirection.x); - - Vector2 velocityToTarget = robotVelocityUnit.times(velocityToTargetInMovingDirection.x) + controller.modifySpeeds( + speeds -> + alterSpeedsToReachTarget( + speeds, + drive.getEstimatedPose().getTranslation(), + target.get().getTranslation(), + strength))) + .onlyIf(() -> isTargetReachable(target.get().getTranslation())) + .onlyWhile(() -> isTargetReachable(target.get().getTranslation())) + .repeatedly(); + } + + public static Pose2d getApproachWaypoint( + Pose2d robotPose, Translation2d finalTarget, double approachDistance) { + Vector2 targetToRobot = new Vector2(robotPose.getTranslation().minus(finalTarget)).unit(); + Vector2 targetToApproachWaypoint = targetToRobot.times(approachDistance); + Vector2 approachWaypoint = new Vector2(finalTarget).plus(targetToApproachWaypoint); + + return approachWaypoint.toPose2d(targetToRobot.negate().toTranslation2d().getAngle()); + } + + public static Pose2d getPickupWaypoint(Pose2d robotPose, Translation2d finalTarget) { + return getApproachWaypoint(robotPose, finalTarget, 0); + } + + public static Translation2d alterTranslationalVelocityToReachTarget( + Translation2d translationalVelocity, Translation2d relativeTargetPosition, double strength) { + Vector2 robotVelocity = new Vector2(translationalVelocity); + Vector2 target = new Vector2(relativeTargetPosition); + Vector2 robotVelocityUnit = robotVelocity.unit(); + Vector2 robotVelocityUnit90CCW = robotVelocity.rotate90CCW(); + + Vector2 targetDirectionInMovingDirection = + new Vector2(target.dot(robotVelocityUnit), target.dot(robotVelocityUnit90CCW)).unit(); + + Vector2 velocityToTargetInMovingDirection = + targetDirectionInMovingDirection.times( + robotVelocity.magnitude() / targetDirectionInMovingDirection.x); + + Vector2 velocityToTarget = + robotVelocityUnit + .times(velocityToTargetInMovingDirection.x) .plus(robotVelocityUnit90CCW.times(velocityToTargetInMovingDirection.y)); - - Vector2 adjustedVelocity = velocityToTarget.times(strength).plus(robotVelocity.times(1 - strength)); - - return new Translation2d(adjustedVelocity.x, adjustedVelocity.y); - } - - public static ChassisSpeeds alterSpeedsToReachTarget(ChassisSpeeds speeds, Translation2d robotPose, Translation2d target, double strength) { - return KinematicsUtils.setTranslationSpeed( - speeds, - alterTranslationalVelocityToReachTarget( - KinematicsUtils.getTranslation(speeds), - target.minus(robotPose), - strength - ) - ); - } - - public static Angle maxAngularDifferenceForPickup = Degrees.of(30); - private static double maxAngularDifferenceForPickupCos = Math.cos(maxAngularDifferenceForPickup.in(Radians)); - - public static boolean isTargetReachable(Pose2d robotPose, ChassisSpeeds robotSpeeds, Translation2d target) { - Vector2 robotForward = new Vector2(robotPose.getRotation()); - Vector2 toTarget = new Vector2(target.minus(robotPose.getTranslation())).unit(); - Vector2 robotVelocity = new Vector2(robotSpeeds); - - double cosBetweenForwardAndTarget = robotForward.dot(toTarget); - double cosBetweenVelocityAndTarget = robotVelocity.unit().dot(toTarget); - - return cosBetweenForwardAndTarget > maxAngularDifferenceForPickupCos && cosBetweenVelocityAndTarget > 0; - } + + Vector2 adjustedVelocity = + velocityToTarget.times(strength).plus(robotVelocity.times(1 - strength)); + + return new Translation2d(adjustedVelocity.x, adjustedVelocity.y); + } + + public static ChassisSpeeds alterSpeedsToReachTarget( + ChassisSpeeds speeds, Translation2d robotPose, Translation2d target, double strength) { + return KinematicsUtils.setTranslationSpeed( + speeds, + alterTranslationalVelocityToReachTarget( + KinematicsUtils.getTranslation(speeds), target.minus(robotPose), strength)); + } + + public static Angle maxAngularDifferenceForPickup = Degrees.of(30); + private static double maxAngularDifferenceForPickupCos = + Math.cos(maxAngularDifferenceForPickup.in(Radians)); + + public static boolean isTargetReachable( + Pose2d robotPose, ChassisSpeeds robotSpeeds, Translation2d target) { + Vector2 robotForward = new Vector2(robotPose.getRotation()); + Vector2 toTarget = new Vector2(target.minus(robotPose.getTranslation())).unit(); + Vector2 robotVelocity = new Vector2(robotSpeeds); + + double cosBetweenForwardAndTarget = robotForward.dot(toTarget); + double cosBetweenVelocityAndTarget = robotVelocity.unit().dot(toTarget); + + return cosBetweenForwardAndTarget > maxAngularDifferenceForPickupCos + && cosBetweenVelocityAndTarget > 0; + } } diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index a8123b00..87b84893 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -6,7 +6,6 @@ import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.utils.CommandUtils; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Time; @@ -114,14 +113,14 @@ public Command createSideAutonomous(Side side, boolean startInMiddle) { public Command createMiddleAutonomous() { return Commands.sequence( Commands.waitSeconds(1), - CommandUtils.annotate("middle-coral-place", swerveDrive.followChoreoPath("middle-coral-place")) + CommandUtils.annotate( + "middle-coral-place", swerveDrive.followChoreoPath("middle-coral-place")) .deadlineFor(elevator.ready().repeatedly()), CommandUtils.annotate("place coral", placeCoral(new CoralPosition(0, 4))), - CommandUtils.annotate( - "safe manipulator pivot", - manipulator.pivot.safe()), + CommandUtils.annotate("safe manipulator pivot", manipulator.pivot.safe()), CommandUtils.annotate("pickup algae", pickupAlgae(0)), - CommandUtils.annotate("middle-algae-place", swerveDrive.followChoreoPath("middle-algae-place")), + CommandUtils.annotate( + "middle-algae-place", swerveDrive.followChoreoPath("middle-algae-place")), CommandUtils.annotate( "align to barge", swerveDrive @@ -163,27 +162,22 @@ private Command pickupAlgae(int face) { alignPose, Inches.of(2), Degrees.of(6)))), CommandUtils.annotate( "elevator to algae l2", - CommandUtils.selectByMode( - elevator.algaeL2(), - Commands.waitSeconds(0.5)))), + CommandUtils.selectByMode(elevator.algaeL2(), Commands.waitSeconds(0.5)))), CommandUtils.annotate( "manipulator to algae l2", Commands.deadline( - CommandUtils.selectByMode(manipulator.pivot.algaeReef(), Commands.waitSeconds(0.2)))), + CommandUtils.selectByMode( + manipulator.pivot.algaeReef(), Commands.waitSeconds(0.2)))), CommandUtils.annotate( "twist to pickup", Commands.deadline( CommandUtils.selectByMode( manipulator.grabber.intakeAlgae(), Commands.waitSeconds(0.5)), - swerveDrive.drivePreciselyTo(pickupPose) - ))); + swerveDrive.drivePreciselyTo(pickupPose)))); } private Command intakeThenRaiseElevator() { - return Commands.sequence( - pieceCombos.intakeCoral(), - elevator.ready() - ); + return Commands.sequence(pieceCombos.intakeCoral(), elevator.ready()); } public Command placeCoral(CoralPosition position) { @@ -239,11 +233,7 @@ public Command placeCoral(CoralPosition position) { CommandUtils.annotate("reposition coral", manipulator.grabber.repositionCoral()), // Drop the coral while keeping the elevator and manipulator // in place. - CommandUtils.annotate( - "drop coral", - manipulator - .grabber - .dropCoral()), + CommandUtils.annotate("drop coral", manipulator.grabber.dropCoral()), // Stow the pivot CommandUtils.annotate( "stow pivot", diff --git a/src/main/java/frc/robot/auto/GroundAuto.java b/src/main/java/frc/robot/auto/GroundAuto.java index 3a1e99aa..00b7ccde 100644 --- a/src/main/java/frc/robot/auto/GroundAuto.java +++ b/src/main/java/frc/robot/auto/GroundAuto.java @@ -4,12 +4,8 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.MetersPerSecond; -import java.util.Set; -import java.util.function.BooleanSupplier; - import com.team6962.lib.utils.CommandUtils; import com.team6962.lib.utils.KinematicsUtils; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,122 +22,179 @@ import frc.robot.field.StationPositioning; import frc.robot.field.StationPositioning.CoralStation; import frc.robot.subsystems.intake.IntakeSensors.CoralLocation; +import java.util.Set; +import java.util.function.BooleanSupplier; public class GroundAuto { - private RobotContainer robot; + private RobotContainer robot; - public GroundAuto(RobotContainer robot) { - this.robot = robot; + public GroundAuto(RobotContainer robot) { + this.robot = robot; + } + + public Command readyMechanismsForScoreCoral(int level, boolean fast) { + if (level <= 1 || level > 4) { + throw new IllegalArgumentException("Level must be between 2 and 4"); } - public Command readyMechanismsForScoreCoral(int level, boolean fast) { - if (level <= 1 || level > 4) { - throw new IllegalArgumentException("Level must be between 2 and 4"); - } + if (RobotBase.isSimulation()) { + return Commands.waitSeconds(0.5); + } - if (RobotBase.isSimulation()) { - return Commands.waitSeconds(0.5); - } + Command elevatorCommand = + fast && level >= 3 ? robot.elevator.ready() : robot.elevator.coral(level); + Command pivotCommand = + level == 4 ? robot.manipulator.pivot.safe() : robot.manipulator.placeCoralL23(); - Command elevatorCommand = fast && level >= 3 ? robot.elevator.ready() : robot.elevator.coral(level); - Command pivotCommand = level == 4 ? robot.manipulator.pivot.safe() : robot.manipulator.placeCoralL23(); + return CommandUtils.annotate( + "Ready coral for L" + level, Commands.parallel(elevatorCommand, pivotCommand)); + } - return CommandUtils.annotate("Ready coral for L" + level, Commands.parallel(elevatorCommand, pivotCommand)); + public Command scoreCoralWithMechanisms(int level) { + if (level <= 1 || level > 4) { + throw new IllegalArgumentException("Level must be between 2 and 4"); } - public Command scoreCoralWithMechanisms(int level) { - if (level <= 1 || level > 4) { - throw new IllegalArgumentException("Level must be between 2 and 4"); - } - - if (RobotBase.isSimulation()) { - return Commands.waitSeconds(0.5); - } - - if (level == 4) { - return Commands.sequence( - robot.elevator.coralL4(), - RobotBase.isReal() ? robot.manipulator.placeCoralL4() : Commands.none(), - robot.manipulator.grabber.dropCoral() - ); - } else { - return Commands.sequence( - Commands.parallel( - robot.elevator.coral(level), - RobotBase.isReal() ? robot.manipulator.placeCoralL23() : Commands.none() - ), - robot.manipulator.grabber.dropCoral() - ); - } + if (RobotBase.isSimulation()) { + return Commands.waitSeconds(0.5); } - public Command scorePreloadCoral(CoralPosition position, boolean scoreL4) { - return CommandUtils.annotate("Score coral on " + position, Commands.deadline( - Commands.sequence( - CommandUtils.annotate("Ready for score on L" + position.level, readyMechanismsForScoreCoral(position.level, !scoreL4)), - CommandUtils.annotate("Wait until aligned", Commands.waitUntil( - () -> robot.swerveDrive.isWithinToleranceOf(ReefPositioning.getCoralPlacePose(position.pole), - Inches.of(1), - Degrees.of(3) - ))), - CommandUtils.annotate("Score coral on L" + position.level, scoreCoralWithMechanisms(position.level)) - ), - CommandUtils.annotate("Align to " + position, robot.autoAlign.alignPole(position.pole, false)) - ).deadlineFor(robot.intake.pivot.deploy())); + if (level == 4) { + return Commands.sequence( + robot.elevator.coralL4(), + RobotBase.isReal() ? robot.manipulator.placeCoralL4() : Commands.none(), + robot.manipulator.grabber.dropCoral()); + } else { + return Commands.sequence( + Commands.parallel( + robot.elevator.coral(level), + RobotBase.isReal() ? robot.manipulator.placeCoralL23() : Commands.none()), + robot.manipulator.grabber.dropCoral()); } - - public Command scoreCoral(CoralPosition position) { - return CommandUtils.annotate("Score coral on " + position, Commands.deadline( + } + + public Command scorePreloadCoral(CoralPosition position, boolean scoreL4) { + return CommandUtils.annotate( + "Score coral on " + position, + Commands.deadline( + Commands.sequence( + CommandUtils.annotate( + "Ready for score on L" + position.level, + readyMechanismsForScoreCoral(position.level, !scoreL4)), + CommandUtils.annotate( + "Wait until aligned", + Commands.waitUntil( + () -> + robot.swerveDrive.isWithinToleranceOf( + ReefPositioning.getCoralPlacePose(position.pole), + Inches.of(1), + Degrees.of(3)))), + CommandUtils.annotate( + "Score coral on L" + position.level, + scoreCoralWithMechanisms(position.level))), + CommandUtils.annotate( + "Align to " + position, robot.autoAlign.alignPole(position.pole, false))) + .deadlineFor(robot.intake.pivot.deploy())); + } + + public Command scoreCoral(CoralPosition position) { + return CommandUtils.annotate( + "Score coral on " + position, + Commands.deadline( Commands.sequence( - CommandUtils.annotate("Intake transfer", IntakeCommands.intakeTransferAuto(robot.intake, robot.elevator, robot.manipulator, robot.safeties, robot.pieceCombos)), - CommandUtils.annotate("Ready for score on L" + position.level, readyMechanismsForScoreCoral(position.level, false).alongWith( - CommandUtils.annotate("Reposition coral", robot.manipulator.grabber.repositionCoral()) - )), - CommandUtils.annotate("Wait until aligned", Commands.waitUntil( - () -> robot.swerveDrive.isWithinToleranceOf(ReefPositioning.getCoralPlacePose(position.pole), - Inches.of(1.5), - Degrees.of(3) - ))), - CommandUtils.annotate("Score coral on L" + position.level, scoreCoralWithMechanisms(position.level)) - ), - CommandUtils.annotate("Align to " + position, robot.autoAlign.alignPole(position.pole, false)) - )); - } + CommandUtils.annotate( + "Intake transfer", + IntakeCommands.intakeTransferAuto( + robot.intake, + robot.elevator, + robot.manipulator, + robot.safeties, + robot.pieceCombos)), + CommandUtils.annotate( + "Ready for score on L" + position.level, + readyMechanismsForScoreCoral(position.level, false) + .alongWith( + CommandUtils.annotate( + "Reposition coral", robot.manipulator.grabber.repositionCoral()))), + CommandUtils.annotate( + "Wait until aligned", + Commands.waitUntil( + () -> + robot.swerveDrive.isWithinToleranceOf( + ReefPositioning.getCoralPlacePose(position.pole), + Inches.of(1.5), + Degrees.of(3)))), + CommandUtils.annotate( + "Score coral on L" + position.level, scoreCoralWithMechanisms(position.level))), + CommandUtils.annotate( + "Align to " + position, robot.autoAlign.alignPole(position.pole, false)))); + } - public Command intakeCoralFromStation(CoralStation coralStation, boolean approachSlow) { - return CommandUtils.annotate("Intake coral from " + coralStation + " coral station", Commands.deadline( + public Command intakeCoralFromStation(CoralStation coralStation, boolean approachSlow) { + return CommandUtils.annotate( + "Intake coral from " + coralStation + " coral station", + Commands.deadline( CommandUtils.selectByMode( - Commands.waitUntil(() -> robot.intake.sensors.getCoralLocation() == CoralLocation.INTAKE), - Commands.waitUntil(() -> robot.swerveDrive.isWithinToleranceOf(StationPositioning.getGroundStartIntakePose(coralStation), Inches.of(12), Degrees.of(30))) - .andThen(Commands.defer(() -> Commands.waitSeconds(Math.pow(Math.random(), 1.5) * Math.signum(Math.random() - 0.5) + 1.25), Set.of())) - ), - CommandUtils.annotate("Intake coral", IntakeCommands.intakeCoral(robot.intake, robot.elevator, robot.manipulator, robot.safeties, robot.pieceCombos)), + Commands.waitUntil( + () -> robot.intake.sensors.getCoralLocation() == CoralLocation.INTAKE), + Commands.waitUntil( + () -> + robot.swerveDrive.isWithinToleranceOf( + StationPositioning.getGroundStartIntakePose(coralStation), + Inches.of(12), + Degrees.of(30))) + .andThen( + Commands.defer( + () -> + Commands.waitSeconds( + Math.pow(Math.random(), 1.5) * Math.signum(Math.random() - 0.5) + + 1.25), + Set.of()))), + CommandUtils.annotate( + "Intake coral", + IntakeCommands.intakeCoral( + robot.intake, + robot.elevator, + robot.manipulator, + robot.safeties, + robot.pieceCombos)), CommandUtils.annotate( - "Drive over to intake from " + coralStation + " coral station", - approachSlow ? robot.swerveDrive.driveQuicklyTo(StationPositioning.getGroundStartIntakePose(coralStation), MetersPerSecond.of(2)) : - robot.swerveDrive.driveQuicklyTo(StationPositioning.getGroundStartIntakePose(coralStation)) - ).andThen(Commands.repeatingSequence( - CommandUtils.annotate( - "Drive beside coral station", - robot.swerveDrive.driveQuicklyTo(StationPositioning.getGroundEndIntakePose(coralStation), MetersPerSecond.of(2)) - ), - CommandUtils.annotate( "Drive over to intake from " + coralStation + " coral station", - robot.swerveDrive.driveQuicklyTo(StationPositioning.getGroundStartIntakePose(coralStation)) - ) - )) - )); - } - - public Command sideAutonomous(CoralStation coralStation) { - boolean reflect = coralStation == CoralStation.LEFT; - - return CommandUtils.annotate(coralStation + " Side", Commands.sequence( + approachSlow + ? robot.swerveDrive.driveQuicklyTo( + StationPositioning.getGroundStartIntakePose(coralStation), + MetersPerSecond.of(2)) + : robot.swerveDrive.driveQuicklyTo( + StationPositioning.getGroundStartIntakePose(coralStation))) + .andThen( + Commands.repeatingSequence( + CommandUtils.annotate( + "Drive beside coral station", + robot.swerveDrive.driveQuicklyTo( + StationPositioning.getGroundEndIntakePose(coralStation), + MetersPerSecond.of(2))), + CommandUtils.annotate( + "Drive over to intake from " + coralStation + " coral station", + robot.swerveDrive.driveQuicklyTo( + StationPositioning.getGroundStartIntakePose(coralStation))))))); + } + + public Command sideAutonomous(CoralStation coralStation) { + boolean reflect = coralStation == CoralStation.LEFT; + + return CommandUtils.annotate( + coralStation + " Side", + Commands.sequence( scorePreloadCoral(new CoralPosition(10, 4).reflectedIf(reflect), false), - robot.swerveDrive.driveQuicklyTo( - StationPositioning.reflectPose(new Pose2d(4.3, 2.42, Rotation2d.fromDegrees(120)), reflect), - KinematicsUtils.createChassisSpeeds(new Translation2d(2, Rotation2d.fromDegrees(210).times(reflect ? -1 : 1)), new Rotation2d()) - ).deadlineFor(robot.intake.pivot.deploy()), + robot + .swerveDrive + .driveQuicklyTo( + StationPositioning.reflectPose( + new Pose2d(4.3, 2.42, Rotation2d.fromDegrees(120)), reflect), + KinematicsUtils.createChassisSpeeds( + new Translation2d(2, Rotation2d.fromDegrees(210).times(reflect ? -1 : 1)), + new Rotation2d())) + .deadlineFor(robot.intake.pivot.deploy()), intakeCoralFromStation(coralStation, true), scoreCoral(new CoralPosition(7, 4).reflectedIf(reflect)), intakeCoralFromStation(coralStation, false), @@ -149,181 +202,238 @@ public Command sideAutonomous(CoralStation coralStation) { intakeCoralFromStation(coralStation, false), scoreCoral(new CoralPosition(9, 4).reflectedIf(reflect)), intakeCoralFromStation(coralStation, false), - scoreCoral(new CoralPosition(7, 3).reflectedIf(reflect)) - )); - } - - public Command prepareLollipops(CoralStation coralStation, boolean scoreL4) { - Pose2d pose = scoreL4 ? new Pose2d(4.36, 2.02, Rotation2d.fromDegrees(60)) : new Pose2d(2.96, 1.36, Rotation2d.fromDegrees(0)); - ChassisSpeeds speeds = scoreL4 ? new ChassisSpeeds(-1, 0.5, 0) : new ChassisSpeeds(-1.5, 0.75, 0); - - if (coralStation == CoralStation.LEFT) { - pose = new Pose2d(pose.getX(), Field.WIDTH - pose.getY(), pose.getRotation().unaryMinus()); - speeds = new ChassisSpeeds(speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond); - } - - return robot.swerveDrive.driveQuicklyTo(pose, speeds); - } - - public static Translation2d LOLLIPOP_1 = new Translation2d(1.27, 2.2); - public static Translation2d LOLLIPOP_2 = new Translation2d(1.27, 4.025); - public static Translation2d LOLLIPOP_3 = new Translation2d(1.27, 5.85); - - public Command intakeLollipop(int id, boolean finalLollipop) { - return Commands.defer(() -> { - Translation2d currentPosition = robot.swerveDrive.getEstimatedPose().getTranslation(); - - Translation2d lollipopPosition = id == 0 ? LOLLIPOP_1 : id == 1 ? - LOLLIPOP_2 : LOLLIPOP_3; - - Rotation2d angleToLollipop = currentPosition.minus(lollipopPosition).getAngle(); - - Pose2d approachPose = new Pose2d(lollipopPosition.plus(new Translation2d(Units.inchesToMeters(50), angleToLollipop)), angleToLollipop); - Pose2d finalPose = new Pose2d(lollipopPosition.minus(new Translation2d(0.3, angleToLollipop)), angleToLollipop); - - ChassisSpeeds approachSpeeds = new ChassisSpeeds(-0.5 * angleToLollipop.getCos(), -0.5 * angleToLollipop.getSin(), 0); - ChassisSpeeds finalSpeeds = new ChassisSpeeds(0 * angleToLollipop.getCos(), 0 * angleToLollipop.getSin(), 0); - - return Commands.deadline( - robot.swerveDrive.driveQuicklyTo(approachPose, approachSpeeds) - .andThen(robot.swerveDrive.driveQuicklyTo(finalPose, finalSpeeds, MetersPerSecond.of(1.25))) - .andThen(finalLollipop ? Commands.waitUntil(() -> false) : Commands.waitSeconds(1)), - IntakeCommands.intakeTransferAuto(robot.intake, robot.elevator, robot.manipulator, robot.safeties, robot.pieceCombos) - ).until(() -> robot.intake.sensors.getCoralLocation() == CoralLocation.INTAKE); - }, Set.of(robot.intake.indexer, robot.intake.pivot, robot.intake.rollers, robot.elevator, robot.manipulator.grabber, robot.manipulator.pivot, robot.swerveDrive)); - } - - public Command recoverFromLollipopFailure(int failedId, int nextId) { - return Commands.defer(() -> { - // Translation2d failedLollipop = failedId == 0 ? LOLLIPOP_1 : failedId == 1 ? - // LOLLIPOP_2 : LOLLIPOP_3; - - Translation2d nextLollipop = nextId == 0 ? LOLLIPOP_1 : nextId == 1 ? - LOLLIPOP_2 : LOLLIPOP_3; - - Rotation2d angleToNextLollipop = Rotation2d.fromDegrees(-30).times(Math.signum(nextId - failedId)); - - Translation2d setupTranslation = new Translation2d(nextLollipop.getX(), nextLollipop.getY()).plus(new Translation2d(Units.inchesToMeters(50), angleToNextLollipop)); - - Pose2d setupPose = new Pose2d(setupTranslation, angleToNextLollipop); - - ChassisSpeeds setupSpeeds = new ChassisSpeeds(0.5 * angleToNextLollipop.getCos(), 0.5 * angleToNextLollipop.getSin(), 0); - // ChassisSpeeds recoverSpeeds = new ChassisSpeeds(1.5, 2 * Math.signum(nextId - failedId), 0); - - return Commands.parallel( - robot.swerveDrive.driveQuicklyTo(setupPose, setupSpeeds), - CommandUtils.selectByMode( - robot.intake.pivot.stow().andThen(robot.intake.pivot.deploy()), - Commands.waitSeconds(0.5) - ) - ); - }, Set.of(robot.intake.indexer, robot.intake.pivot, robot.intake.rollers, robot.elevator, robot.manipulator.grabber, robot.manipulator.pivot, robot.swerveDrive)); - } - - public BooleanSupplier successfullyIntaked() { - return () -> robot.intake.sensors.getCoralLocation() != CoralLocation.OUTSIDE; - } - - public BooleanSupplier failedToIntake() { - return () -> robot.intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE; + scoreCoral(new CoralPosition(7, 3).reflectedIf(reflect)))); + } + + public Command prepareLollipops(CoralStation coralStation, boolean scoreL4) { + Pose2d pose = + scoreL4 + ? new Pose2d(4.36, 2.02, Rotation2d.fromDegrees(60)) + : new Pose2d(2.96, 1.36, Rotation2d.fromDegrees(0)); + ChassisSpeeds speeds = + scoreL4 ? new ChassisSpeeds(-1, 0.5, 0) : new ChassisSpeeds(-1.5, 0.75, 0); + + if (coralStation == CoralStation.LEFT) { + pose = new Pose2d(pose.getX(), Field.WIDTH - pose.getY(), pose.getRotation().unaryMinus()); + speeds = + new ChassisSpeeds( + speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond); } - public Command lollipopAuto(CoralStation coralStation, boolean scoreL4) { - boolean reflect = coralStation == CoralStation.LEFT; - - int lollipop1 = coralStation == CoralStation.LEFT ? 2 : 0; - int lollipop2 = 1; - int lollipop3 = coralStation == CoralStation.LEFT ? 0 : 2; - - return CommandUtils.annotate(coralStation + " " + (scoreL4 ? "Side & " : "") + "Lollipop", Commands.sequence( + return robot.swerveDrive.driveQuicklyTo(pose, speeds); + } + + public static Translation2d LOLLIPOP_1 = new Translation2d(1.27, 2.2); + public static Translation2d LOLLIPOP_2 = new Translation2d(1.27, 4.025); + public static Translation2d LOLLIPOP_3 = new Translation2d(1.27, 5.85); + + public Command intakeLollipop(int id, boolean finalLollipop) { + return Commands.defer( + () -> { + Translation2d currentPosition = robot.swerveDrive.getEstimatedPose().getTranslation(); + + Translation2d lollipopPosition = id == 0 ? LOLLIPOP_1 : id == 1 ? LOLLIPOP_2 : LOLLIPOP_3; + + Rotation2d angleToLollipop = currentPosition.minus(lollipopPosition).getAngle(); + + Pose2d approachPose = + new Pose2d( + lollipopPosition.plus( + new Translation2d(Units.inchesToMeters(50), angleToLollipop)), + angleToLollipop); + Pose2d finalPose = + new Pose2d( + lollipopPosition.minus(new Translation2d(0.3, angleToLollipop)), angleToLollipop); + + ChassisSpeeds approachSpeeds = + new ChassisSpeeds( + -0.5 * angleToLollipop.getCos(), -0.5 * angleToLollipop.getSin(), 0); + ChassisSpeeds finalSpeeds = + new ChassisSpeeds(0 * angleToLollipop.getCos(), 0 * angleToLollipop.getSin(), 0); + + return Commands.deadline( + robot + .swerveDrive + .driveQuicklyTo(approachPose, approachSpeeds) + .andThen( + robot.swerveDrive.driveQuicklyTo( + finalPose, finalSpeeds, MetersPerSecond.of(1.25))) + .andThen( + finalLollipop + ? Commands.waitUntil(() -> false) + : Commands.waitSeconds(1)), + IntakeCommands.intakeTransferAuto( + robot.intake, + robot.elevator, + robot.manipulator, + robot.safeties, + robot.pieceCombos)) + .until(() -> robot.intake.sensors.getCoralLocation() == CoralLocation.INTAKE); + }, + Set.of( + robot.intake.indexer, + robot.intake.pivot, + robot.intake.rollers, + robot.elevator, + robot.manipulator.grabber, + robot.manipulator.pivot, + robot.swerveDrive)); + } + + public Command recoverFromLollipopFailure(int failedId, int nextId) { + return Commands.defer( + () -> { + // Translation2d failedLollipop = failedId == 0 ? LOLLIPOP_1 : failedId == 1 ? + // LOLLIPOP_2 : LOLLIPOP_3; + + Translation2d nextLollipop = + nextId == 0 ? LOLLIPOP_1 : nextId == 1 ? LOLLIPOP_2 : LOLLIPOP_3; + + Rotation2d angleToNextLollipop = + Rotation2d.fromDegrees(-30).times(Math.signum(nextId - failedId)); + + Translation2d setupTranslation = + new Translation2d(nextLollipop.getX(), nextLollipop.getY()) + .plus(new Translation2d(Units.inchesToMeters(50), angleToNextLollipop)); + + Pose2d setupPose = new Pose2d(setupTranslation, angleToNextLollipop); + + ChassisSpeeds setupSpeeds = + new ChassisSpeeds( + 0.5 * angleToNextLollipop.getCos(), 0.5 * angleToNextLollipop.getSin(), 0); + // ChassisSpeeds recoverSpeeds = new ChassisSpeeds(1.5, 2 * Math.signum(nextId - + // failedId), 0); + + return Commands.parallel( + robot.swerveDrive.driveQuicklyTo(setupPose, setupSpeeds), + CommandUtils.selectByMode( + robot.intake.pivot.stow().andThen(robot.intake.pivot.deploy()), + Commands.waitSeconds(0.5))); + }, + Set.of( + robot.intake.indexer, + robot.intake.pivot, + robot.intake.rollers, + robot.elevator, + robot.manipulator.grabber, + robot.manipulator.pivot, + robot.swerveDrive)); + } + + public BooleanSupplier successfullyIntaked() { + return () -> robot.intake.sensors.getCoralLocation() != CoralLocation.OUTSIDE; + } + + public BooleanSupplier failedToIntake() { + return () -> robot.intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE; + } + + public Command lollipopAuto(CoralStation coralStation, boolean scoreL4) { + boolean reflect = coralStation == CoralStation.LEFT; + + int lollipop1 = coralStation == CoralStation.LEFT ? 2 : 0; + int lollipop2 = 1; + int lollipop3 = coralStation == CoralStation.LEFT ? 0 : 2; + + return CommandUtils.annotate( + coralStation + " " + (scoreL4 ? "Side & " : "") + "Lollipop", + Commands.sequence( prepareLollipops(coralStation, scoreL4), - scorePreloadCoral((scoreL4 ? new CoralPosition(7, 4) : new CoralPosition(6, 4)).reflectedIf(reflect), scoreL4), + scorePreloadCoral( + (scoreL4 ? new CoralPosition(7, 4) : new CoralPosition(6, 4)).reflectedIf(reflect), + scoreL4), intakeLollipop(lollipop1, false), Commands.either( - scoreL4 ? scoreCoral(new CoralPosition(6, 4).reflectedIf(reflect)) : scoreCoral(new CoralPosition(5, 4).reflectedIf(reflect)), + scoreL4 + ? scoreCoral(new CoralPosition(6, 4).reflectedIf(reflect)) + : scoreCoral(new CoralPosition(5, 4).reflectedIf(reflect)), recoverFromLollipopFailure(lollipop1, lollipop2), - successfullyIntaked() - ), + successfullyIntaked()), intakeLollipop(lollipop2, false), Commands.either( - scoreCoral((scoreL4 ? new CoralPosition(5, 4) : new CoralPosition(6, 2)).reflectedIf(reflect)), + scoreCoral( + (scoreL4 ? new CoralPosition(5, 4) : new CoralPosition(6, 2)) + .reflectedIf(reflect)), recoverFromLollipopFailure(lollipop2, lollipop3), - successfullyIntaked() - ), + successfullyIntaked()), intakeLollipop(lollipop3, true), - scoreCoral(new CoralPosition(5, 2).reflectedIf(reflect)).onlyIf(successfullyIntaked()) - )); - } + scoreCoral(new CoralPosition(5, 2).reflectedIf(reflect)) + .onlyIf(successfullyIntaked()))); + } - private Command pickupAlgae(int face) { - int level = ReefPositioning.getAlgaeHeight(face); + private Command pickupAlgae(int face) { + int level = ReefPositioning.getAlgaeHeight(face); - return Commands.sequence( - Commands.deadline( - CommandUtils.selectByMode( - robot.manipulator.pivot.stow().until(() -> robot.manipulator.pivot.getPosition().gt(Degrees.of(-10))), - Commands.waitSeconds(0.5) - ), - robot.swerveDrive - .driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face)), - robot.elevator.algae(level) - ), - Commands.parallel( + return Commands.sequence( + Commands.deadline( + CommandUtils.selectByMode( + robot + .manipulator + .pivot + .stow() + .until(() -> robot.manipulator.pivot.getPosition().gt(Degrees.of(-10))), + Commands.waitSeconds(0.5)), + robot.swerveDrive.driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face)), + robot.elevator.algae(level)), + Commands.parallel( robot.swerveDrive.driveTo(ReefPositioning.getAlgaePickupPose(face)), CommandUtils.selectByMode( Commands.parallel( - robot.pieceCombos.algae(level), - robot.manipulator.grabber.intakeAlgae() - ), - Commands.waitSeconds(0.5) - ) - ).withDeadline(CommandUtils.selectByMode( - Commands.waitUntil(robot.manipulator.grabber::hasAlgae), - Commands.waitSeconds(0.5) - )), - robot.swerveDrive.driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face)).deadlineFor(robot.manipulator.pivot.stow()) - ); - } - - private Command scoreAlgae() { - return Commands.sequence( - robot.autoAlign.autoAlignSetupBarge().deadlineFor(robot.elevator.stow(), robot.manipulator.pivot.stow()), - CommandUtils.selectByMode( - Commands.parallel( - robot.elevator.stow(), - robot.manipulator.pivot.safe() - ), - Commands.waitSeconds(0.1) - ), - CommandUtils.selectByMode( - robot.pieceCombos.stow() - .andThen(robot.elevator.launchBarge()) - .withDeadline(Commands.sequence( + robot.pieceCombos.algae(level), robot.manipulator.grabber.intakeAlgae()), + Commands.waitSeconds(0.5))) + .withDeadline( + CommandUtils.selectByMode( + Commands.waitUntil(robot.manipulator.grabber::hasAlgae), + Commands.waitSeconds(0.5))), + robot + .swerveDrive + .driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face)) + .deadlineFor(robot.manipulator.pivot.stow())); + } + + private Command scoreAlgae() { + return Commands.sequence( + robot + .autoAlign + .autoAlignSetupBarge() + .deadlineFor(robot.elevator.stow(), robot.manipulator.pivot.stow()), + CommandUtils.selectByMode( + Commands.parallel(robot.elevator.stow(), robot.manipulator.pivot.safe()), + Commands.waitSeconds(0.1)), + CommandUtils.selectByMode( + robot + .pieceCombos + .stow() + .andThen(robot.elevator.launchBarge()) + .withDeadline( + Commands.sequence( Commands.waitUntil(() -> robot.elevator.getPosition().gt(Inches.of(52.5))), - robot.manipulator.grabber.dropAlgae().withTimeout(0.5) - )), - Commands.waitSeconds(1) - ) - ); - } - - public Command middleAuto(int algaeCount) { - return CommandUtils.annotate(algaeCount > 0 ? "Middle Coral & " + algaeCount + " Algae" : "Middle Coral", Commands.sequence( + robot.manipulator.grabber.dropAlgae().withTimeout(0.5))), + Commands.waitSeconds(1))); + } + + public Command middleAuto(int algaeCount) { + return CommandUtils.annotate( + algaeCount > 0 ? "Middle Coral & " + algaeCount + " Algae" : "Middle Coral", + Commands.sequence( Commands.either( scorePreloadCoral(new CoralPosition(0, 4), false), scorePreloadCoral(new CoralPosition(11, 4), false), - () -> robot.swerveDrive.getEstimatedPose().getY() > ReefPositioning.REEF_CENTER.getY() - ), - Commands.sequence( - pickupAlgae(0), - scoreAlgae() - ).onlyIf(() -> algaeCount >= 1), + () -> + robot.swerveDrive.getEstimatedPose().getY() + > ReefPositioning.REEF_CENTER.getY()), + Commands.sequence(pickupAlgae(0), scoreAlgae()).onlyIf(() -> algaeCount >= 1), Commands.sequence( - robot.swerveDrive.driveQuicklyTo(new Pose2d(6.3, 5.25, Rotation2d.fromDegrees(-40))).deadlineFor(robot.pieceCombos.algaeL3()), - robot.swerveDrive.driveQuicklyTo(new Pose2d(5.6, 5.5, Rotation2d.fromDegrees(-120))).deadlineFor(robot.pieceCombos.algaeL3()), - pickupAlgae(1), - scoreAlgae() - ).onlyIf(() -> algaeCount >= 2) - )); - } + robot + .swerveDrive + .driveQuicklyTo(new Pose2d(6.3, 5.25, Rotation2d.fromDegrees(-40))) + .deadlineFor(robot.pieceCombos.algaeL3()), + robot + .swerveDrive + .driveQuicklyTo(new Pose2d(5.6, 5.5, Rotation2d.fromDegrees(-120))) + .deadlineFor(robot.pieceCombos.algaeL3()), + pickupAlgae(1), + scoreAlgae()) + .onlyIf(() -> algaeCount >= 2))); + } } diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 5ea5990b..e78e54c4 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -10,104 +10,177 @@ import frc.robot.subsystems.manipulator.Manipulator; public final class IntakeCommands { - private IntakeCommands() { - } + private IntakeCommands() {} - public static Command intakeTransfer(Intake intake, Elevator elevator, Manipulator manipulator, SafeSubsystems safeSubsystems, PieceCombos pieceCombos) { - return Commands.sequence( - Commands.deadline( + public static Command intakeTransfer( + Intake intake, + Elevator elevator, + Manipulator manipulator, + SafeSubsystems safeSubsystems, + PieceCombos pieceCombos) { + return Commands.sequence( + Commands.deadline( Commands.parallel( - intake.rollers.intake(), - intake.pivot.deploy().until(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE || intake.sensors.getCoralLocation() == CoralLocation.INDEXER) - .andThen( - Commands.waitUntil(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER), - intake.pivot.stow() - ), - intake.indexer.intake() - ).until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER), - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT) - ).onlyIf(() -> !manipulator.grabber.hasCoral() && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE || intake.sensors.getCoralLocation() == CoralLocation.INTAKE || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_INDEXER)), - Commands.deadline( - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), - intake.pivot.stow() - ), - Commands.deadline( - intake.transfer(), - pieceCombos.intakeCoral(), - intake.pivot.stow() - ).onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_MANIPULATOR || !manipulator.grabber.isCoralClear()) - ); - } + intake.rollers.intake(), + intake + .pivot + .deploy() + .until( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INTAKE + || intake.sensors.getCoralLocation() + == CoralLocation.INDEXER) + .andThen( + Commands.waitUntil( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INDEXER), + intake.pivot.stow()), + intake.indexer.intake()) + .until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER), + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT)) + .onlyIf( + () -> + !manipulator.grabber.hasCoral() + && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE + || intake.sensors.getCoralLocation() == CoralLocation.INTAKE + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_INDEXER)), + Commands.deadline( + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT), + intake.pivot.stow()), + Commands.deadline(intake.transfer(), pieceCombos.intakeCoral(), intake.pivot.stow()) + .onlyIf( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INDEXER + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_MANIPULATOR + || !manipulator.grabber.isCoralClear())); + } - public static Command intakeTransferAuto(Intake intake, Elevator elevator, Manipulator manipulator, SafeSubsystems safeSubsystems, PieceCombos pieceCombos) { - if (RobotBase.isSimulation()) { - return Commands.either( - Commands.waitSeconds(2).andThen(() -> intake.sensors.simIntakeCoral()), - Commands.waitUntil(() -> false), - () -> Math.random() < 0.75 - ).onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE) - .andThen(Commands.waitSeconds(0.5)); - } + public static Command intakeTransferAuto( + Intake intake, + Elevator elevator, + Manipulator manipulator, + SafeSubsystems safeSubsystems, + PieceCombos pieceCombos) { + if (RobotBase.isSimulation()) { + return Commands.either( + Commands.waitSeconds(2).andThen(() -> intake.sensors.simIntakeCoral()), + Commands.waitUntil(() -> false), + () -> Math.random() < 0.75) + .onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE) + .andThen(Commands.waitSeconds(0.5)); + } - return Commands.sequence( - Commands.deadline( + return Commands.sequence( + Commands.deadline( Commands.parallel( - intake.rollers.intake(), - intake.pivot.deploy().until(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE || intake.sensors.getCoralLocation() == CoralLocation.INDEXER), - intake.indexer.intake() - ).until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER), - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT) - ).onlyIf(() -> !manipulator.grabber.hasCoral() && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE || intake.sensors.getCoralLocation() == CoralLocation.INTAKE || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_INDEXER)), - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), - Commands.deadline( - intake.transfer(), - pieceCombos.intakeCoral() - ).onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_MANIPULATOR || !manipulator.grabber.isCoralClear()) - ); - } + intake.rollers.intake(), + intake + .pivot + .deploy() + .until( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INTAKE + || intake.sensors.getCoralLocation() + == CoralLocation.INDEXER), + intake.indexer.intake()) + .until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER), + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT)) + .onlyIf( + () -> + !manipulator.grabber.hasCoral() + && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE + || intake.sensors.getCoralLocation() == CoralLocation.INTAKE + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_INDEXER)), + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), + Commands.deadline(intake.transfer(), pieceCombos.intakeCoral()) + .onlyIf( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INDEXER + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_MANIPULATOR + || !manipulator.grabber.isCoralClear())); + } - public static Command intakeTransferVertical(Intake intake, Elevator elevator, Manipulator manipulator, SafeSubsystems safeSubsystems, PieceCombos pieceCombos) { - return Commands.sequence( - Commands.deadline( + public static Command intakeTransferVertical( + Intake intake, + Elevator elevator, + Manipulator manipulator, + SafeSubsystems safeSubsystems, + PieceCombos pieceCombos) { + return Commands.sequence( + Commands.deadline( Commands.sequence( - Commands.parallel( - intake.rollers.intake(), - intake.pivot.deployVertical() - ).until(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE), + Commands.parallel(intake.rollers.intake(), intake.pivot.deployVertical()) + .until(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE), intake.pivot.deploy(), - Commands.parallel( - intake.rollers.intake(), - intake.indexer.intake() - ).until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER) - ), - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT) - ).onlyIf(() -> !manipulator.grabber.hasCoral() && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_INDEXER)), - Commands.deadline( - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), - intake.pivot.stow() - ), - Commands.deadline( - intake.transfer(), - pieceCombos.intakeCoral(), - intake.pivot.stow() - ).onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER || intake.sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_MANIPULATOR) - ); - } + Commands.parallel(intake.rollers.intake(), intake.indexer.intake()) + .until(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER)), + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT)) + .onlyIf( + () -> + !manipulator.grabber.hasCoral() + && (intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_INDEXER)), + Commands.deadline( + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT), + intake.pivot.stow()), + Commands.deadline(intake.transfer(), pieceCombos.intakeCoral(), intake.pivot.stow()) + .onlyIf( + () -> + intake.sensors.getCoralLocation() == CoralLocation.INDEXER + || intake.sensors.getCoralLocation() + == CoralLocation.TRANSFER_TO_MANIPULATOR)); + } - public static Command intakeCoral(Intake intake, Elevator elevator, Manipulator manipulator, SafeSubsystems safeSubsystems, PieceCombos pieceCombos) { - return Commands.deadline( + public static Command intakeCoral( + Intake intake, + Elevator elevator, + Manipulator manipulator, + SafeSubsystems safeSubsystems, + PieceCombos pieceCombos) { + return Commands.deadline( intake.intake(), - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT) - ).onlyIf(() -> RobotBase.isSimulation() || (!manipulator.grabber.hasCoral() && intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE)); - } + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), + manipulator.pivot.coralIntake(), + ELEVATOR.CORAL.INTAKE_HEIGHT)) + .onlyIf( + () -> + RobotBase.isSimulation() + || (!manipulator.grabber.hasCoral() + && intake.sensors.getCoralLocation() == CoralLocation.OUTSIDE)); + } - public static Command transferCoral(Intake intake, Elevator elevator, Manipulator manipulator, SafeSubsystems safeSubsystems, PieceCombos pieceCombos) { - return Commands.sequence( - safeSubsystems.safeMoveCommand(elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), - Commands.deadline( - intake.transfer(), - pieceCombos.intakeCoral() - ).onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER) - ); - } + public static Command transferCoral( + Intake intake, + Elevator elevator, + Manipulator manipulator, + SafeSubsystems safeSubsystems, + PieceCombos pieceCombos) { + return Commands.sequence( + safeSubsystems.safeMoveCommand( + elevator.coralIntake(), manipulator.pivot.coralIntake(), ELEVATOR.CORAL.INTAKE_HEIGHT), + Commands.deadline(intake.transfer(), pieceCombos.intakeCoral()) + .onlyIf(() -> intake.sensors.getCoralLocation() == CoralLocation.INDEXER)); + } } diff --git a/src/main/java/frc/robot/commands/PieceCombos.java b/src/main/java/frc/robot/commands/PieceCombos.java index 069565f5..36c763dc 100644 --- a/src/main/java/frc/robot/commands/PieceCombos.java +++ b/src/main/java/frc/robot/commands/PieceCombos.java @@ -1,7 +1,6 @@ package frc.robot.commands; import com.team6962.lib.utils.CommandUtils; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; @@ -57,15 +56,13 @@ public Command coralL3() { public Command readyL3() { return safeSubsystems - .safeMoveCommand( - elevator.ready(), manipulator.stow(), ELEVATOR.AUTO.READY_HEIGHT) + .safeMoveCommand(elevator.ready(), manipulator.stow(), ELEVATOR.AUTO.READY_HEIGHT) .withName("READY L3"); } public Command readyL2() { return safeSubsystems - .safeMoveCommand( - elevator.coralL2(), manipulator.stow(), ELEVATOR.AUTO.READY_HEIGHT) + .safeMoveCommand(elevator.coralL2(), manipulator.stow(), ELEVATOR.AUTO.READY_HEIGHT) .withName("READY L3"); } @@ -107,10 +104,11 @@ public Command algaeBargeSetup() { public Command algaeBargeShoot() { return Commands.sequence( manipulator.pivot.algaeBargeSetup(), - manipulator.pivot.algaeBargeShoot().deadlineFor(Commands.sequence( - Commands.waitSeconds(0.15), - manipulator.grabber.dropAlgae()) - )) + manipulator + .pivot + .algaeBargeShoot() + .deadlineFor( + Commands.sequence(Commands.waitSeconds(0.15), manipulator.grabber.dropAlgae()))) .onlyIf(() -> elevator.isNear(ELEVATOR.ALGAE.BARGE_HEIGHT)); } diff --git a/src/main/java/frc/robot/commands/XBoxSwerve.java b/src/main/java/frc/robot/commands/XBoxSwerve.java index 69802aa8..73da4f17 100644 --- a/src/main/java/frc/robot/commands/XBoxSwerve.java +++ b/src/main/java/frc/robot/commands/XBoxSwerve.java @@ -7,15 +7,10 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; -import java.util.LinkedList; -import java.util.List; -import java.util.function.Function; - import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.swerve.auto.RobotCoordinates; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.KinematicsUtils; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -29,6 +24,9 @@ import frc.robot.util.CachedRobotState; import frc.robot.util.software.MathUtils; import frc.robot.util.software.MathUtils.InputMath; +import java.util.LinkedList; +import java.util.List; +import java.util.function.Function; public class XBoxSwerve extends Command { private XboxController controller; @@ -175,7 +173,7 @@ public void execute() { ChassisSpeeds drivenSpeeds = new ChassisSpeeds(velocity.getX(), velocity.getY(), angularVelocity); - + for (Function modifier : speedsModifiers) { drivenSpeeds = modifier.apply(drivenSpeeds); } @@ -229,8 +227,6 @@ public boolean isFinished() { public Command modifySpeeds(Function modifier) { return Commands.startEnd( - () -> speedsModifiers.add(modifier), - () -> speedsModifiers.remove(modifier) - ); + () -> speedsModifiers.add(modifier), () -> speedsModifiers.remove(modifier)); } } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index c8b83b4e..66dac3ab 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -12,14 +12,8 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; -import java.util.Map; -import java.util.NavigableMap; -import java.util.TreeMap; -import java.util.function.Supplier; - import com.team6962.lib.swerve.SwerveConfig; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -31,6 +25,10 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import frc.robot.util.CachedRobotState; +import java.util.Map; +import java.util.NavigableMap; +import java.util.TreeMap; +import java.util.function.Supplier; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean diff --git a/src/main/java/frc/robot/field/Field.java b/src/main/java/frc/robot/field/Field.java index 039defbc..507e5f01 100644 --- a/src/main/java/frc/robot/field/Field.java +++ b/src/main/java/frc/robot/field/Field.java @@ -3,12 +3,11 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.field; -import java.util.ArrayList; -import java.util.List; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import frc.robot.util.CachedRobotState; +import java.util.ArrayList; +import java.util.List; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean diff --git a/src/main/java/frc/robot/field/ReefPositioning.java b/src/main/java/frc/robot/field/ReefPositioning.java index efcafdfe..432f1c47 100644 --- a/src/main/java/frc/robot/field/ReefPositioning.java +++ b/src/main/java/frc/robot/field/ReefPositioning.java @@ -115,9 +115,8 @@ public static Pose2d getCoralAlignPose(int pole) { public static Pose2d getL1PlacePose(int pole) { return rotatePose( - getPolePose(new Translation2d(0.99, 0.97), new Translation2d(0, 0), pole), - Rotation2d.fromDegrees(180 + (pole % 2 == 0 ? 68.62 : -68.62)) - ); + getPolePose(new Translation2d(0.99, 0.97), new Translation2d(0, 0), pole), + Rotation2d.fromDegrees(180 + (pole % 2 == 0 ? 68.62 : -68.62))); } public static Pose2d getAlgaePickupPose(int face) { diff --git a/src/main/java/frc/robot/field/StationPositioning.java b/src/main/java/frc/robot/field/StationPositioning.java index aa223690..6cb6d4a1 100644 --- a/src/main/java/frc/robot/field/StationPositioning.java +++ b/src/main/java/frc/robot/field/StationPositioning.java @@ -106,7 +106,9 @@ public static Pose2d getCenterAlignPose(CoralStation coralStation) { } public static Pose2d reflectPose(Pose2d pose, boolean reflect) { - return reflect ? new Pose2d(pose.getX(), Field.WIDTH - pose.getY(), pose.getRotation().unaryMinus()) : pose; + return reflect + ? new Pose2d(pose.getX(), Field.WIDTH - pose.getY(), pose.getRotation().unaryMinus()) + : pose; } public static Pose2d getGroundIntakePose(boolean reflect) { @@ -120,11 +122,13 @@ public static Pose2d getGroundIntakePose(CoralStation coralStation) { } public static Pose2d getGroundStartIntakePose(CoralStation coralStation) { - return reflectPose(new Pose2d(2.78, 0.7, Rotation2d.fromDegrees(0)), coralStation == CoralStation.LEFT); + return reflectPose( + new Pose2d(2.78, 0.7, Rotation2d.fromDegrees(0)), coralStation == CoralStation.LEFT); } public static Pose2d getGroundEndIntakePose(CoralStation coralStation) { - return reflectPose(new Pose2d(1.37, 1.44, Rotation2d.fromDegrees(0)), coralStation == CoralStation.LEFT); + return reflectPose( + new Pose2d(1.37, 1.44, Rotation2d.fromDegrees(0)), coralStation == CoralStation.LEFT); } public static Pose2d rotate180(Pose2d pose) { diff --git a/src/main/java/frc/robot/subsystems/Controls.java b/src/main/java/frc/robot/subsystems/Controls.java index a4e1c375..e4357046 100644 --- a/src/main/java/frc/robot/subsystems/Controls.java +++ b/src/main/java/frc/robot/subsystems/Controls.java @@ -4,13 +4,9 @@ import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Inches; -import java.util.Set; -import java.util.function.BooleanSupplier; - import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; @@ -35,6 +31,8 @@ import frc.robot.subsystems.intake.IntakeSensors.CoralLocation; import frc.robot.subsystems.leds.LEDs; import frc.robot.subsystems.manipulator.Manipulator; +import java.util.Set; +import java.util.function.BooleanSupplier; public class Controls { public final CommandXboxController operator = @@ -75,14 +73,15 @@ public void configureBindings( // Button to move to left/right reef (dpad left right) // Button for aligning to algae on the reef (dpad up) - driver.a().whileTrue( - autoAlign.autoAlignBarge() - .andThen( - rumbleBoth() - .repeatedly() - .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)) - ) - ); + driver + .a() + .whileTrue( + autoAlign + .autoAlignBarge() + .andThen( + rumbleBoth() + .repeatedly() + .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)))); driver .b() .whileTrue( @@ -102,35 +101,42 @@ public void configureBindings( rumbleBoth() .repeatedly() .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)))); - driver.y().whileTrue(autograbAlgae2(swerveDrive, elevator, manipulator, pieceCombos, autoAlign)); - driver.start().whileTrue(autoAlign.alignToClosestL1Teleop( - AutoAlign.PolePattern.LEFT, - () -> rumbleBoth() - .repeatedly() - .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)) - )); - driver.back().whileTrue(autoAlign.alignToClosestL1Teleop( - AutoAlign.PolePattern.RIGHT, - () -> rumbleBoth() - .repeatedly() - .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)) - )); + driver + .y() + .whileTrue(autograbAlgae2(swerveDrive, elevator, manipulator, pieceCombos, autoAlign)); + driver + .start() + .whileTrue( + autoAlign.alignToClosestL1Teleop( + AutoAlign.PolePattern.LEFT, + () -> + rumbleBoth() + .repeatedly() + .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)))); + driver + .back() + .whileTrue( + autoAlign.alignToClosestL1Teleop( + AutoAlign.PolePattern.RIGHT, + () -> + rumbleBoth() + .repeatedly() + .alongWith(LEDs.setStateCommand(LEDs.State.AUTO_ALIGN)))); // driver.leftBumper(); driver.rightBumper().whileTrue(intake.drop()); - driver.rightStick().whileTrue(Commands.parallel( - IntakeCommands.intakeTransfer(intake, elevator, manipulator, manipulatorSafeties, pieceCombos) - .andThen(Commands.waitUntil(() -> manipulator.grabber.hasCoral())) - .andThen(Commands.parallel( - pieceCombos.readyL2() - )) - .andThen(rumbleOperator()), - Commands.waitUntil(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE) - .andThen(Commands.parallel( - rumbleDriver(), - LEDs.setStateCommand(LEDs.State.GOOD) - )) - )); + driver + .rightStick() + .whileTrue( + Commands.parallel( + IntakeCommands.intakeTransfer( + intake, elevator, manipulator, manipulatorSafeties, pieceCombos) + .andThen(Commands.waitUntil(() -> manipulator.grabber.hasCoral())) + .andThen(Commands.parallel(pieceCombos.readyL2())) + .andThen(rumbleOperator()), + Commands.waitUntil(() -> intake.sensors.getCoralLocation() == CoralLocation.INTAKE) + .andThen( + Commands.parallel(rumbleDriver(), LEDs.setStateCommand(LEDs.State.GOOD))))); driver.leftStick().whileTrue(intake.intake()); driver.povCenter(); // USED driver.povUp(); // USED @@ -171,21 +177,20 @@ public void configureBindings( operator .leftStick() .onTrue( - pieceCombos.stow() - .andThen( - elevator.launchBarge() - .withDeadline(Commands.sequence( - Commands.waitUntil(() -> elevator.getPosition().gt(Inches.of(52.5))), - manipulator.grabber.dropAlgae().withTimeout(0.5) - )) - ) - ); // barge combo - + pieceCombos + .stow() + .andThen( + elevator + .launchBarge() + .withDeadline( + Commands.sequence( + Commands.waitUntil( + () -> elevator.getPosition().gt(Inches.of(52.5))), + manipulator.grabber.dropAlgae().withTimeout(0.5))))); // barge combo + // Big Right Paddle - Stow - operator - .rightStick() - .whileTrue(pieceCombos.stow()); // big right paddle - + operator.rightStick().whileTrue(pieceCombos.stow()); // big right paddle + operator.rightBumper().onTrue(manipulator.grabber.repositionCoral()); // Right Trigger - Intake algae / Shoot coral @@ -198,16 +203,21 @@ public void configureBindings( rumbleBoth() .alongWith( LEDs.setStateCommand(LEDs.State.GOOD)))); // drop coral/intake algae - + // Left Bumper - Backup barge - operator.leftBumper().onTrue( - Commands.either( - manipulator.stow().andThen(elevator.algaeBarge()).andThen(manipulator.grabber.dropAlgae()), - manipulator.stow() - .andThen(elevator.algaeBarge()), - () -> elevator.getPosition().gt(ELEVATOR.ALGAE.BARGE_HEIGHT.minus(Inches.of(2))) - ) - ); // shoot barge + operator + .leftBumper() + .onTrue( + Commands.either( + manipulator + .stow() + .andThen(elevator.algaeBarge()) + .andThen(manipulator.grabber.dropAlgae()), + manipulator.stow().andThen(elevator.algaeBarge()), + () -> + elevator + .getPosition() + .gt(ELEVATOR.ALGAE.BARGE_HEIGHT.minus(Inches.of(2))))); // shoot barge // Right Trigger - Drop algae operator @@ -221,7 +231,11 @@ public void configureBindings( ); } - private boolean isNearCoralPole(SwerveDrive swerveDrive, Distance translationTolerance, Angle rotationTolerance, PolePattern pattern) { + private boolean isNearCoralPole( + SwerveDrive swerveDrive, + Distance translationTolerance, + Angle rotationTolerance, + PolePattern pattern) { for (int i = pattern.start; i < 12; i += pattern.increment) { Pose2d placePose = ReefPositioning.getCoralPlacePose(i); @@ -240,123 +254,138 @@ private Command autoscoreCoral( PieceCombos pieceCombos, int level) { return Commands.sequence( - Commands.deadline( - Commands.waitUntil(() -> { - return (driver.getHID().getBButton() || driver.getHID().getXButton()) && isNearCoralPole( - swerveDrive, Feet.of(2), Degrees.of(45), - driver.getHID().getBButton() ? PolePattern.RIGHT : PolePattern.LEFT - ); - }), - level == 2 ? pieceCombos.coralL2() : pieceCombos.readyL3() - ), - Commands.parallel( - pieceCombos.coral(level), - Commands.waitUntil( - () -> { - if (level == 1) return false; + Commands.deadline( + Commands.waitUntil( + () -> { + return (driver.getHID().getBButton() || driver.getHID().getXButton()) + && isNearCoralPole( + swerveDrive, + Feet.of(2), + Degrees.of(45), + driver.getHID().getBButton() ? PolePattern.RIGHT : PolePattern.LEFT); + }), + level == 2 ? pieceCombos.coralL2() : pieceCombos.readyL3()), + Commands.parallel( + pieceCombos.coral(level), + Commands.waitUntil( + () -> { + if (level == 1) return false; - if (!isNearCoralPole(swerveDrive, Inches.of(0.85), Degrees.of(4), PolePattern.ALL)) return false; + if (!isNearCoralPole( + swerveDrive, Inches.of(0.85), Degrees.of(4), PolePattern.ALL)) return false; - Distance targetHeight = - level == 2 - ? ELEVATOR.CORAL.L2_HEIGHT - : level == 3 ? ELEVATOR.CORAL.L3_HEIGHT : ELEVATOR.CORAL.L4_HEIGHT; + Distance targetHeight = + level == 2 + ? ELEVATOR.CORAL.L2_HEIGHT + : level == 3 ? ELEVATOR.CORAL.L3_HEIGHT : ELEVATOR.CORAL.L4_HEIGHT; - if (!elevator.isNear(targetHeight)) return false; + if (!elevator.isNear(targetHeight)) return false; - Angle targetAngle = - level == 2 || level == 3 - ? MANIPULATOR_PIVOT.CORAL.L23_ANGLE - : MANIPULATOR_PIVOT.CORAL.L4_ANGLE; + Angle targetAngle = + level == 2 || level == 3 + ? MANIPULATOR_PIVOT.CORAL.L23_ANGLE + : MANIPULATOR_PIVOT.CORAL.L4_ANGLE; - if (MeasureMath.minAbsDifference(manipulator.pivot.getPosition(), targetAngle) - .gte(Degrees.of(2))) return false; + if (MeasureMath.minAbsDifference(manipulator.pivot.getPosition(), targetAngle) + .gte(Degrees.of(2))) return false; - return true; - } - ) - ), - Commands.parallel(pieceCombos.coral(level), manipulator.grabber.dropCoral()) - ); + return true; + })), + Commands.parallel(pieceCombos.coral(level), manipulator.grabber.dropCoral())); } public Command autograbAlgae( - SwerveDrive swerveDrive, - Elevator elevator, - Manipulator manipulator, - PieceCombos pieceCombos, - AutoAlign autoAlign - ) { - return Commands.defer(() -> { - int face = autoAlign.getClosestReefFace(swerveDrive.getEstimatedPose()); - int level = ReefPositioning.getAlgaeHeight(face); - - return Commands.sequence( - Commands.deadline( - swerveDrive - .pathfindTo(ReefPositioning.getAlgaeAlignPose(face)) - .andThen(swerveDrive.driveTo(ReefPositioning.getAlgaeAlignPose(face))) - .until(() -> swerveDrive.isWithinToleranceOf(ReefPositioning.getAlgaeAlignPose(face), Inches.of(3), Degrees.of(10))), - Commands.sequence( - manipulator.pivot.stow().until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), - (level == 2 ? elevator.algaeL2() : elevator.ready()) - ) - ), - manipulator.pivot.stow().until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), - level == 2 ? elevator.algaeL2() : elevator.algaeL3(), - Commands.deadline( - manipulator.grabber.intakeAlgae(), - manipulator.pivot.algaeReef(), - swerveDrive.driveTo(ReefPositioning.getAlgaePickupPose(face)) - ), - swerveDrive.driveTo(ReefPositioning.getAlgaeAlignPose(face)) - .deadlineFor( - manipulator.pivot.pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE), - elevator.ready() - ) - .until(() -> swerveDrive.isWithinToleranceOf(ReefPositioning.getAlgaeAlignPose(face), Inches.of(3), Degrees.of(30))) - ); - }, Set.of(swerveDrive.useRotation(), swerveDrive.useTranslation(), elevator, manipulator)); + SwerveDrive swerveDrive, + Elevator elevator, + Manipulator manipulator, + PieceCombos pieceCombos, + AutoAlign autoAlign) { + return Commands.defer( + () -> { + int face = autoAlign.getClosestReefFace(swerveDrive.getEstimatedPose()); + int level = ReefPositioning.getAlgaeHeight(face); + + return Commands.sequence( + Commands.deadline( + swerveDrive + .pathfindTo(ReefPositioning.getAlgaeAlignPose(face)) + .andThen(swerveDrive.driveTo(ReefPositioning.getAlgaeAlignPose(face))) + .until( + () -> + swerveDrive.isWithinToleranceOf( + ReefPositioning.getAlgaeAlignPose(face), + Inches.of(3), + Degrees.of(10))), + Commands.sequence( + manipulator + .pivot + .stow() + .until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), + (level == 2 ? elevator.algaeL2() : elevator.ready()))), + manipulator + .pivot + .stow() + .until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), + level == 2 ? elevator.algaeL2() : elevator.algaeL3(), + Commands.deadline( + manipulator.grabber.intakeAlgae(), + manipulator.pivot.algaeReef(), + swerveDrive.driveTo(ReefPositioning.getAlgaePickupPose(face))), + swerveDrive + .driveTo(ReefPositioning.getAlgaeAlignPose(face)) + .deadlineFor( + manipulator.pivot.pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE), + elevator.ready()) + .until( + () -> + swerveDrive.isWithinToleranceOf( + ReefPositioning.getAlgaeAlignPose(face), + Inches.of(3), + Degrees.of(30)))); + }, + Set.of(swerveDrive.useRotation(), swerveDrive.useTranslation(), elevator, manipulator)); } public Command autograbAlgae2( - SwerveDrive swerveDrive, - Elevator elevator, - Manipulator manipulator, - PieceCombos pieceCombos, - AutoAlign autoAlign - ) { - return Commands.defer(() -> { - int face = autoAlign.getClosestReefFace(swerveDrive.getEstimatedPose()); - int level = ReefPositioning.getAlgaeHeight(face); - - return Commands.sequence( - Commands.parallel( - manipulator.pivot.stow().until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), - swerveDrive - .driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face)) - ), - Commands.parallel( - swerveDrive.driveTo(ReefPositioning.getAlgaePickupPose(face)), - Commands.sequence( - Commands.deadline( - (level == 2 ? elevator.algaeL2() : elevator.algaeL3()), - manipulator.pivot.stow() - ), - Commands.parallel( - manipulator.grabber.intakeAlgae(), - manipulator.pivot.algaeReef() - ) - ) - ).until(manipulator.grabber::hasAlgae), - swerveDrive.driveTo(ReefPositioning.getAlgaeAlignPose(face)) - .deadlineFor( - manipulator.pivot.pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE), - elevator.ready() - ) - .until(() -> swerveDrive.isWithinToleranceOf(ReefPositioning.getAlgaeAlignPose(face), Inches.of(3), Degrees.of(30))) - ); - }, Set.of(swerveDrive.useRotation(), swerveDrive.useTranslation(), elevator, manipulator)); + SwerveDrive swerveDrive, + Elevator elevator, + Manipulator manipulator, + PieceCombos pieceCombos, + AutoAlign autoAlign) { + return Commands.defer( + () -> { + int face = autoAlign.getClosestReefFace(swerveDrive.getEstimatedPose()); + int level = ReefPositioning.getAlgaeHeight(face); + + return Commands.sequence( + Commands.parallel( + manipulator + .pivot + .stow() + .until(() -> manipulator.pivot.getPosition().gt(Degrees.of(-10))), + swerveDrive.driveQuicklyTo(ReefPositioning.getAlgaeAlignPose(face))), + Commands.parallel( + swerveDrive.driveTo(ReefPositioning.getAlgaePickupPose(face)), + Commands.sequence( + Commands.deadline( + (level == 2 ? elevator.algaeL2() : elevator.algaeL3()), + manipulator.pivot.stow()), + Commands.parallel( + manipulator.grabber.intakeAlgae(), manipulator.pivot.algaeReef()))) + .until(manipulator.grabber::hasAlgae), + swerveDrive + .driveTo(ReefPositioning.getAlgaeAlignPose(face)) + .deadlineFor( + manipulator.pivot.pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE), + elevator.ready()) + .until( + () -> + swerveDrive.isWithinToleranceOf( + ReefPositioning.getAlgaeAlignPose(face), + Inches.of(3), + Degrees.of(30)))); + }, + Set.of(swerveDrive.useRotation(), swerveDrive.useTranslation(), elevator, manipulator)); } private Command rumble(CommandXboxController controller) { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index f4008a74..c069603c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -24,7 +24,6 @@ import com.team6962.lib.telemetry.MechanismLogger; import com.team6962.lib.utils.CTREUtils; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -44,317 +43,359 @@ import frc.robot.util.software.MathUtils; public class Elevator extends SubsystemBase { - protected TalonFX leftMotor; - protected TalonFX rightMotor; + protected TalonFX leftMotor; + protected TalonFX rightMotor; - protected DigitalInput bottomLimitSwitch; - protected DigitalInput topLimitSwitch; + protected DigitalInput bottomLimitSwitch; + protected DigitalInput topLimitSwitch; - protected ControlRequest controlRequest; + protected ControlRequest controlRequest; - protected boolean elevatorZeroed = false; - protected Distance targetPosition = Inches.of(0); - private Distance commandTarget = Inches.of(0); + protected boolean elevatorZeroed = false; + protected Distance targetPosition = Inches.of(0); + private Distance commandTarget = Inches.of(0); - private StatusSignal positionSignal; - private StatusSignal velocitySignal; - private StatusSignal supplyCurrentLeft; - private StatusSignal supplyCurrentRight; - private StatusSignal statorCurrentLeft; - private StatusSignal statorCurrentRight; - private StatusSignal motorVoltageLeft; - private StatusSignal motorVoltageRight; - private StatusSignal profilePositionSignal; + private StatusSignal positionSignal; + private StatusSignal velocitySignal; + private StatusSignal supplyCurrentLeft; + private StatusSignal supplyCurrentRight; + private StatusSignal statorCurrentLeft; + private StatusSignal statorCurrentRight; + private StatusSignal motorVoltageLeft; + private StatusSignal motorVoltageRight; + private StatusSignal profilePositionSignal; - private Grabber grabber; + private Grabber grabber; - public Elevator(Grabber grabber) { - this.grabber = grabber; + public Elevator(Grabber grabber) { + this.grabber = grabber; - leftMotor = new TalonFX(ElevatorConstants.LEFT_MOTOR_ID, "drivetrain"); // Replace with actual CAN ID - rightMotor = new TalonFX(ElevatorConstants.RIGHT_MOTOR_ID, "drivetrain"); // Replace with actual CAN ID + leftMotor = + new TalonFX(ElevatorConstants.LEFT_MOTOR_ID, "drivetrain"); // Replace with actual CAN ID + rightMotor = + new TalonFX(ElevatorConstants.RIGHT_MOTOR_ID, "drivetrain"); // Replace with actual CAN ID - bottomLimitSwitch = new DigitalInput(ElevatorConstants.DIO_FLOOR_PORT); // Replace with actual DIO port - topLimitSwitch = new DigitalInput(ElevatorConstants.DIO_CEILING_PORT); // Replace with actual DIO port + bottomLimitSwitch = + new DigitalInput(ElevatorConstants.DIO_FLOOR_PORT); // Replace with actual DIO port + topLimitSwitch = + new DigitalInput(ElevatorConstants.DIO_CEILING_PORT); // Replace with actual DIO port - TalonFXConfiguration talonFXConfiguration = new TalonFXConfiguration() + TalonFXConfiguration talonFXConfiguration = + new TalonFXConfiguration() .withSlot0(Slot0Configs.from(ElevatorConstants.emptySlot)) .withSlot1(Slot1Configs.from(ElevatorConstants.coralSlot)) .withSlot2(Slot2Configs.from(ElevatorConstants.algaeSlot)) .withMotionMagic(ElevatorConstants.motionMagicConfigs) .withCurrentLimits(ElevatorConstants.currentLimitsConfigs) .withMotorOutput(ElevatorConstants.motorOutputConfigs) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(ElevatorConstants.SENSOR_MECHANISM_RATIO)); // Assuming 1:1 ratio, adjust if necessary - - talonFXConfiguration.MotorOutput.Inverted = ElevatorConstants.LEFT_MOTOR_INVERTED_VALUE; // Set inversion for left motor - CTREUtils.check(leftMotor.getConfigurator().apply(talonFXConfiguration)); - - talonFXConfiguration.MotorOutput.Inverted = ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE; // Set inversion for left motor - CTREUtils.check(rightMotor.getConfigurator().apply(talonFXConfiguration)); - - initStatusSignals(); - - Logger.logMeasure("NewElevator/position", this::getPosition); - Logger.logMeasure("NewElevator/velocity", this::getVelocity); - Logger.logMeasure("NewElevator/commandTarget", () -> commandTarget); - Logger.logBoolean("NewElevator/topLimitSwitchTriggered", this::topLimitSwitchTriggered); - Logger.logBoolean("NewElevator/bottomLimitSwitchTriggered", this::bottomLimitSwitchTriggered); - Logger.logMeasure("NewElevator/supplyCurrentLeft", () -> supplyCurrentLeft.getValue()); - Logger.logMeasure("NewElevator/supplyCurrentRight", () -> supplyCurrentRight.getValue()); - Logger.logMeasure("NewElevator/statorCurrentLeft", () -> statorCurrentLeft.getValue()); - Logger.logMeasure("NewElevator/statorCurrentRight", () -> statorCurrentRight.getValue()); - Logger.logMeasure("NewElevator/voltageLeft", () -> motorVoltageLeft.getValue()); - Logger.logMeasure("NewElevator/voltageRight", () -> motorVoltageRight.getValue()); - Logger.logMeasure("NewElevator/targetPosition", () -> targetPosition); - Logger.logMeasure("NewElevator/profilePosition", () -> Meters.of(profilePositionSignal.getValue())); - - MechanismRoot2d root = MechanismLogger.getRoot("Elevator", -6, 39.457496 - ElevatorConstants.MIN_HEIGHT.in(Inches) + 1); - MechanismLigament2d ligament = MechanismLogger.getLigament("Elevator Top", ElevatorConstants.MIN_HEIGHT.in(Inches), 90, 6, new Color8Bit(255, 0, 0)); - - root.append(ligament); - - MechanismLogger.addDynamicLength(ligament, () -> getPosition().minus(Inches.of(1))); - - MechanismRoot2d carriage = MechanismLogger.getRoot("Manipulator Pivot"); - - MechanismLogger.addDyanmicPosition( - carriage, - () -> Inches.of(-9.495), - () -> Inches.of(MathUtils.map(getPosition().in(Inches), ElevatorConstants.MIN_HEIGHT.in(Inches), ElevatorConstants.MAX_HEIGHT.in(Inches), 9.825, 67.717005)) - ); - } - - private void initStatusSignals() { - positionSignal = leftMotor.getPosition(); - velocitySignal = leftMotor.getVelocity(); - supplyCurrentLeft = leftMotor.getSupplyCurrent(); - supplyCurrentRight = rightMotor.getSupplyCurrent(); - statorCurrentLeft = leftMotor.getStatorCurrent(); - statorCurrentRight = rightMotor.getStatorCurrent(); - motorVoltageLeft = leftMotor.getMotorVoltage(); - motorVoltageRight = rightMotor.getMotorVoltage(); - profilePositionSignal = leftMotor.getClosedLoopReference(); - - CTREUtils.check(BaseStatusSignal.setUpdateFrequencyForAll( + .withFeedback( + new FeedbackConfigs() + .withSensorToMechanismRatio( + ElevatorConstants + .SENSOR_MECHANISM_RATIO)); // Assuming 1:1 ratio, adjust if necessary + + talonFXConfiguration.MotorOutput.Inverted = + ElevatorConstants.LEFT_MOTOR_INVERTED_VALUE; // Set inversion for left motor + CTREUtils.check(leftMotor.getConfigurator().apply(talonFXConfiguration)); + + talonFXConfiguration.MotorOutput.Inverted = + ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE; // Set inversion for left motor + CTREUtils.check(rightMotor.getConfigurator().apply(talonFXConfiguration)); + + initStatusSignals(); + + Logger.logMeasure("NewElevator/position", this::getPosition); + Logger.logMeasure("NewElevator/velocity", this::getVelocity); + Logger.logMeasure("NewElevator/commandTarget", () -> commandTarget); + Logger.logBoolean("NewElevator/topLimitSwitchTriggered", this::topLimitSwitchTriggered); + Logger.logBoolean("NewElevator/bottomLimitSwitchTriggered", this::bottomLimitSwitchTriggered); + Logger.logMeasure("NewElevator/supplyCurrentLeft", () -> supplyCurrentLeft.getValue()); + Logger.logMeasure("NewElevator/supplyCurrentRight", () -> supplyCurrentRight.getValue()); + Logger.logMeasure("NewElevator/statorCurrentLeft", () -> statorCurrentLeft.getValue()); + Logger.logMeasure("NewElevator/statorCurrentRight", () -> statorCurrentRight.getValue()); + Logger.logMeasure("NewElevator/voltageLeft", () -> motorVoltageLeft.getValue()); + Logger.logMeasure("NewElevator/voltageRight", () -> motorVoltageRight.getValue()); + Logger.logMeasure("NewElevator/targetPosition", () -> targetPosition); + Logger.logMeasure( + "NewElevator/profilePosition", () -> Meters.of(profilePositionSignal.getValue())); + + MechanismRoot2d root = + MechanismLogger.getRoot( + "Elevator", -6, 39.457496 - ElevatorConstants.MIN_HEIGHT.in(Inches) + 1); + MechanismLigament2d ligament = + MechanismLogger.getLigament( + "Elevator Top", + ElevatorConstants.MIN_HEIGHT.in(Inches), + 90, + 6, + new Color8Bit(255, 0, 0)); + + root.append(ligament); + + MechanismLogger.addDynamicLength(ligament, () -> getPosition().minus(Inches.of(1))); + + MechanismRoot2d carriage = MechanismLogger.getRoot("Manipulator Pivot"); + + MechanismLogger.addDyanmicPosition( + carriage, + () -> Inches.of(-9.495), + () -> + Inches.of( + MathUtils.map( + getPosition().in(Inches), + ElevatorConstants.MIN_HEIGHT.in(Inches), + ElevatorConstants.MAX_HEIGHT.in(Inches), + 9.825, + 67.717005))); + } + + private void initStatusSignals() { + positionSignal = leftMotor.getPosition(); + velocitySignal = leftMotor.getVelocity(); + supplyCurrentLeft = leftMotor.getSupplyCurrent(); + supplyCurrentRight = rightMotor.getSupplyCurrent(); + statorCurrentLeft = leftMotor.getStatorCurrent(); + statorCurrentRight = rightMotor.getStatorCurrent(); + motorVoltageLeft = leftMotor.getMotorVoltage(); + motorVoltageRight = rightMotor.getMotorVoltage(); + profilePositionSignal = leftMotor.getClosedLoopReference(); + + CTREUtils.check( + BaseStatusSignal.setUpdateFrequencyForAll( Hertz.of(50), - positionSignal, velocitySignal, supplyCurrentLeft, supplyCurrentRight, - statorCurrentLeft, statorCurrentRight, motorVoltageLeft, motorVoltageRight, - profilePositionSignal - )); - - CTREUtils.check(ParentDevice.optimizeBusUtilizationForAll(leftMotor, rightMotor)); - } - - private void refreshStatusSignals() { - BaseStatusSignal.refreshAll( - positionSignal, velocitySignal, supplyCurrentLeft, supplyCurrentRight, - statorCurrentLeft, statorCurrentRight, motorVoltageLeft, motorVoltageRight, - profilePositionSignal - ); - } - - public Distance getPosition() { - return Meters.of(BaseStatusSignal.getLatencyCompensatedValueAsDouble(positionSignal, velocitySignal)); - } - - public LinearVelocity getVelocity() { - return MetersPerSecond.of(velocitySignal.getValueAsDouble()); - } - - public boolean isNear(Distance position) { - return getPosition().isNear(position, ElevatorConstants.TOLERANCE); - } - - protected boolean topLimitSwitchTriggered() { - return topLimitSwitch.get(); - } - - protected boolean bottomLimitSwitchTriggered() { - return !bottomLimitSwitch.get(); - } - - private void startPositionControl(Distance position, boolean hold) { - targetPosition = position; - - Distance clampedPosition = MeasureMath.clamp(position, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT); - - MotionMagicVoltage controlRequest = new MotionMagicVoltage(clampedPosition.in(Meters)); - // VelocityVoltage controlRequest = new VelocityVoltage(InchesPerSecond.of(6).in(MetersPerSecond)); - - // 0.145 - // 0.1524 - - setLimits(controlRequest); - - if (grabber.hasCoral()) controlRequest.Slot = 1; - else if (grabber.hasAlgae()) controlRequest.Slot = 2; - else controlRequest.Slot = 0; - - this.controlRequest = controlRequest; - - leftMotor.setControl(controlRequest); - rightMotor.setControl(controlRequest); - } - - public Command moveToPosition(Distance position) { - Command moveToCommand = new Command() { - @Override - public void initialize() { - commandTarget = position; - startPositionControl(position, false); - } - - @Override - public void end(boolean interrupted) { - commandTarget = Inches.of(0); - - if (!isNear(position)) { - startPositionControl(getPosition(), true); - } else { - startPositionControl(position, true); - } + positionSignal, + velocitySignal, + supplyCurrentLeft, + supplyCurrentRight, + statorCurrentLeft, + statorCurrentRight, + motorVoltageLeft, + motorVoltageRight, + profilePositionSignal)); + + CTREUtils.check(ParentDevice.optimizeBusUtilizationForAll(leftMotor, rightMotor)); + } + + private void refreshStatusSignals() { + BaseStatusSignal.refreshAll( + positionSignal, + velocitySignal, + supplyCurrentLeft, + supplyCurrentRight, + statorCurrentLeft, + statorCurrentRight, + motorVoltageLeft, + motorVoltageRight, + profilePositionSignal); + } + + public Distance getPosition() { + return Meters.of( + BaseStatusSignal.getLatencyCompensatedValueAsDouble(positionSignal, velocitySignal)); + } + + public LinearVelocity getVelocity() { + return MetersPerSecond.of(velocitySignal.getValueAsDouble()); + } + + public boolean isNear(Distance position) { + return getPosition().isNear(position, ElevatorConstants.TOLERANCE); + } + + protected boolean topLimitSwitchTriggered() { + return topLimitSwitch.get(); + } + + protected boolean bottomLimitSwitchTriggered() { + return !bottomLimitSwitch.get(); + } + + private void startPositionControl(Distance position, boolean hold) { + targetPosition = position; + + Distance clampedPosition = + MeasureMath.clamp(position, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT); + + MotionMagicVoltage controlRequest = new MotionMagicVoltage(clampedPosition.in(Meters)); + // VelocityVoltage controlRequest = new + // VelocityVoltage(InchesPerSecond.of(6).in(MetersPerSecond)); + + // 0.145 + // 0.1524 + + setLimits(controlRequest); + + if (grabber.hasCoral()) controlRequest.Slot = 1; + else if (grabber.hasAlgae()) controlRequest.Slot = 2; + else controlRequest.Slot = 0; + + this.controlRequest = controlRequest; + + leftMotor.setControl(controlRequest); + rightMotor.setControl(controlRequest); + } + + public Command moveToPosition(Distance position) { + Command moveToCommand = + new Command() { + @Override + public void initialize() { + commandTarget = position; + startPositionControl(position, false); + } + + @Override + public void end(boolean interrupted) { + commandTarget = Inches.of(0); + + if (!isNear(position)) { + startPositionControl(getPosition(), true); + } else { + startPositionControl(position, true); } + } - @Override - public boolean isFinished() { - return isNear(position); - } + @Override + public boolean isFinished() { + return isNear(position); + } }; - moveToCommand.addRequirements(this); + moveToCommand.addRequirements(this); - return moveToCommand; - } + return moveToCommand; + } - private Command moveVoltage(Voltage voltage) { - return startEnd( - () -> { - VoltageOut controlRequest = new VoltageOut(voltage); - - leftMotor.setControl(controlRequest); - rightMotor.setControl(controlRequest); - this.controlRequest = controlRequest; - }, - () -> startPositionControl(getPosition(), true) - ); - } + private Command moveVoltage(Voltage voltage) { + return startEnd( + () -> { + VoltageOut controlRequest = new VoltageOut(voltage); - public Command fineControlUp() { - return moveVoltage(ElevatorConstants.FINE_CONTROL_UP); - } - - public Command fineControlDown() { - return moveVoltage(ElevatorConstants.FINE_CONTROL_DOWN); - } - - @Override - public void periodic() { - refreshStatusSignals(); - - if (bottomLimitSwitchTriggered() && !elevatorZeroed) { - CTREUtils.check(leftMotor.setPosition(ElevatorConstants.MIN_HEIGHT.in(Meters))); - CTREUtils.check(rightMotor.setPosition(ElevatorConstants.MIN_HEIGHT.in(Meters))); - - elevatorZeroed = true; - - startPositionControl(ElevatorConstants.MIN_HEIGHT, true); - - return; - } - - if (RobotState.isDisabled()) { - startPositionControl(getPosition(), true); - } - - if (controlRequest != null) { - controlRequest = setLimits(controlRequest); - - CTREUtils.check(leftMotor.setControl(controlRequest)); - CTREUtils.check(rightMotor.setControl(controlRequest)); - } - } + leftMotor.setControl(controlRequest); + rightMotor.setControl(controlRequest); + this.controlRequest = controlRequest; + }, + () -> startPositionControl(getPosition(), true)); + } - private ControlRequest setLimits(ControlRequest request) { - return setLimits(request, topLimitSwitchTriggered() || !elevatorZeroed, bottomLimitSwitchTriggered()); - } + public Command fineControlUp() { + return moveVoltage(ElevatorConstants.FINE_CONTROL_UP); + } - private static ControlRequest setLimits(ControlRequest request, boolean forward, boolean reverse) { - if (request instanceof MotionMagicVoltage r) { - return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); - } else if (request instanceof VoltageOut r) { - return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); - } else if (request instanceof VelocityVoltage r) { - return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); - } else { - return request; - } - } + public Command fineControlDown() { + return moveVoltage(ElevatorConstants.FINE_CONTROL_DOWN); + } - public Command stow() { - return moveToPosition(Constants.ELEVATOR.STOW_HEIGHT); - } + @Override + public void periodic() { + refreshStatusSignals(); - public Command coralL1() { - return moveToPosition(Constants.ELEVATOR.CORAL.L1_HEIGHT); - } + if (bottomLimitSwitchTriggered() && !elevatorZeroed) { + CTREUtils.check(leftMotor.setPosition(ElevatorConstants.MIN_HEIGHT.in(Meters))); + CTREUtils.check(rightMotor.setPosition(ElevatorConstants.MIN_HEIGHT.in(Meters))); - public Command coralL2() { - return moveToPosition(Constants.ELEVATOR.CORAL.L2_HEIGHT); - } + elevatorZeroed = true; - public Command coralL3() { - return moveToPosition(Constants.ELEVATOR.CORAL.L3_HEIGHT); - } + startPositionControl(ElevatorConstants.MIN_HEIGHT, true); - public Command coralL4() { - return moveToPosition(Constants.ELEVATOR.CORAL.L4_HEIGHT); + return; } - public Command coralIntake() { - return moveToPosition(Constants.ELEVATOR.CORAL.INTAKE_HEIGHT); + if (RobotState.isDisabled()) { + startPositionControl(getPosition(), true); } - public Command algaeL2() { - return moveToPosition(Constants.ELEVATOR.ALGAE.L2_HEIGHT); - } + if (controlRequest != null) { + controlRequest = setLimits(controlRequest); - public Command algaeL3() { - return moveToPosition(Constants.ELEVATOR.ALGAE.L3_HEIGHT); + CTREUtils.check(leftMotor.setControl(controlRequest)); + CTREUtils.check(rightMotor.setControl(controlRequest)); } - - public Command algaeBarge() { - return moveToPosition(Constants.ELEVATOR.ALGAE.BARGE_HEIGHT); - } - - public Command ready() { - return moveToPosition(Constants.ELEVATOR.AUTO.READY_HEIGHT); - } - - public Command launchBarge() { - return moveToPosition(Constants.ELEVATOR.MIN_HEIGHT) - .andThen(moveVoltage(Volts.of(12)).until(() -> getPosition().gte(Inches.of(62.5)))) - .andThen(moveVoltage(Volts.of(-6)).until(() -> getVelocity().lte(InchesPerSecond.of(2)))); - } - - public Command coral(int level) { - return switch (level) { - case 1 -> coralL1(); - case 2 -> coralL2(); - case 3 -> coralL3(); - case 4 -> coralL4(); - default -> throw new IllegalArgumentException("Invalid coral level: " + level); - }; - } - - public Command algae(int level) { - return switch (level) { - case 2 -> algaeL2(); - case 3 -> algaeL3(); - default -> throw new IllegalArgumentException("Invalid algae level: " + level); - }; - } - - public static Elevator create(Grabber grabber) { - return RobotBase.isReal() ? new Elevator(grabber) : new SimElevator(grabber); + } + + private ControlRequest setLimits(ControlRequest request) { + return setLimits( + request, topLimitSwitchTriggered() || !elevatorZeroed, bottomLimitSwitchTriggered()); + } + + private static ControlRequest setLimits( + ControlRequest request, boolean forward, boolean reverse) { + if (request instanceof MotionMagicVoltage r) { + return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); + } else if (request instanceof VoltageOut r) { + return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); + } else if (request instanceof VelocityVoltage r) { + return r.withLimitForwardMotion(forward).withLimitReverseMotion(reverse); + } else { + return request; } + } + + public Command stow() { + return moveToPosition(Constants.ELEVATOR.STOW_HEIGHT); + } + + public Command coralL1() { + return moveToPosition(Constants.ELEVATOR.CORAL.L1_HEIGHT); + } + + public Command coralL2() { + return moveToPosition(Constants.ELEVATOR.CORAL.L2_HEIGHT); + } + + public Command coralL3() { + return moveToPosition(Constants.ELEVATOR.CORAL.L3_HEIGHT); + } + + public Command coralL4() { + return moveToPosition(Constants.ELEVATOR.CORAL.L4_HEIGHT); + } + + public Command coralIntake() { + return moveToPosition(Constants.ELEVATOR.CORAL.INTAKE_HEIGHT); + } + + public Command algaeL2() { + return moveToPosition(Constants.ELEVATOR.ALGAE.L2_HEIGHT); + } + + public Command algaeL3() { + return moveToPosition(Constants.ELEVATOR.ALGAE.L3_HEIGHT); + } + + public Command algaeBarge() { + return moveToPosition(Constants.ELEVATOR.ALGAE.BARGE_HEIGHT); + } + + public Command ready() { + return moveToPosition(Constants.ELEVATOR.AUTO.READY_HEIGHT); + } + + public Command launchBarge() { + return moveToPosition(Constants.ELEVATOR.MIN_HEIGHT) + .andThen(moveVoltage(Volts.of(12)).until(() -> getPosition().gte(Inches.of(62.5)))) + .andThen(moveVoltage(Volts.of(-6)).until(() -> getVelocity().lte(InchesPerSecond.of(2)))); + } + + public Command coral(int level) { + return switch (level) { + case 1 -> coralL1(); + case 2 -> coralL2(); + case 3 -> coralL3(); + case 4 -> coralL4(); + default -> throw new IllegalArgumentException("Invalid coral level: " + level); + }; + } + + public Command algae(int level) { + return switch (level) { + case 2 -> algaeL2(); + case 3 -> algaeL3(); + default -> throw new IllegalArgumentException("Invalid algae level: " + level); + }; + } + + public static Elevator create(Grabber grabber) { + return RobotBase.isReal() ? new Elevator(grabber) : new SimElevator(grabber); + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 61bd9c79..29e7c89d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -13,79 +13,80 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.measure.Voltage; public final class ElevatorConstants { - private ElevatorConstants() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - public static final Distance MIN_HEIGHT = Inches.of(41.50); - public static final Distance MAX_HEIGHT = Inches.of(73); - public static final Distance TOLERANCE = Inches.of(0.75); + private ElevatorConstants() { + throw new UnsupportedOperationException("This is a utility class!"); + } - public static final Mass ELEVATOR_MASS = Pounds.of(20.0); + public static final Distance MIN_HEIGHT = Inches.of(41.50); + public static final Distance MAX_HEIGHT = Inches.of(73); + public static final Distance TOLERANCE = Inches.of(0.75); - public static final double MOTOR_GEAR_REDUCTION = 50.0 / 12.0; - public static final Distance SPOOL_DIAMETER = Inches.of(2.148); - public static final double SENSOR_MECHANISM_RATIO = MOTOR_GEAR_REDUCTION / SPOOL_DIAMETER.times(Math.PI).in(Meters); + public static final Mass ELEVATOR_MASS = Pounds.of(20.0); - public static final int LEFT_MOTOR_ID = 5; - public static final int RIGHT_MOTOR_ID = 4; + public static final double MOTOR_GEAR_REDUCTION = 50.0 / 12.0; + public static final Distance SPOOL_DIAMETER = Inches.of(2.148); + public static final double SENSOR_MECHANISM_RATIO = + MOTOR_GEAR_REDUCTION / SPOOL_DIAMETER.times(Math.PI).in(Meters); - public static final InvertedValue LEFT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - public static final InvertedValue RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; + public static final int LEFT_MOTOR_ID = 5; + public static final int RIGHT_MOTOR_ID = 4; - public static final SlotConfigs emptySlot = new SlotConfigs() - .withKP(6) - .withKI(0.3) - .withKD(1) - .withKG(0.8195) - .withKV(3.13) - .withKA(0.079) - .withKS(0.2605) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); - public static final SlotConfigs coralSlot = new SlotConfigs() - .withKP(6) - .withKI(0.3) - .withKD(1) - .withKG(0.93) - .withKV(3.13) - .withKA(0.0795) - .withKS(0.2605) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); - public static final SlotConfigs algaeSlot = new SlotConfigs() - .withKP(6) - .withKI(0.3) - .withKD(1) - .withKG(1.0) - .withKV(3.13) - .withKA(0.081) - .withKS(0.2605) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); - public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(2) - .withMotionMagicAcceleration(3); - public static final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs() - .withSupplyCurrentLimit(90) - .withSupplyCurrentLimitEnable(true); - public static final MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake); + public static final InvertedValue LEFT_MOTOR_INVERTED_VALUE = + InvertedValue.CounterClockwise_Positive; + public static final InvertedValue RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; - // KG - KS correct - // KG + KS too low + public static final SlotConfigs emptySlot = + new SlotConfigs() + .withKP(6) + .withKI(0.3) + .withKD(1) + .withKG(0.8195) + .withKV(3.13) + .withKA(0.079) + .withKS(0.2605) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); + public static final SlotConfigs coralSlot = + new SlotConfigs() + .withKP(6) + .withKI(0.3) + .withKD(1) + .withKG(0.93) + .withKV(3.13) + .withKA(0.0795) + .withKS(0.2605) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); + public static final SlotConfigs algaeSlot = + new SlotConfigs() + .withKP(6) + .withKI(0.3) + .withKD(1) + .withKG(1.0) + .withKV(3.13) + .withKA(0.081) + .withKS(0.2605) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); + public static final MotionMagicConfigs motionMagicConfigs = + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(3); + public static final CurrentLimitsConfigs currentLimitsConfigs = + new CurrentLimitsConfigs().withSupplyCurrentLimit(90).withSupplyCurrentLimitEnable(true); + public static final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + // KG - KS correct + // KG + KS too low - public static final int DIO_FLOOR_PORT = 1; - public static final int DIO_CEILING_PORT = 0; + public static final int DIO_FLOOR_PORT = 1; + public static final int DIO_CEILING_PORT = 0; - public static final Voltage KG = Volts.of(0.8195); - public static final Voltage FINE_CONTROL_UP = KG.plus(Volts.of(1)); - public static final Voltage FINE_CONTROL_DOWN = KG.minus(Volts.of(1)); + public static final Voltage KG = Volts.of(0.8195); + public static final Voltage FINE_CONTROL_UP = KG.plus(Volts.of(1)); + public static final Voltage FINE_CONTROL_DOWN = KG.minus(Volts.of(1)); } diff --git a/src/main/java/frc/robot/subsystems/elevator/SimElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimElevator.java index 921ac769..e660895e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimElevator.java @@ -5,7 +5,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.sim.TalonFXSimState; - import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; @@ -13,79 +12,100 @@ import frc.robot.subsystems.manipulator.grabber.Grabber; public class SimElevator extends Elevator { - private TalonFXSimState leftSimState = new TalonFXSimState(leftMotor); - private TalonFXSimState rightSimState = new TalonFXSimState(rightMotor); - - private ElevatorSim elevatorSim = new ElevatorSim( - LinearSystemId.createElevatorSystem( - DCMotor.getKrakenX60Foc(2), - ElevatorConstants.ELEVATOR_MASS.in(Kilograms), - ElevatorConstants.SPOOL_DIAMETER.in(Meters) / 2.0, - ElevatorConstants.MOTOR_GEAR_REDUCTION - ), - DCMotor.getKrakenX60Foc(2), - ElevatorConstants.MIN_HEIGHT.in(Meters), - ElevatorConstants.MAX_HEIGHT.in(Meters), - true, - ElevatorConstants.MIN_HEIGHT.in(Meters) - ); - - public SimElevator(Grabber grabber) { - super(grabber); - } - - @Override - protected boolean topLimitSwitchTriggered() { - return elevatorSim.getPositionMeters() >= ElevatorConstants.MAX_HEIGHT.in(Meters) - 0.001; - } - - @Override - protected boolean bottomLimitSwitchTriggered() { - return elevatorSim.getPositionMeters() <= ElevatorConstants.MIN_HEIGHT.in(Meters) + 0.001; - } - - private double getLeftMotorVoltage() { - return (ElevatorConstants.LEFT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1) * leftSimState.getMotorVoltage(); - } - - private double getRightMotorVoltage() { - return (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1) * rightSimState.getMotorVoltage(); - } - - private void setLeftRotorPosition(double angle) { - leftSimState.setRawRotorPosition(angle * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1)); - } - - private void setRightRotorPosition(double angle) { - rightSimState.setRawRotorPosition(angle * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1)); - } - - private void setLeftRotorVelocity(double velocity) { - leftSimState.setRotorVelocity(velocity * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1)); - } - - private void setRightRotorVelocity(double velocity) { - rightSimState.setRotorVelocity(velocity * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive ? -1 : 1)); - } - - @Override - public void simulationPeriodic() { - elevatorSim.setInputVoltage((getLeftMotorVoltage() + getRightMotorVoltage()) / 2); - elevatorSim.update(0.02); - - // double sprocketCircumference = Math.PI * NewElevatorConstants.SPOOL_DIAMETER.in(Meters); - double position = elevatorSim.getPositionMeters(); - double velocity = elevatorSim.getVelocityMetersPerSecond(); - double motorAngle = position * ElevatorConstants.SENSOR_MECHANISM_RATIO; - double motorVelocity = velocity * ElevatorConstants.SENSOR_MECHANISM_RATIO; - - setLeftRotorPosition(motorAngle); - setRightRotorPosition(motorAngle); - - setLeftRotorVelocity(motorVelocity); - setRightRotorVelocity(motorVelocity); - - leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - } + private TalonFXSimState leftSimState = new TalonFXSimState(leftMotor); + private TalonFXSimState rightSimState = new TalonFXSimState(rightMotor); + + private ElevatorSim elevatorSim = + new ElevatorSim( + LinearSystemId.createElevatorSystem( + DCMotor.getKrakenX60Foc(2), + ElevatorConstants.ELEVATOR_MASS.in(Kilograms), + ElevatorConstants.SPOOL_DIAMETER.in(Meters) / 2.0, + ElevatorConstants.MOTOR_GEAR_REDUCTION), + DCMotor.getKrakenX60Foc(2), + ElevatorConstants.MIN_HEIGHT.in(Meters), + ElevatorConstants.MAX_HEIGHT.in(Meters), + true, + ElevatorConstants.MIN_HEIGHT.in(Meters)); + + public SimElevator(Grabber grabber) { + super(grabber); + } + + @Override + protected boolean topLimitSwitchTriggered() { + return elevatorSim.getPositionMeters() >= ElevatorConstants.MAX_HEIGHT.in(Meters) - 0.001; + } + + @Override + protected boolean bottomLimitSwitchTriggered() { + return elevatorSim.getPositionMeters() <= ElevatorConstants.MIN_HEIGHT.in(Meters) + 0.001; + } + + private double getLeftMotorVoltage() { + return (ElevatorConstants.LEFT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1) + * leftSimState.getMotorVoltage(); + } + + private double getRightMotorVoltage() { + return (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1) + * rightSimState.getMotorVoltage(); + } + + private void setLeftRotorPosition(double angle) { + leftSimState.setRawRotorPosition( + angle + * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1)); + } + + private void setRightRotorPosition(double angle) { + rightSimState.setRawRotorPosition( + angle + * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1)); + } + + private void setLeftRotorVelocity(double velocity) { + leftSimState.setRotorVelocity( + velocity + * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1)); + } + + private void setRightRotorVelocity(double velocity) { + rightSimState.setRotorVelocity( + velocity + * (ElevatorConstants.RIGHT_MOTOR_INVERTED_VALUE == InvertedValue.Clockwise_Positive + ? -1 + : 1)); + } + + @Override + public void simulationPeriodic() { + elevatorSim.setInputVoltage((getLeftMotorVoltage() + getRightMotorVoltage()) / 2); + elevatorSim.update(0.02); + + // double sprocketCircumference = Math.PI * NewElevatorConstants.SPOOL_DIAMETER.in(Meters); + double position = elevatorSim.getPositionMeters(); + double velocity = elevatorSim.getVelocityMetersPerSecond(); + double motorAngle = position * ElevatorConstants.SENSOR_MECHANISM_RATIO; + double motorVelocity = velocity * ElevatorConstants.SENSOR_MECHANISM_RATIO; + + setLeftRotorPosition(motorAngle); + setRightRotorPosition(motorAngle); + + setLeftRotorVelocity(motorVelocity); + setRightRotorVelocity(motorVelocity); + + leftSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + rightSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0d868588..af290ef5 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -12,157 +12,168 @@ import frc.robot.subsystems.manipulator.grabber.Grabber; /** - * The ground coral intake subsystem, which pulls coral off of the ground and - * into the robot. This subsystem consists of two controllable subsystems: the - * intake rollers, which spin to pull coral in, and the intake pivot, which - * pivots the rollers down to the ground to intake coral and up to stow the - * intake where it won't hit anything. + * The ground coral intake subsystem, which pulls coral off of the ground and into the robot. This + * subsystem consists of two controllable subsystems: the intake rollers, which spin to pull coral + * in, and the intake pivot, which pivots the rollers down to the ground to intake coral and up to + * stow the intake where it won't hit anything. + * *

Controlling the Intake

- * Generally, the intake should be only controlled via the {@link #intake()} and - * {@link #stow()} methods, which return {@link Command Commands}. The - * intake() command will lower the pivot and run the rollers until a piece of - * coral has been detected to have completely passed through the intake. The - * stow() command will raise the pivot to its stowed position, where it won't - * hit anything. + * + * Generally, the intake should be only controlled via the {@link #intake()} and {@link #stow()} + * methods, which return {@link Command Commands}. The intake() command will lower the pivot and run + * the rollers until a piece of coral has been detected to have completely passed through the + * intake. The stow() command will raise the pivot to its stowed position, where it won't hit + * anything. + * *

Getting the Intake's State

- * The {@link #coralInIntake()} method can be used to check whether the intake - * currently has a piece of coral in it. This is primarily useful in autonomous, - * where the autonomous code may want to wait until the intake's beam break - * sensor has detected a piece of coral before driving to score the coral. + * + * The {@link #coralInIntake()} method can be used to check whether the intake currently has a piece + * of coral in it. This is primarily useful in autonomous, where the autonomous code may want to + * wait until the intake's beam break sensor has detected a piece of coral before driving to score + * the coral. + * *

Simulation

- * This class is compatible with both real and simulated hardware. In - * simulation, the Intake will automatically switch to using simulated versions - * of the rollers, pivot, and sensor subsystems that fully simulate the physics - * and device behavior of those subsystems. + * + * This class is compatible with both real and simulated hardware. In simulation, the Intake will + * automatically switch to using simulated versions of the rollers, pivot, and sensor subsystems + * that fully simulate the physics and device behavior of those subsystems. */ public class Intake extends SubsystemBase { - public final IntakeRollers rollers; - public final IntakePivot pivot; - public final Indexer indexer; - public final IntakeSensors sensors; - - /** - * Creates a new Intake subsystem. - */ - public Intake(Grabber grabber) { - indexer = new Indexer(); - rollers = new IntakeRollers(); - sensors = new IntakeSensors(grabber, indexer); - - if (RobotBase.isSimulation()) { - pivot = new IntakePivotSim(sensors); - } else { - pivot = new IntakePivot(sensors); - } + public final IntakeRollers rollers; + public final IntakePivot pivot; + public final Indexer indexer; + public final IntakeSensors sensors; + + /** Creates a new Intake subsystem. */ + public Intake(Grabber grabber) { + indexer = new Indexer(); + rollers = new IntakeRollers(); + sensors = new IntakeSensors(grabber, indexer); + + if (RobotBase.isSimulation()) { + pivot = new IntakePivotSim(sensors); + } else { + pivot = new IntakePivot(sensors); } - - /** - * Returns a command that intakes a piece of coral from the ground. This - * command will lower the pivot and run the rollers until a piece of coral - * has been detected to have completely passed through the intake, and it is - * safe to raise the intake again. - * @return the command that intakes a piece of coral from the ground - */ - public Command intake() { - Command command = Commands.parallel( + } + + /** + * Returns a command that intakes a piece of coral from the ground. This command will lower the + * pivot and run the rollers until a piece of coral has been detected to have completely passed + * through the intake, and it is safe to raise the intake again. + * + * @return the command that intakes a piece of coral from the ground + */ + public Command intake() { + Command command = + Commands.parallel( rollers.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER), - pivot.deploy().until(() -> sensors.getCoralLocation() == CoralLocation.INTAKE || sensors.getCoralLocation() == CoralLocation.INDEXER), - indexer.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER) - ); - - if (RobotBase.isSimulation()) { - command = command.deadlineFor(Commands.sequence( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> sensors.simIntakeCoral()), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> sensors.simIndexCoral()) - )); - } - - return command; + pivot + .deploy() + .until( + () -> + sensors.getCoralLocation() == CoralLocation.INTAKE + || sensors.getCoralLocation() == CoralLocation.INDEXER), + indexer.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER)); + + if (RobotBase.isSimulation()) { + command = + command.deadlineFor( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> sensors.simIntakeCoral()), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> sensors.simIndexCoral()))); } - public Command intakeVertical() { - Command command = Commands.parallel( + return command; + } + + public Command intakeVertical() { + Command command = + Commands.parallel( rollers.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER), - pivot.deployVertical().until(() -> sensors.getCoralLocation() == CoralLocation.INTAKE || sensors.getCoralLocation() == CoralLocation.INDEXER), - indexer.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER) - ); - - if (RobotBase.isSimulation()) { - command = command.deadlineFor(Commands.sequence( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> sensors.simIntakeCoral()), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> sensors.simIndexCoral()) - )); - } - - return command; + pivot + .deployVertical() + .until( + () -> + sensors.getCoralLocation() == CoralLocation.INTAKE + || sensors.getCoralLocation() == CoralLocation.INDEXER), + indexer.intake().until(() -> sensors.getCoralLocation() == CoralLocation.INDEXER)); + + if (RobotBase.isSimulation()) { + command = + command.deadlineFor( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> sensors.simIntakeCoral()), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> sensors.simIndexCoral()))); } - /** - * Returns a command that drops a piece of coral from the indexer onto the - * ground. This command will stow the pivot, run the indexer in - * reverse until the coral has exited the robot, and then run the indexer - * for a short time to ensure the coral is fully clear of the robot. - * @return the command that drops a piece of coral from the indexer - */ - public Command drop() { - Command command = Commands.either( - Commands.sequence( - pivot.stow(), - indexer.drop() - ), - Commands.parallel( - pivot.deploy(), - indexer.drop(), - rollers.drop() - ), - () -> sensors.getCoralLocation() == CoralLocation.INDEXER || sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_MANIPULATOR - ).finallyDo(() -> sensors.setCoralDropped()); - - if (RobotBase.isSimulation()) { - command = command.deadlineFor(Commands.sequence( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> sensors.simDropCoral()) - )); - } - - return command; + return command; + } + + /** + * Returns a command that drops a piece of coral from the indexer onto the ground. This command + * will stow the pivot, run the indexer in reverse until the coral has exited the robot, and then + * run the indexer for a short time to ensure the coral is fully clear of the robot. + * + * @return the command that drops a piece of coral from the indexer + */ + public Command drop() { + Command command = + Commands.either( + Commands.sequence(pivot.stow(), indexer.drop()), + Commands.parallel(pivot.deploy(), indexer.drop(), rollers.drop()), + () -> + sensors.getCoralLocation() == CoralLocation.INDEXER + || sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_MANIPULATOR) + .finallyDo(() -> sensors.setCoralDropped()); + + if (RobotBase.isSimulation()) { + command = + command.deadlineFor( + Commands.sequence( + Commands.waitSeconds(0.25), Commands.runOnce(() -> sensors.simDropCoral()))); } - /** - * Returns a command that stows the intake by raising the pivot to its - * stowed position, where it won't hit anything. - * @return the command that stows the intake - */ - public Command stow() { - return pivot.stow(); + return command; + } + + /** + * Returns a command that stows the intake by raising the pivot to its stowed position, where it + * won't hit anything. + * + * @return the command that stows the intake + */ + public Command stow() { + return pivot.stow(); + } + + public Command transfer() { + Command command = + indexer.intake().until(() -> sensors.getCoralLocation() == CoralLocation.OUTSIDE); + + if (RobotBase.isSimulation()) { + command = + command.deadlineFor( + Commands.sequence( + Commands.waitSeconds(0.25), Commands.runOnce(() -> sensors.simTransferCoral()))); } - public Command transfer() { - Command command = indexer.intake() - .until(() -> sensors.getCoralLocation() == CoralLocation.OUTSIDE); - - if (RobotBase.isSimulation()) { - command = command.deadlineFor(Commands.sequence( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> sensors.simTransferCoral()) - )); - } + return command; + } - return command; - } + public CoralLocation getCoralLocation() { + return sensors.getCoralLocation(); + } - public CoralLocation getCoralLocation() { - return sensors.getCoralLocation(); - } - - @Override - public void periodic() { - // if (rollers.getCurrentCommand() == null && sensors.getCoralLocation() == CoralLocation.TRANSFER_TO_INDEXER || sensors.getCoralLocation() == CoralLocation.INTAKE) { - // intake().schedule(); - // } - } + @Override + public void periodic() { + // if (rollers.getCurrentCommand() == null && sensors.getCoralLocation() == + // CoralLocation.TRANSFER_TO_INDEXER || sensors.getCoralLocation() == CoralLocation.INTAKE) { + // intake().schedule(); + // } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index b32afa78..cb132827 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -18,7 +18,6 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.team6962.lib.digitalsensor.DigitalSensor; - import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; @@ -28,87 +27,83 @@ import edu.wpi.first.units.measure.Voltage; public final class IntakeConstants { - private IntakeConstants() {} - - public static final int pivotMotorID = 2; - public static final int absoluteEncoderID = 1; - public static final int rollerMotorID = 3; - public static final String canBus = "drivetrain"; - - public static final int intakeSensorChannel = 4; - public static final DigitalSensor.Wiring intakeSensorWiring = DigitalSensor.Wiring.NormallyOpen; - - public static final TalonFXConfiguration pivotMotorConfiguration = new TalonFXConfiguration() - .withSlot0( - new Slot0Configs() - .withKP(72) - .withKD(27) - .withKV(0) - .withKA(0) - .withKS(4.2) - .withKG(0) // DO NOT USE - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) - ) - .withTorqueCurrent( - new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(Amps.of(60)) - .withPeakReverseTorqueCurrent(Amps.of(-60)) - ) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - ); - public static final Current kG = Amps.of(11); - public static final double pivotSensorToMechanism = 2.25; - public static final double pivotRotorToSensor = 24.0; // 49 - 15 amps - I = Kg * cos(-1 deg) => 15 / 0.9998476952 = Kg - - public static final Angle absoluteEncoderOffset = Degrees.of(47.7); - public static final Angle centerOfMassAngularOffset = Degrees.of(-40); - public static final SensorDirectionValue absoluteEncoderDirection = SensorDirectionValue.Clockwise_Positive; - public static final Angle absoluteEncoderDiscontinuityPoint = Degrees.of(10); - - public static final Angle pivotMinAngle = Degrees.of(21); - public static final Angle pivotMaxAngle = Degrees.of(157); - - public static final Angle pivotIntakeAngle = Degrees.of(21); - public static final Angle pivotStowAngle = Degrees.of(150); - public static final Angle pivotVerticalAngle = Degrees.of(50); - - public static final Angle pivotTolerance = Degrees.of(4); - - public static final TalonFXConfiguration rollerMotorConfiguration = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - ); - public static final Voltage rollerIntakeVoltage = Volts.of(12); - public static final Voltage rollerDropVoltage = Volts.of(-12); - - public static final Time delayAfterIntake = Seconds.of(0); - - public static final int indexerMotorId = 1; - - public static final TalonFXConfiguration indexerMotorConfiguration = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - ); - - public static final int indexerSensorChannel = 6; - public static final DigitalSensor.Wiring indexerSensorWiring = DigitalSensor.Wiring.NormallyOpen; - - public static final Voltage indexerIntakeVoltage = Volts.of(8); - public static final Voltage indexerDropVoltage = Volts.of(-10); - - public static final class Simulation { - private Simulation() {} - - public static final DCMotor pivotMotor = DCMotor.getKrakenX60Foc(1); - public static final MomentOfInertia pivotMomentOfInertia = KilogramSquareMeters.of(Pounds.of(10).in(Kilograms) * Inches.of(9.47).in(Meters) * Inches.of(9.47).in(Meters)); - public static final Distance pivotCenterOfMassDistance = Inches.of(9.47); - public static final Angle pivotStartAngle = pivotStowAngle; - - public static final Time intakeSensorDetectionTime = Seconds.of(0.125); - public static final Time indexerSensorDetectionTime = Seconds.of(0.125); - } + private IntakeConstants() {} + + public static final int pivotMotorID = 2; + public static final int absoluteEncoderID = 1; + public static final int rollerMotorID = 3; + public static final String canBus = "drivetrain"; + + public static final int intakeSensorChannel = 4; + public static final DigitalSensor.Wiring intakeSensorWiring = DigitalSensor.Wiring.NormallyOpen; + + public static final TalonFXConfiguration pivotMotorConfiguration = + new TalonFXConfiguration() + .withSlot0( + new Slot0Configs() + .withKP(72) + .withKD(27) + .withKV(0) + .withKA(0) + .withKS(4.2) + .withKG(0) // DO NOT USE + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)) + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(Amps.of(60)) + .withPeakReverseTorqueCurrent(Amps.of(-60))) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + public static final Current kG = Amps.of(11); + public static final double pivotSensorToMechanism = 2.25; + public static final double pivotRotorToSensor = + 24.0; // 49 - 15 amps - I = Kg * cos(-1 deg) => 15 / 0.9998476952 = Kg + + public static final Angle absoluteEncoderOffset = Degrees.of(47.7); + public static final Angle centerOfMassAngularOffset = Degrees.of(-40); + public static final SensorDirectionValue absoluteEncoderDirection = + SensorDirectionValue.Clockwise_Positive; + public static final Angle absoluteEncoderDiscontinuityPoint = Degrees.of(10); + + public static final Angle pivotMinAngle = Degrees.of(21); + public static final Angle pivotMaxAngle = Degrees.of(157); + + public static final Angle pivotIntakeAngle = Degrees.of(21); + public static final Angle pivotStowAngle = Degrees.of(150); + public static final Angle pivotVerticalAngle = Degrees.of(50); + + public static final Angle pivotTolerance = Degrees.of(4); + + public static final TalonFXConfiguration rollerMotorConfiguration = + new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + public static final Voltage rollerIntakeVoltage = Volts.of(12); + public static final Voltage rollerDropVoltage = Volts.of(-12); + + public static final Time delayAfterIntake = Seconds.of(0); + + public static final int indexerMotorId = 1; + + public static final TalonFXConfiguration indexerMotorConfiguration = + new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + + public static final int indexerSensorChannel = 6; + public static final DigitalSensor.Wiring indexerSensorWiring = DigitalSensor.Wiring.NormallyOpen; + + public static final Voltage indexerIntakeVoltage = Volts.of(8); + public static final Voltage indexerDropVoltage = Volts.of(-10); + + public static final class Simulation { + private Simulation() {} + + public static final DCMotor pivotMotor = DCMotor.getKrakenX60Foc(1); + public static final MomentOfInertia pivotMomentOfInertia = + KilogramSquareMeters.of( + Pounds.of(10).in(Kilograms) * Inches.of(9.47).in(Meters) * Inches.of(9.47).in(Meters)); + public static final Distance pivotCenterOfMassDistance = Inches.of(9.47); + public static final Angle pivotStartAngle = pivotStowAngle; + + public static final Time intakeSensorDetectionTime = Seconds.of(0.125); + public static final Time indexerSensorDetectionTime = Seconds.of(0.125); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSensors.java b/src/main/java/frc/robot/subsystems/intake/IntakeSensors.java index 54c307c8..8028ff7a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSensors.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSensors.java @@ -2,104 +2,115 @@ import com.team6962.lib.digitalsensor.BeamBreak; import com.team6962.lib.telemetry.Logger; - import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.intake.indexer.Indexer; import frc.robot.subsystems.manipulator.grabber.Grabber; public class IntakeSensors extends SubsystemBase { - private final BeamBreak intakeSensor; - private final BeamBreak indexerSensor; - private final Indexer indexer; - private final Grabber grabber; + private final BeamBreak intakeSensor; + private final BeamBreak indexerSensor; + private final Indexer indexer; + private final Grabber grabber; - private CoralLocation location; + private CoralLocation location; - public static enum CoralLocation { - OUTSIDE("Outside of Intake and Indexer"), - INTAKE("Intake"), - TRANSFER_TO_INDEXER("Transferring from Intake to Indexer"), - INDEXER("Indexer"), - TRANSFER_TO_MANIPULATOR("Transferring to Manipulator"); + public static enum CoralLocation { + OUTSIDE("Outside of Intake and Indexer"), + INTAKE("Intake"), + TRANSFER_TO_INDEXER("Transferring from Intake to Indexer"), + INDEXER("Indexer"), + TRANSFER_TO_MANIPULATOR("Transferring to Manipulator"); - public final String name; + public final String name; - private CoralLocation(String name) { - this.name = name; - } + private CoralLocation(String name) { + this.name = name; } - - public IntakeSensors(Grabber grabber, Indexer indexer) { - intakeSensor = new BeamBreak(IntakeConstants.intakeSensorChannel, IntakeConstants.intakeSensorWiring, IntakeConstants.Simulation.intakeSensorDetectionTime); - indexerSensor = new BeamBreak(IntakeConstants.indexerSensorChannel, IntakeConstants.indexerSensorWiring, IntakeConstants.Simulation.indexerSensorDetectionTime); - this.grabber = grabber; - this.indexer = indexer; - this.location = CoralLocation.OUTSIDE; - - Logger.logDigitalSensor("Intake/intakeCoralDetected", intakeSensor); - Logger.logDigitalSensor("Intake/indexerCoralDetected", indexerSensor); - Logger.logString("Intake/coralLocation", () -> location.name); + } + + public IntakeSensors(Grabber grabber, Indexer indexer) { + intakeSensor = + new BeamBreak( + IntakeConstants.intakeSensorChannel, + IntakeConstants.intakeSensorWiring, + IntakeConstants.Simulation.intakeSensorDetectionTime); + indexerSensor = + new BeamBreak( + IntakeConstants.indexerSensorChannel, + IntakeConstants.indexerSensorWiring, + IntakeConstants.Simulation.indexerSensorDetectionTime); + this.grabber = grabber; + this.indexer = indexer; + this.location = CoralLocation.OUTSIDE; + + Logger.logDigitalSensor("Intake/intakeCoralDetected", intakeSensor); + Logger.logDigitalSensor("Intake/indexerCoralDetected", indexerSensor); + Logger.logString("Intake/coralLocation", () -> location.name); + } + + public CoralLocation getCoralLocation() { + return location; + } + + public void simIntakeCoral() { + if (RobotBase.isReal()) return; + location = CoralLocation.INTAKE; + } + + public void simIndexCoral() { + if (RobotBase.isReal()) return; + location = CoralLocation.INDEXER; + } + + public void simTransferCoral() { + if (RobotBase.isReal()) return; + location = CoralLocation.TRANSFER_TO_MANIPULATOR; + } + + public void simDropCoral() { + if (RobotBase.isReal()) return; + location = CoralLocation.OUTSIDE; + } + + public void setCoralDropped() { + if (location == CoralLocation.INDEXER + || location == CoralLocation.TRANSFER_TO_INDEXER + || location == CoralLocation.TRANSFER_TO_MANIPULATOR) { + location = CoralLocation.OUTSIDE; } + } - public CoralLocation getCoralLocation() { - return location; + @Override + public void periodic() { + if (location == CoralLocation.INDEXER && !grabber.isCoralClear()) { + location = CoralLocation.TRANSFER_TO_MANIPULATOR; } - public void simIntakeCoral() { - if (RobotBase.isReal()) return; - location = CoralLocation.INTAKE; + if (location == CoralLocation.INDEXER && !indexerSensor.isTriggered()) { + location = CoralLocation.OUTSIDE; } - public void simIndexCoral() { - if (RobotBase.isReal()) return; - location = CoralLocation.INDEXER; + if (location == CoralLocation.TRANSFER_TO_MANIPULATOR + && grabber.isCoralClear() + && grabber.hasCoral()) { + location = CoralLocation.OUTSIDE; } - public void simTransferCoral() { - if (RobotBase.isReal()) return; - location = CoralLocation.TRANSFER_TO_MANIPULATOR; + if (location == CoralLocation.OUTSIDE && intakeSensor.isTriggered()) { + location = CoralLocation.INTAKE; } - public void simDropCoral() { - if (RobotBase.isReal()) return; - location = CoralLocation.OUTSIDE; + if (location == CoralLocation.INDEXER && !intakeSensor.isTriggered() && indexer.isDropping()) { + location = CoralLocation.OUTSIDE; } - public void setCoralDropped() { - if (location == CoralLocation.INDEXER || location == CoralLocation.TRANSFER_TO_INDEXER || location == CoralLocation.TRANSFER_TO_MANIPULATOR) { - location = CoralLocation.OUTSIDE; - } + if (location == CoralLocation.INTAKE && !intakeSensor.isTriggered()) { + location = CoralLocation.TRANSFER_TO_INDEXER; } - @Override - public void periodic() { - if (location == CoralLocation.INDEXER && !grabber.isCoralClear()) { - location = CoralLocation.TRANSFER_TO_MANIPULATOR; - } - - if (location == CoralLocation.INDEXER && !indexerSensor.isTriggered()) { - location = CoralLocation.OUTSIDE; - } - - if (location == CoralLocation.TRANSFER_TO_MANIPULATOR && grabber.isCoralClear() && grabber.hasCoral()) { - location = CoralLocation.OUTSIDE; - } - - if (location == CoralLocation.OUTSIDE && intakeSensor.isTriggered()) { - location = CoralLocation.INTAKE; - } - - if (location == CoralLocation.INDEXER && !intakeSensor.isTriggered() && indexer.isDropping()) { - location = CoralLocation.OUTSIDE; - } - - if (location == CoralLocation.INTAKE && !intakeSensor.isTriggered()) { - location = CoralLocation.TRANSFER_TO_INDEXER; - } - - if (location == CoralLocation.TRANSFER_TO_INDEXER && indexerSensor.isTriggered()) { - location = CoralLocation.INDEXER; - } + if (location == CoralLocation.TRANSFER_TO_INDEXER && indexerSensor.isTriggered()) { + location = CoralLocation.INDEXER; } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/intake/indexer/Indexer.java index 3b34b8c3..ac8f5f39 100644 --- a/src/main/java/frc/robot/subsystems/intake/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/intake/indexer/Indexer.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.CTREUtils; - import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -20,110 +19,100 @@ import frc.robot.subsystems.intake.IntakeConstants; public class Indexer extends SubsystemBase { - protected TalonFX motor; - - private StatusSignal velocityIn; - private StatusSignal statorCurrentIn; - private StatusSignal supplyCurrentIn; - private StatusSignal appliedVoltageIn; - - private Voltage simVoltage; - - private boolean isIntaking = false; - private boolean isDropping = false; - - public Indexer() { - motor = new TalonFX(IntakeConstants.indexerMotorId, IntakeConstants.canBus); - - motor.getConfigurator().apply(IntakeConstants.indexerMotorConfiguration); - - initStatusSignals(); - - if (RobotBase.isReal()) { - Logger.logMeasure("Intake/Indexer/velocity", () -> CTREUtils.unwrap(velocityIn)); - Logger.logMeasure("Intake/Indexer/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); - Logger.logMeasure("Intake/Indexer/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); - Logger.logMeasure("Intake/Indexer/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); - } else { - Logger.logMeasure("Intake/Indexer/appliedVoltage", () -> simVoltage); - } - } - - private void initStatusSignals() { - velocityIn = motor.getVelocity(); - statorCurrentIn = motor.getStatorCurrent(); - supplyCurrentIn = motor.getSupplyCurrent(); - appliedVoltageIn = motor.getMotorVoltage(); - - CTREUtils.check(BaseStatusSignal.setUpdateFrequencyForAll( - Hertz.of(50), - velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); + protected TalonFX motor; - CTREUtils.check(motor.optimizeBusUtilization()); - } + private StatusSignal velocityIn; + private StatusSignal statorCurrentIn; + private StatusSignal supplyCurrentIn; + private StatusSignal appliedVoltageIn; - private void refreshStatusSignals() { - CTREUtils.check(BaseStatusSignal.refreshAll( - velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); - } + private Voltage simVoltage; - private void setVoltage(Voltage voltage) { - if (RobotBase.isSimulation()) { - simVoltage = voltage; - } + private boolean isIntaking = false; + private boolean isDropping = false; - motor.setVoltage(voltage.in(Volts)); - } - - private Command run(Voltage voltage) { - return startEnd( - () -> setVoltage(voltage), - () -> setVoltage(Volts.of(0)) - ); - } + public Indexer() { + motor = new TalonFX(IntakeConstants.indexerMotorId, IntakeConstants.canBus); - /** - * Runs the indexer motor to move a piece of coral into the manipulator. The - * returned command does not end on its own; it must be interrupted or - * canceled. - * - * @return A command that runs the indexer motor to intake coral. - */ - public Command intake() { - return run(IntakeConstants.indexerIntakeVoltage).alongWith(Commands.startEnd( - () -> isIntaking = true, - () -> isIntaking = false - )); - } + motor.getConfigurator().apply(IntakeConstants.indexerMotorConfiguration); - /** - * Runs the indexer motor in reverse to drop a piece of coral out of the - * robot. The returned command does not end on its own; it must be - * interrupted or canceled. - * - * @return A command that runs the indexer motor to drop coral. - */ - public Command drop() { - return run(IntakeConstants.indexerDropVoltage).alongWith(Commands.startEnd( - () -> isDropping = true, - () -> isDropping = false - )); - } + initStatusSignals(); - public boolean isDropping() { - return isDropping; + if (RobotBase.isReal()) { + Logger.logMeasure("Intake/Indexer/velocity", () -> CTREUtils.unwrap(velocityIn)); + Logger.logMeasure("Intake/Indexer/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); + Logger.logMeasure("Intake/Indexer/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); + Logger.logMeasure("Intake/Indexer/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); + } else { + Logger.logMeasure("Intake/Indexer/appliedVoltage", () -> simVoltage); } - - public boolean isIntaking() { - return isIntaking; + } + + private void initStatusSignals() { + velocityIn = motor.getVelocity(); + statorCurrentIn = motor.getStatorCurrent(); + supplyCurrentIn = motor.getSupplyCurrent(); + appliedVoltageIn = motor.getMotorVoltage(); + + CTREUtils.check( + BaseStatusSignal.setUpdateFrequencyForAll( + Hertz.of(50), velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + + CTREUtils.check(motor.optimizeBusUtilization()); + } + + private void refreshStatusSignals() { + CTREUtils.check( + BaseStatusSignal.refreshAll( + velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + } + + private void setVoltage(Voltage voltage) { + if (RobotBase.isSimulation()) { + simVoltage = voltage; } - @Override - public void periodic() { - refreshStatusSignals(); - - if (RobotState.isDisabled()) setVoltage(Volts.of(0)); - } + motor.setVoltage(voltage.in(Volts)); + } + + private Command run(Voltage voltage) { + return startEnd(() -> setVoltage(voltage), () -> setVoltage(Volts.of(0))); + } + + /** + * Runs the indexer motor to move a piece of coral into the manipulator. The returned command does + * not end on its own; it must be interrupted or canceled. + * + * @return A command that runs the indexer motor to intake coral. + */ + public Command intake() { + return run(IntakeConstants.indexerIntakeVoltage) + .alongWith(Commands.startEnd(() -> isIntaking = true, () -> isIntaking = false)); + } + + /** + * Runs the indexer motor in reverse to drop a piece of coral out of the robot. The returned + * command does not end on its own; it must be interrupted or canceled. + * + * @return A command that runs the indexer motor to drop coral. + */ + public Command drop() { + return run(IntakeConstants.indexerDropVoltage) + .alongWith(Commands.startEnd(() -> isDropping = true, () -> isDropping = false)); + } + + public boolean isDropping() { + return isDropping; + } + + public boolean isIntaking() { + return isIntaking; + } + + @Override + public void periodic() { + refreshStatusSignals(); + + if (RobotState.isDisabled()) setVoltage(Volts.of(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivot.java b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivot.java index 13a155ab..2aebfa73 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivot.java @@ -5,8 +5,6 @@ import static edu.wpi.first.units.Units.Hertz; import static edu.wpi.first.units.Units.Radians; -import java.util.Set; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -22,7 +20,6 @@ import com.team6962.lib.telemetry.MechanismLogger; import com.team6962.lib.utils.CTREUtils; import com.team6962.lib.utils.MeasureMath; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -35,291 +32,313 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeSensors; +import java.util.Set; /** * The intake pivot subsystem, which pivots the ground coral intake up and down. + * *

Controlling the Pivot

- * The pivot can be controlled with the {@link #deploy()} method and - * {@link #stow()} method, which return {@link Command Commands}. + * + * The pivot can be controlled with the {@link #deploy()} method and {@link #stow()} method, which + * return {@link Command Commands}. + * *

Getting the Pivot's State

- * The position and velocity of the pivot can be read with the - * {@link #getPosition()} and {@link #getVelocity()} methods. + * + * The position and velocity of the pivot can be read with the {@link #getPosition()} and {@link + * #getVelocity()} methods. + * *

Automatic Holding Behavior

- * When no command is actively controlling the pivot with an ongoing - * {@link #moveTo()} or {@link #holdAt()} command, the pivot will use feedback - * control to stay in its current position. + * + * When no command is actively controlling the pivot with an ongoing {@link #moveTo()} or {@link + * #holdAt()} command, the pivot will use feedback control to stay in its current position. + * *

Simulation

- * In simulation, instantiate an instance of the {@link IntakePivotSim} subclass - * instead of this one. The IntakePivotSim class fully simulates the pivot's - * physics and motor behavior. + * + * In simulation, instantiate an instance of the {@link IntakePivotSim} subclass instead of this + * one. The IntakePivotSim class fully simulates the pivot's physics and motor behavior. + * *

Internal Position Offset

- * The TalonFX's position is offset by the angle between the center of mass and - * the external code's zero angle. This means that if the center of mass is - * 10 degrees above the zero angle, then when the external code sets the pivot to - * 0 degrees, the TalonFX will actually be set to 10 degrees. This offset is - * handled internally and does not affect the external code's view of the - * pivot's position. + * + * The TalonFX's position is offset by the angle between the center of mass and the external code's + * zero angle. This means that if the center of mass is 10 degrees above the zero angle, then when + * the external code sets the pivot to 0 degrees, the TalonFX will actually be set to 10 degrees. + * This offset is handled internally and does not affect the external code's view of the pivot's + * position. */ public class IntakePivot extends SubsystemBase { - protected TalonFX motor; - protected CANcoder encoder; - - private IntakeSensors sensors; - - private StatusSignal positionIn; - private StatusSignal velocityIn; - private StatusSignal statorCurrentIn; - private StatusSignal supplyCurrentIn; - private StatusSignal appliedVoltageIn; - - private ControlRequest controlRequest; - private Angle targetPosition; - - /** - * Creates a new IntakePivot. - */ - public IntakePivot(IntakeSensors sensors) { - this.sensors = sensors; - - motor = new TalonFX(IntakeConstants.pivotMotorID, IntakeConstants.canBus); - encoder = new CANcoder(IntakeConstants.absoluteEncoderID, IntakeConstants.canBus); - - TalonFXConfiguration motorConfig = IntakeConstants.pivotMotorConfiguration; - - motorConfig.Feedback - .withRotorToSensorRatio(IntakeConstants.pivotRotorToSensor) - .withSensorToMechanismRatio(IntakeConstants.pivotSensorToMechanism) - .withRemoteCANcoder(encoder); - - modifyConfiguration(motorConfig); - - CTREUtils.check(motor.getConfigurator().apply(motorConfig)); - - CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - - encoderConfig.MagnetSensor - .withSensorDirection(IntakeConstants.absoluteEncoderDirection) - .withMagnetOffset(IntakeConstants.absoluteEncoderOffset.times(IntakeConstants.pivotSensorToMechanism)) - .withAbsoluteSensorDiscontinuityPoint(IntakeConstants.absoluteEncoderDiscontinuityPoint.times(IntakeConstants.pivotSensorToMechanism)); - - modifyConfiguration(encoderConfig); - - CTREUtils.check(encoder.getConfigurator().apply(encoderConfig)); - - initStatusSignals(); - - Logger.logMeasure("Intake/Pivot/position", this::getPosition); - Logger.logMeasure("Intake/Pivot/targetPosition", () -> targetPosition); - Logger.logMeasure("Intake/Pivot/velocity", this::getVelocity); - Logger.logMeasure("Intake/Pivot/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); - Logger.logMeasure("Intake/Pivot/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); - Logger.logMeasure("Intake/Pivot/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); - - MechanismRoot2d root = MechanismLogger.getRoot("Intake", 13.0, 7.8); - MechanismLigament2d ligament = MechanismLogger.getLigament("Intake Pivot", 14.4, getCenterOfMassPosition().in(Degrees)); - - root.append(ligament); - - MechanismLogger.addDynamicAngle(ligament, this::getCenterOfMassPosition); - - // While using a hold command as the default command is problematic, - // because in sequential command groups the mechanism will sag when not - // actively being held, using the stow command as default is really - // useful here. In this case, we want to the pivot to not be stowed when - // a sequential command is running, but we want it to stow automatically - // when no command are running that require it - setDefaultCommand(stow().repeatedly()); - } - - private Angle getCenterOfMassPosition() { - return getPosition().plus(IntakeConstants.centerOfMassAngularOffset); - } - - protected TalonFXConfiguration modifyConfiguration(TalonFXConfiguration config) { - return config; - } - - protected CANcoderConfiguration modifyConfiguration(CANcoderConfiguration config) { - return config; - } - - /** - * Gets the current position of the pivot. - * @return The current position of the pivot, in a measure. - */ - public Angle getPosition() { - return MeasureMath.toAngle(BaseStatusSignal.getLatencyCompensatedValue(positionIn, velocityIn)).plus(Degrees.of(160)); - } - - /** - * Gets the current velocity of the pivot. - * @return The current velocity of the pivot, in a measure. - */ - public AngularVelocity getVelocity() { - return CTREUtils.unwrap(velocityIn); + protected TalonFX motor; + protected CANcoder encoder; + + private IntakeSensors sensors; + + private StatusSignal positionIn; + private StatusSignal velocityIn; + private StatusSignal statorCurrentIn; + private StatusSignal supplyCurrentIn; + private StatusSignal appliedVoltageIn; + + private ControlRequest controlRequest; + private Angle targetPosition; + + /** Creates a new IntakePivot. */ + public IntakePivot(IntakeSensors sensors) { + this.sensors = sensors; + + motor = new TalonFX(IntakeConstants.pivotMotorID, IntakeConstants.canBus); + encoder = new CANcoder(IntakeConstants.absoluteEncoderID, IntakeConstants.canBus); + + TalonFXConfiguration motorConfig = IntakeConstants.pivotMotorConfiguration; + + motorConfig + .Feedback + .withRotorToSensorRatio(IntakeConstants.pivotRotorToSensor) + .withSensorToMechanismRatio(IntakeConstants.pivotSensorToMechanism) + .withRemoteCANcoder(encoder); + + modifyConfiguration(motorConfig); + + CTREUtils.check(motor.getConfigurator().apply(motorConfig)); + + CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); + + encoderConfig + .MagnetSensor + .withSensorDirection(IntakeConstants.absoluteEncoderDirection) + .withMagnetOffset( + IntakeConstants.absoluteEncoderOffset.times(IntakeConstants.pivotSensorToMechanism)) + .withAbsoluteSensorDiscontinuityPoint( + IntakeConstants.absoluteEncoderDiscontinuityPoint.times( + IntakeConstants.pivotSensorToMechanism)); + + modifyConfiguration(encoderConfig); + + CTREUtils.check(encoder.getConfigurator().apply(encoderConfig)); + + initStatusSignals(); + + Logger.logMeasure("Intake/Pivot/position", this::getPosition); + Logger.logMeasure("Intake/Pivot/targetPosition", () -> targetPosition); + Logger.logMeasure("Intake/Pivot/velocity", this::getVelocity); + Logger.logMeasure("Intake/Pivot/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); + Logger.logMeasure("Intake/Pivot/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); + Logger.logMeasure("Intake/Pivot/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); + + MechanismRoot2d root = MechanismLogger.getRoot("Intake", 13.0, 7.8); + MechanismLigament2d ligament = + MechanismLogger.getLigament("Intake Pivot", 14.4, getCenterOfMassPosition().in(Degrees)); + + root.append(ligament); + + MechanismLogger.addDynamicAngle(ligament, this::getCenterOfMassPosition); + + // While using a hold command as the default command is problematic, + // because in sequential command groups the mechanism will sag when not + // actively being held, using the stow command as default is really + // useful here. In this case, we want to the pivot to not be stowed when + // a sequential command is running, but we want it to stow automatically + // when no command are running that require it + setDefaultCommand(stow().repeatedly()); + } + + private Angle getCenterOfMassPosition() { + return getPosition().plus(IntakeConstants.centerOfMassAngularOffset); + } + + protected TalonFXConfiguration modifyConfiguration(TalonFXConfiguration config) { + return config; + } + + protected CANcoderConfiguration modifyConfiguration(CANcoderConfiguration config) { + return config; + } + + /** + * Gets the current position of the pivot. + * + * @return The current position of the pivot, in a measure. + */ + public Angle getPosition() { + return MeasureMath.toAngle(BaseStatusSignal.getLatencyCompensatedValue(positionIn, velocityIn)) + .plus(Degrees.of(160)); + } + + /** + * Gets the current velocity of the pivot. + * + * @return The current velocity of the pivot, in a measure. + */ + public AngularVelocity getVelocity() { + return CTREUtils.unwrap(velocityIn); + } + + private boolean isSafeToMove() { + return sensors.getCoralLocation() != IntakeSensors.CoralLocation.TRANSFER_TO_INDEXER; + } + + /** + * Gets the position the pivot should move to when a command ends, based on the previous target + * position. + * + * @param previousTarget The previous target position of the pivot. + * @return The position the pivot should move to when a command ends. + */ + private Angle getEndPosition(Angle previousTarget) { + if (isNear(previousTarget)) { + return previousTarget; + } else { + return getPosition(); } - - private boolean isSafeToMove() { - return sensors.getCoralLocation() != IntakeSensors.CoralLocation.TRANSFER_TO_INDEXER; + } + + /** + * Creates a command that moves the pivot to the specified angle, ending when the target is + * reached. + * + * @param targetPosition The angle to move the pivot to. + * @return A command that moves the pivot to the specified angle. + */ + private Command moveTo(Angle targetPosition) { + return Commands.defer( + () -> { + Command pivotCommand = + startEnd( + () -> setPositionControl(targetPosition), + () -> setPositionControl(getEndPosition(targetPosition))) + .until(() -> isNear(targetPosition)); + + if (isSafeToMove()) { + return pivotCommand; + } else { + return Commands.none(); + } + }, + Set.of(this)); + } + + /** + * Creates a command that moves the pivot to the intake position, ending when the target is + * reached. + * + * @return A command that moves the pivot to the intake position. + */ + public Command deploy() { + return moveTo(IntakeConstants.pivotIntakeAngle); + } + + public Command deployVertical() { + return moveTo(IntakeConstants.pivotVerticalAngle); + } + + /** + * Creates a command that moves the pivot to the stow position, ending when the target is reached. + * + * @return A command that moves the pivot to the stow position. + */ + public Command stow() { + return moveTo(IntakeConstants.pivotStowAngle); + } + + /** + * Returns whether the pivot is near the specified position, within the tolerance specified in + * {@link IntakeConstants#pivotTolerance}. + * + * @param position The position to check if the pivot is near to. + * @return Whether the pivot is near the specified position. + */ + public boolean isNear(Angle position) { + return getPosition().isNear(position, IntakeConstants.pivotTolerance); + } + + @Override + public void periodic() { + refreshStatusSignals(); + + if (RobotState.isDisabled()) setPositionControl(getPosition()); + + setControlWithLimits(controlRequest); + } + + private Current calculateGravityFeedforwardCurrent() { + return IntakeConstants.kG.times( + Math.cos(getPosition().plus(IntakeConstants.centerOfMassAngularOffset).in(Radians))); + } + + private void setPositionControl(Angle targetPosition) { + this.targetPosition = targetPosition; + + setControlWithLimits(new PositionTorqueCurrentFOC(targetPosition.minus(Degrees.of(160)))); + } + + private void setControlWithLimits(ControlRequest request) { + controlRequest = request; + + applyLimitsToControlRequest(controlRequest); + applyFeedforwardToControlRequest(controlRequest); + CTREUtils.check(motor.setControl(controlRequest)); + } + + private void applyFeedforwardToControlRequest(ControlRequest request) { + if (request instanceof PositionTorqueCurrentFOC r) { + r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); + } else if (request instanceof MotionMagicTorqueCurrentFOC r) { + r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); + } else if (request instanceof MotionMagicExpoTorqueCurrentFOC r) { + r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); } + } + private void initStatusSignals() { + positionIn = motor.getPosition(); + velocityIn = motor.getVelocity(); + statorCurrentIn = motor.getStatorCurrent(); + supplyCurrentIn = motor.getSupplyCurrent(); + appliedVoltageIn = motor.getMotorVoltage(); - /** - * Gets the position the pivot should move to when a command ends, based - * on the previous target position. - * @param previousTarget The previous target position of the pivot. - * @return The position the pivot should move to when a command ends. - */ - private Angle getEndPosition(Angle previousTarget) { - if (isNear(previousTarget)) { - return previousTarget; - } else { - return getPosition(); - } - } - - /** - * Creates a command that moves the pivot to the specified angle, ending - * when the target is reached. - * @param targetPosition The angle to move the pivot to. - * @return A command that moves the pivot to the specified angle. - */ - private Command moveTo(Angle targetPosition) { - return Commands.defer(() -> { - Command pivotCommand = startEnd( - () -> setPositionControl(targetPosition), - () -> setPositionControl(getEndPosition(targetPosition)) - ).until(() -> isNear(targetPosition)); - - if (isSafeToMove()) { - return pivotCommand; - } else { - return Commands.none(); - } - }, Set.of(this)); - } - - /** - * Creates a command that moves the pivot to the intake position, ending - * when the target is reached. - * @return A command that moves the pivot to the intake position. - */ - public Command deploy() { - return moveTo(IntakeConstants.pivotIntakeAngle); - } - - public Command deployVertical() { - return moveTo(IntakeConstants.pivotVerticalAngle); - } - - /** - * Creates a command that moves the pivot to the stow position, ending - * when the target is reached. - * @return A command that moves the pivot to the stow position. - */ - public Command stow() { - return moveTo(IntakeConstants.pivotStowAngle); - } - - /** - * Returns whether the pivot is near the specified position, within the - * tolerance specified in {@link IntakeConstants#pivotTolerance}. - * - * @param position The position to check if the pivot is near to. - * @return Whether the pivot is near the specified position. - */ - public boolean isNear(Angle position) { - return getPosition().isNear(position, IntakeConstants.pivotTolerance); - } - - @Override - public void periodic() { - refreshStatusSignals(); - - if (RobotState.isDisabled()) setPositionControl(getPosition()); - - setControlWithLimits(controlRequest); - } - - private Current calculateGravityFeedforwardCurrent() { - return IntakeConstants.kG.times(Math.cos(getPosition().plus(IntakeConstants.centerOfMassAngularOffset).in(Radians))); - } - - private void setPositionControl(Angle targetPosition) { - this.targetPosition = targetPosition; - - setControlWithLimits(new PositionTorqueCurrentFOC(targetPosition.minus(Degrees.of(160)))); - } - - private void setControlWithLimits(ControlRequest request) { - controlRequest = request; - - applyLimitsToControlRequest(controlRequest); - applyFeedforwardToControlRequest(controlRequest); - CTREUtils.check(motor.setControl(controlRequest)); - } - - private void applyFeedforwardToControlRequest(ControlRequest request) { - if (request instanceof PositionTorqueCurrentFOC r) { - r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); - } else if (request instanceof MotionMagicTorqueCurrentFOC r) { - r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); - } else if (request instanceof MotionMagicExpoTorqueCurrentFOC r) { - r.FeedForward = calculateGravityFeedforwardCurrent().in(Amps); - } - } - - private void initStatusSignals() { - positionIn = motor.getPosition(); - velocityIn = motor.getVelocity(); - statorCurrentIn = motor.getStatorCurrent(); - supplyCurrentIn = motor.getSupplyCurrent(); - appliedVoltageIn = motor.getMotorVoltage(); - - CTREUtils.check(BaseStatusSignal.setUpdateFrequencyForAll( + CTREUtils.check( + BaseStatusSignal.setUpdateFrequencyForAll( Hertz.of(100), - positionIn, velocityIn, - encoder.getPosition(), encoder.getVelocity() // TODO: Test to see if this is needed - )); - - CTREUtils.check(BaseStatusSignal.setUpdateFrequencyForAll( - Hertz.of(50), - statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); - - CTREUtils.check(ParentDevice.optimizeBusUtilizationForAll(motor, encoder)); - } - - private void refreshStatusSignals() { - CTREUtils.check(positionIn.getStatus()); - CTREUtils.check(velocityIn.getStatus()); - CTREUtils.check(statorCurrentIn.getStatus()); - CTREUtils.check(supplyCurrentIn.getStatus()); - CTREUtils.check(appliedVoltageIn.getStatus()); - - CTREUtils.check(BaseStatusSignal.refreshAll( - positionIn, velocityIn, - statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); - } - - private boolean limitReverseMotion() { - return getPosition().in(Degrees) <= IntakeConstants.pivotMinAngle.in(Degrees); - } - - private boolean limitForwardMotion() { - return getPosition().in(Degrees) >= IntakeConstants.pivotMaxAngle.in(Degrees); - } - - private void applyLimitsToControlRequest(ControlRequest request) { - if (request == null) return; - - if (request instanceof MotionMagicTorqueCurrentFOC r) { - r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); - } else if (request instanceof MotionMagicExpoTorqueCurrentFOC r) { - r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); - } else if (request instanceof PositionTorqueCurrentFOC r) { - r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); - } + positionIn, + velocityIn, + encoder.getPosition(), + encoder.getVelocity() // TODO: Test to see if this is needed + )); + + CTREUtils.check( + BaseStatusSignal.setUpdateFrequencyForAll( + Hertz.of(50), statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + + CTREUtils.check(ParentDevice.optimizeBusUtilizationForAll(motor, encoder)); + } + + private void refreshStatusSignals() { + CTREUtils.check(positionIn.getStatus()); + CTREUtils.check(velocityIn.getStatus()); + CTREUtils.check(statorCurrentIn.getStatus()); + CTREUtils.check(supplyCurrentIn.getStatus()); + CTREUtils.check(appliedVoltageIn.getStatus()); + + CTREUtils.check( + BaseStatusSignal.refreshAll( + positionIn, velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + } + + private boolean limitReverseMotion() { + return getPosition().in(Degrees) <= IntakeConstants.pivotMinAngle.in(Degrees); + } + + private boolean limitForwardMotion() { + return getPosition().in(Degrees) >= IntakeConstants.pivotMaxAngle.in(Degrees); + } + + private void applyLimitsToControlRequest(ControlRequest request) { + if (request == null) return; + + if (request instanceof MotionMagicTorqueCurrentFOC r) { + r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); + } else if (request instanceof MotionMagicExpoTorqueCurrentFOC r) { + r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); + } else if (request instanceof PositionTorqueCurrentFOC r) { + r.withLimitForwardMotion(limitForwardMotion()).withLimitReverseMotion(limitReverseMotion()); } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotSim.java b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotSim.java index 69208abd..c45d4213 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotSim.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotSim.java @@ -12,7 +12,6 @@ import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team6962.lib.utils.CTREUtils; - import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; @@ -22,80 +21,93 @@ import frc.robot.subsystems.intake.IntakeSensors; /** - * A simulation of the intake pivot, which pivots the ground coral intake up and - * down. This class extends the {@link IntakePivot} class, which represents a - * real intake pivot. + * A simulation of the intake pivot, which pivots the ground coral intake up and down. This class + * extends the {@link IntakePivot} class, which represents a real intake pivot. */ public class IntakePivotSim extends IntakePivot { - private SingleJointedArmSim physicsSim; - private TalonFXSimState controllerSim; - private CANcoderSimState encoderSim; + private SingleJointedArmSim physicsSim; + private TalonFXSimState controllerSim; + private CANcoderSimState encoderSim; - private Timer deltaTimer; + private Timer deltaTimer; - /** - * Creates a new IntakePivotSim. - */ - public IntakePivotSim(IntakeSensors sensors) { - super(sensors); + /** Creates a new IntakePivotSim. */ + public IntakePivotSim(IntakeSensors sensors) { + super(sensors); - physicsSim = new SingleJointedArmSim( + physicsSim = + new SingleJointedArmSim( LinearSystemId.createSingleJointedArmSystem( IntakeConstants.Simulation.pivotMotor, IntakeConstants.Simulation.pivotMomentOfInertia.in(KilogramSquareMeters), - IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism - ), + IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism), IntakeConstants.Simulation.pivotMotor, IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism, IntakeConstants.Simulation.pivotCenterOfMassDistance.in(Meters), - IntakeConstants.pivotMinAngle.plus(IntakeConstants.centerOfMassAngularOffset).in(Radians), - IntakeConstants.pivotMaxAngle.plus(IntakeConstants.centerOfMassAngularOffset).in(Radians), + IntakeConstants.pivotMinAngle + .plus(IntakeConstants.centerOfMassAngularOffset) + .in(Radians), + IntakeConstants.pivotMaxAngle + .plus(IntakeConstants.centerOfMassAngularOffset) + .in(Radians), true, - IntakeConstants.Simulation.pivotStartAngle.plus(IntakeConstants.centerOfMassAngularOffset).in(Radians) - ); - - controllerSim = new TalonFXSimState(motor); - encoderSim = new CANcoderSimState(encoder); - } - - @Override - public TalonFXConfiguration modifyConfiguration(TalonFXConfiguration config) { - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - return config; + IntakeConstants.Simulation.pivotStartAngle + .plus(IntakeConstants.centerOfMassAngularOffset) + .in(Radians)); + + controllerSim = new TalonFXSimState(motor); + encoderSim = new CANcoderSimState(encoder); + } + + @Override + public TalonFXConfiguration modifyConfiguration(TalonFXConfiguration config) { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + return config; + } + + @Override + protected CANcoderConfiguration modifyConfiguration(CANcoderConfiguration config) { + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + return config; + } + + @Override + public void periodic() { + super.periodic(); + + if (deltaTimer == null) { + deltaTimer = new Timer(); + deltaTimer.start(); + return; } - @Override - protected CANcoderConfiguration modifyConfiguration(CANcoderConfiguration config) { - config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - return config; - } - - @Override - public void periodic() { - super.periodic(); - - if (deltaTimer == null) { - deltaTimer = new Timer(); - deltaTimer.start(); - return; - } - - double dtSeconds = deltaTimer.get(); - deltaTimer.restart(); - - CTREUtils.check(controllerSim.setSupplyVoltage(RobotController.getBatteryVoltage())); - physicsSim.setInputVoltage(controllerSim.getMotorVoltage()); - - physicsSim.update(dtSeconds); - - Angle physicsAngle = Radians.of(physicsSim.getAngleRads()); - Angle externalAngle = physicsAngle.minus(IntakeConstants.centerOfMassAngularOffset); - Angle motorAngle = externalAngle.minus(IntakeConstants.absoluteEncoderOffset); - - CTREUtils.check(controllerSim.setRawRotorPosition(motorAngle.times(IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism))); - CTREUtils.check(controllerSim.setRotorVelocity(RadiansPerSecond.of(physicsSim.getVelocityRadPerSec()).times(IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism))); - - CTREUtils.check(encoderSim.setRawPosition(motorAngle.times(IntakeConstants.pivotSensorToMechanism))); - CTREUtils.check(encoderSim.setVelocity(RadiansPerSecond.of(physicsSim.getVelocityRadPerSec()).times(IntakeConstants.pivotSensorToMechanism))); - } + double dtSeconds = deltaTimer.get(); + deltaTimer.restart(); + + CTREUtils.check(controllerSim.setSupplyVoltage(RobotController.getBatteryVoltage())); + physicsSim.setInputVoltage(controllerSim.getMotorVoltage()); + + physicsSim.update(dtSeconds); + + Angle physicsAngle = Radians.of(physicsSim.getAngleRads()); + Angle externalAngle = physicsAngle.minus(IntakeConstants.centerOfMassAngularOffset); + Angle motorAngle = externalAngle.minus(IntakeConstants.absoluteEncoderOffset); + + CTREUtils.check( + controllerSim.setRawRotorPosition( + motorAngle.times( + IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism))); + CTREUtils.check( + controllerSim.setRotorVelocity( + RadiansPerSecond.of(physicsSim.getVelocityRadPerSec()) + .times( + IntakeConstants.pivotRotorToSensor * IntakeConstants.pivotSensorToMechanism))); + + CTREUtils.check( + encoderSim.setRawPosition(motorAngle.times(IntakeConstants.pivotSensorToMechanism))); + CTREUtils.check( + encoderSim.setVelocity( + RadiansPerSecond.of(physicsSim.getVelocityRadPerSec()) + .times(IntakeConstants.pivotSensorToMechanism))); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index 21699c78..58349f7f 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.CTREUtils; - import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -20,105 +19,102 @@ import frc.robot.subsystems.intake.IntakeConstants; /** - * The intake rollers subsystem, which pulls coral off of the ground and into - * the robot. + * The intake rollers subsystem, which pulls coral off of the ground and into the robot. + * *

Controlling the Rollers

- * The rollers can be moved via the {@link #intake()} method, which runs the - * rollers in the direction and speed that intakes coral from the ground. The - * intake() method returns a command that can be scheduled or added to a - * composition to run the rollers. + * + * The rollers can be moved via the {@link #intake()} method, which runs the rollers in the + * direction and speed that intakes coral from the ground. The intake() method returns a command + * that can be scheduled or added to a composition to run the rollers. */ public class IntakeRollers extends SubsystemBase { - protected TalonFX motor; - - private StatusSignal velocityIn; - private StatusSignal statorCurrentIn; - private StatusSignal supplyCurrentIn; - private StatusSignal appliedVoltageIn; - - private Voltage simVoltage; - - /** - * Creates a new IntakeRollers subsystem. - */ - public IntakeRollers() { - motor = new TalonFX(IntakeConstants.rollerMotorID, IntakeConstants.canBus); - - motor.getConfigurator().apply(IntakeConstants.rollerMotorConfiguration); - - initStatusSignals(); - - if (RobotBase.isReal()) { - Logger.logMeasure("Intake/Rollers/velocity", () -> CTREUtils.unwrap(velocityIn)); - Logger.logMeasure("Intake/Rollers/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); - Logger.logMeasure("Intake/Rollers/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); - Logger.logMeasure("Intake/Rollers/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); - } else { - Logger.logMeasure("Intake/Rollers/appliedVoltage", () -> simVoltage); - } - } + protected TalonFX motor; - private void initStatusSignals() { - velocityIn = motor.getVelocity(); - statorCurrentIn = motor.getStatorCurrent(); - supplyCurrentIn = motor.getSupplyCurrent(); - appliedVoltageIn = motor.getMotorVoltage(); + private StatusSignal velocityIn; + private StatusSignal statorCurrentIn; + private StatusSignal supplyCurrentIn; + private StatusSignal appliedVoltageIn; - CTREUtils.check(BaseStatusSignal.setUpdateFrequencyForAll( - Hertz.of(50), - velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); + private Voltage simVoltage; - CTREUtils.check(motor.optimizeBusUtilization()); - } + /** Creates a new IntakeRollers subsystem. */ + public IntakeRollers() { + motor = new TalonFX(IntakeConstants.rollerMotorID, IntakeConstants.canBus); - private void refreshStatusSignals() { - CTREUtils.check(BaseStatusSignal.refreshAll( - velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn - )); - } + motor.getConfigurator().apply(IntakeConstants.rollerMotorConfiguration); - private void setVoltage(Voltage voltage) { - if (RobotBase.isSimulation()) { - simVoltage = voltage; - } + initStatusSignals(); - motor.setVoltage(voltage.in(Volts)); + if (RobotBase.isReal()) { + Logger.logMeasure("Intake/Rollers/velocity", () -> CTREUtils.unwrap(velocityIn)); + Logger.logMeasure("Intake/Rollers/statorCurrent", () -> CTREUtils.unwrap(statorCurrentIn)); + Logger.logMeasure("Intake/Rollers/supplyCurrent", () -> CTREUtils.unwrap(supplyCurrentIn)); + Logger.logMeasure("Intake/Rollers/appliedVoltage", () -> CTREUtils.unwrap(appliedVoltageIn)); + } else { + Logger.logMeasure("Intake/Rollers/appliedVoltage", () -> simVoltage); } - - private Command run(Voltage voltage) { - return startEnd( - () -> setVoltage(voltage), - () -> setVoltage(Volts.of(0)) - ); - } - - /** - * Runs the intake rollers in a way that intakes coral from the ground. - * @return a command that runs the rollers to intake coral - */ - public Command intake() { - return run(IntakeConstants.rollerIntakeVoltage); + } + + private void initStatusSignals() { + velocityIn = motor.getVelocity(); + statorCurrentIn = motor.getStatorCurrent(); + supplyCurrentIn = motor.getSupplyCurrent(); + appliedVoltageIn = motor.getMotorVoltage(); + + CTREUtils.check( + BaseStatusSignal.setUpdateFrequencyForAll( + Hertz.of(50), velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + + CTREUtils.check(motor.optimizeBusUtilization()); + } + + private void refreshStatusSignals() { + CTREUtils.check( + BaseStatusSignal.refreshAll( + velocityIn, statorCurrentIn, supplyCurrentIn, appliedVoltageIn)); + } + + private void setVoltage(Voltage voltage) { + if (RobotBase.isSimulation()) { + simVoltage = voltage; } - public Command drop() { - return run(IntakeConstants.rollerDropVoltage); - } - - /** - * Returns whether the intake is currently moving. This is determined by - * checking if the rollers are spinning at a non-negligible speed. - * - * @return whether the intake is currently intaking coral - */ - public boolean isIntaking() { - return RobotBase.isSimulation() ? Math.abs(simVoltage.in(Volts)) > 1 : Math.abs(CTREUtils.unwrap(velocityIn).in(RotationsPerSecond)) > 1; - } - - @Override - public void periodic() { - refreshStatusSignals(); - - if (RobotState.isDisabled()) setVoltage(Volts.of(0)); - } + motor.setVoltage(voltage.in(Volts)); + } + + private Command run(Voltage voltage) { + return startEnd(() -> setVoltage(voltage), () -> setVoltage(Volts.of(0))); + } + + /** + * Runs the intake rollers in a way that intakes coral from the ground. + * + * @return a command that runs the rollers to intake coral + */ + public Command intake() { + return run(IntakeConstants.rollerIntakeVoltage); + } + + public Command drop() { + return run(IntakeConstants.rollerDropVoltage); + } + + /** + * Returns whether the intake is currently moving. This is determined by checking if the rollers + * are spinning at a non-negligible speed. + * + * @return whether the intake is currently intaking coral + */ + public boolean isIntaking() { + return RobotBase.isSimulation() + ? Math.abs(simVoltage.in(Volts)) > 1 + : Math.abs(CTREUtils.unwrap(velocityIn).in(RotationsPerSecond)) > 1; + } + + @Override + public void periodic() { + refreshStatusSignals(); + + if (RobotState.isDisabled()) setVoltage(Volts.of(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 122ea19c..083e5937 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -87,19 +87,18 @@ private static Color convertVisibleColorToDriverColor(Color visibleColor) { private static LEDPattern createDiscontinuousGradient( Color from, Color to, Frequency scrollSpeed) { return LEDPattern.gradient( - LEDPattern.GradientType.kDiscontinuous, - convertVisibleColorToDriverColor(from), - convertVisibleColorToDriverColor(to) - ).scrollAtRelativeSpeed(scrollSpeed); + LEDPattern.GradientType.kDiscontinuous, + convertVisibleColorToDriverColor(from), + convertVisibleColorToDriverColor(to)) + .scrollAtRelativeSpeed(scrollSpeed); } - private static LEDPattern createContinuousGradient( - Color from, Color to, Frequency scrollSpeed) { + private static LEDPattern createContinuousGradient(Color from, Color to, Frequency scrollSpeed) { return LEDPattern.gradient( - LEDPattern.GradientType.kContinuous, - convertVisibleColorToDriverColor(from), - convertVisibleColorToDriverColor(to) - ).scrollAtRelativeSpeed(scrollSpeed); + LEDPattern.GradientType.kContinuous, + convertVisibleColorToDriverColor(from), + convertVisibleColorToDriverColor(to)) + .scrollAtRelativeSpeed(scrollSpeed); } private static void apply(LEDPattern pattern) { @@ -127,10 +126,16 @@ public void periodic() { apply(createDiscontinuousGradient(new Color(255, 100, 0), ANTARES_YELLOW, Hertz.of(2))); break; case AUTO_BLUE: - apply(createDiscontinuousGradient(new Color(0, 255, 100), new Color(0, 255, 255), Hertz.of(2))); + apply( + createDiscontinuousGradient( + new Color(0, 255, 100), new Color(0, 255, 255), Hertz.of(2))); break; case TELEOP_RED: - apply(createDiscontinuousGradient(new Color(1., 0.35, 0.1), new Color(1., 222. / 242. * 0.8, 139. / 242.), Hertz.of(0.5))); + apply( + createDiscontinuousGradient( + new Color(1., 0.35, 0.1), + new Color(1., 222. / 242. * 0.8, 139. / 242.), + Hertz.of(0.5))); break; case TELEOP_BLUE: apply(createDiscontinuousGradient(ANTARES_BLUE_BRIGHT, BLUE, Hertz.of(1.25))); @@ -139,23 +144,18 @@ public void periodic() { apply(createContinuousGradient(ANTARES_YELLOW, ORANGE, Hertz.of(1))); break; case GOOD: - apply(createContinuousGradient(new Color(125, 0, 255), new Color(255, 75, 125), Hertz.of(1))); + apply( + createContinuousGradient(new Color(125, 0, 255), new Color(255, 75, 125), Hertz.of(1))); break; - } if (CachedRobotState.isDisabled() && AprilTags.changingHeading) { state = State.DEFAULT; } else { - state = (CachedRobotState.isAutonomous() && CachedRobotState.isEnabled()) ? ( - CachedRobotState.isBlue().orElse(false) - ? State.AUTO_BLUE - : State.AUTO_RED - ) : ( - CachedRobotState.isBlue().orElse(false) - ? State.TELEOP_BLUE - : State.TELEOP_RED - ); + state = + (CachedRobotState.isAutonomous() && CachedRobotState.isEnabled()) + ? (CachedRobotState.isBlue().orElse(false) ? State.AUTO_BLUE : State.AUTO_RED) + : (CachedRobotState.isBlue().orElse(false) ? State.TELEOP_BLUE : State.TELEOP_RED); } } } diff --git a/src/main/java/frc/robot/subsystems/manipulator/grabber/AlgaeSensor.java b/src/main/java/frc/robot/subsystems/manipulator/grabber/AlgaeSensor.java index ca65ef89..b50aa740 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/grabber/AlgaeSensor.java +++ b/src/main/java/frc/robot/subsystems/manipulator/grabber/AlgaeSensor.java @@ -10,57 +10,56 @@ import com.ctre.phoenix6.hardware.CANrange; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.utils.CTREUtils; - import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class AlgaeSensor extends SubsystemBase { - private final CANrange canRange; + private final CANrange canRange; + + private final StatusSignal distanceSignal; + private Distance distance = Meters.of(0); + private Debouncer debouncer = new Debouncer(0.1, DebounceType.kBoth); + private Debouncer fullyIntakedDebouncer = new Debouncer(0.3, DebounceType.kRising); + private boolean hasAlgae; + private boolean fullyIntaked; - private final StatusSignal distanceSignal; - private Distance distance = Meters.of(0); - private Debouncer debouncer = new Debouncer(0.1, DebounceType.kBoth); - private Debouncer fullyIntakedDebouncer = new Debouncer(0.3, DebounceType.kRising); - private boolean hasAlgae; - private boolean fullyIntaked; + public AlgaeSensor(int deviceId, String canBus) { + canRange = new CANrange(deviceId, canBus); - public AlgaeSensor(int deviceId, String canBus) { - canRange = new CANrange(deviceId, canBus); - - CANrangeConfiguration canRangeConfig = new CANrangeConfiguration() + CANrangeConfiguration canRangeConfig = + new CANrangeConfiguration() .withFovParams( new FovParamsConfigs() .withFOVRangeX(Degrees.of(6.75)) - .withFOVRangeY(Degrees.of(6.75)) - ); - - CTREUtils.check(canRange.getConfigurator().apply(canRangeConfig)); - - distanceSignal = canRange.getDistance(); + .withFOVRangeY(Degrees.of(6.75))); + + CTREUtils.check(canRange.getConfigurator().apply(canRangeConfig)); + + distanceSignal = canRange.getDistance(); - Logger.logMeasure("Grabber/AlgaeSensor/algaeDistance", () -> distanceSignal.getValue()); - Logger.logBoolean("Grabber/AlgaeSensor/hasAlgae", this::hasAlgae); - Logger.logBoolean("Grabber/AlgaeSensor/algaeFullyIntaked", this::isAlgaeFullyIntaked); - } + Logger.logMeasure("Grabber/AlgaeSensor/algaeDistance", () -> distanceSignal.getValue()); + Logger.logBoolean("Grabber/AlgaeSensor/hasAlgae", this::hasAlgae); + Logger.logBoolean("Grabber/AlgaeSensor/algaeFullyIntaked", this::isAlgaeFullyIntaked); + } - public Distance getDistance() { - return distance; - } + public Distance getDistance() { + return distance; + } - public boolean hasAlgae() { - return hasAlgae; - } + public boolean hasAlgae() { + return hasAlgae; + } - public boolean isAlgaeFullyIntaked() { - return fullyIntaked; - } + public boolean isAlgaeFullyIntaked() { + return fullyIntaked; + } - @Override - public void periodic() { - distance = CTREUtils.unwrap(distanceSignal.refresh()); - hasAlgae = debouncer.calculate(distance.lt(Inches.of(3))); - fullyIntaked = fullyIntakedDebouncer.calculate(distance.lt(Inches.of(1))); - } + @Override + public void periodic() { + distance = CTREUtils.unwrap(distanceSignal.refresh()); + hasAlgae = debouncer.calculate(distance.lt(Inches.of(3))); + fullyIntaked = fullyIntakedDebouncer.calculate(distance.lt(Inches.of(1))); + } } diff --git a/src/main/java/frc/robot/subsystems/manipulator/grabber/Grabber.java b/src/main/java/frc/robot/subsystems/manipulator/grabber/Grabber.java index 368991b2..a568bb7e 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/grabber/Grabber.java +++ b/src/main/java/frc/robot/subsystems/manipulator/grabber/Grabber.java @@ -1,14 +1,12 @@ package frc.robot.subsystems.manipulator.grabber; -import java.util.Set; - import com.team6962.lib.telemetry.Logger; - import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants.ENABLED_SYSTEMS; +import java.util.Set; public abstract class Grabber extends SubsystemBase { public Grabber() { @@ -20,7 +18,9 @@ public Grabber() { } public abstract boolean hasCoral(); + public abstract boolean hasAlgae(); + public abstract boolean isAlgaeFullyIntaked(); public Command coralMagicButton() { diff --git a/src/main/java/frc/robot/subsystems/manipulator/grabber/RealGrabber.java b/src/main/java/frc/robot/subsystems/manipulator/grabber/RealGrabber.java index 0156d719..cf095cbc 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/grabber/RealGrabber.java +++ b/src/main/java/frc/robot/subsystems/manipulator/grabber/RealGrabber.java @@ -8,7 +8,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.telemetry.StatusChecks; - import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; @@ -53,15 +52,14 @@ public RealGrabber() { private Command run(double speed) { return startEnd( - () -> { - running = true; - motor.set(speed); - }, - () -> { - running = false; - motor.set(getStoppedSpeed()); - } - ); + () -> { + running = true; + motor.set(speed); + }, + () -> { + running = false; + motor.set(getStoppedSpeed()); + }); } private double getStoppedSpeed() { @@ -89,9 +87,10 @@ public boolean isCoralClear() { @Override public Command intakeCoral() { return Commands.sequence( - run(MANIPULATOR.CORAL_IN_SPEED).until(this::hasCoral), - run(MANIPULATOR.CORAL_SLOW_IN_SPEED).until(this::isCoralClear).onlyIf(() -> !hasCoral || !coralClear) - ); + run(MANIPULATOR.CORAL_IN_SPEED).until(this::hasCoral), + run(MANIPULATOR.CORAL_SLOW_IN_SPEED) + .until(this::isCoralClear) + .onlyIf(() -> !hasCoral || !coralClear)); } @Override @@ -111,23 +110,17 @@ public Command adjustCoral() { public Command repositionCoral() { return Commands.sequence( run(MANIPULATOR.CORAL_ADJUST_SPEED).until(() -> !isCoralClear()), - run(MANIPULATOR.CORAL_REPOSITION_SPEED).until(() -> isCoralClear()) - ); + run(MANIPULATOR.CORAL_REPOSITION_SPEED).until(() -> isCoralClear())); } @Override public Command intakeAlgae() { - return run(MANIPULATOR.ALGAE_IN_SPEED) - .until(this::isAlgaeFullyIntaked); + return run(MANIPULATOR.ALGAE_IN_SPEED).until(this::isAlgaeFullyIntaked); } @Override public Command dropAlgae() { - return Commands.either( - Commands.none(), - run(MANIPULATOR.ALGAE_OUT_SPEED), - () -> hasCoral() - ); + return Commands.either(Commands.none(), run(MANIPULATOR.ALGAE_OUT_SPEED), () -> hasCoral()); } @Override diff --git a/src/main/java/frc/robot/subsystems/manipulator/grabber/SimGrabber.java b/src/main/java/frc/robot/subsystems/manipulator/grabber/SimGrabber.java index fbbfedf7..5e388df6 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/grabber/SimGrabber.java +++ b/src/main/java/frc/robot/subsystems/manipulator/grabber/SimGrabber.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Seconds; import com.team6962.lib.utils.CommandUtils; - import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -36,7 +35,7 @@ public SimGrabber( @Override public boolean isAlgaeFullyIntaked() { - return hasAlgae; + return hasAlgae; } public static SimGrabber simulated() { diff --git a/src/main/java/frc/robot/subsystems/manipulator/pivot/DisabledManipulatorPivot.java b/src/main/java/frc/robot/subsystems/manipulator/pivot/DisabledManipulatorPivot.java index 7439be6e..a7e186fe 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/pivot/DisabledManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/manipulator/pivot/DisabledManipulatorPivot.java @@ -94,8 +94,7 @@ public Command down() { } @Override - public void setMinMaxAngle(Angle min, Angle max) { - } + public void setMinMaxAngle(Angle min, Angle max) {} @Override public boolean inRange(Angle angle) { diff --git a/src/main/java/frc/robot/subsystems/manipulator/pivot/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/manipulator/pivot/ManipulatorPivot.java index 2b8a9afb..d0811a40 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/pivot/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/manipulator/pivot/ManipulatorPivot.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.constants.Constants.ENABLED_SYSTEMS; - import java.util.function.BooleanSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java b/src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java index 8129bb77..cc4e98c1 100644 --- a/src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java @@ -7,8 +7,6 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; -import java.util.function.BooleanSupplier; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -17,6 +15,7 @@ import frc.robot.constants.Constants.CAN; import frc.robot.constants.Constants.MANIPULATOR_PIVOT; import frc.robot.util.hardware.motion.PivotController; +import java.util.function.BooleanSupplier; @SuppressWarnings("deprecation") public class RealManipulatorPivot extends PivotController implements ManipulatorPivot { @@ -24,7 +23,7 @@ public class RealManipulatorPivot extends PivotController implements Manipulator public RealManipulatorPivot(BooleanSupplier hasAlgae) { super( - "Manipulator Pivot", + "Manipulator Pivot", CAN.MANIPULATOR_PIVOT, MANIPULATOR_PIVOT.ABSOLUTE_POSITION_OFFSET.in(Rotations), MANIPULATOR_PIVOT.PROFILE.kP, @@ -36,7 +35,7 @@ public RealManipulatorPivot(BooleanSupplier hasAlgae) { MANIPULATOR_PIVOT.MAX_ANGLE, MANIPULATOR_PIVOT.TOLERANCE, MANIPULATOR_PIVOT.INVERTED); - + this.hasAlgae = hasAlgae; } @@ -88,7 +87,11 @@ public Command algaeGround() { @Override public Command stow() { - return pivotTo(() -> hasAlgae.getAsBoolean() ? MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE : MANIPULATOR_PIVOT.STOW_ANGLE); + return pivotTo( + () -> + hasAlgae.getAsBoolean() + ? MANIPULATOR_PIVOT.ALGAE.HOLD_ANGLE + : MANIPULATOR_PIVOT.STOW_ANGLE); } @Override @@ -102,8 +105,7 @@ public Command safe(Angle tolerance) { @Override public boolean inRange(Angle angle) { - return getPosition().minus(angle).abs(Rotations) - < MANIPULATOR_PIVOT.TOLERANCE.in(Rotations); + return getPosition().minus(angle).abs(Rotations) < MANIPULATOR_PIVOT.TOLERANCE.in(Rotations); } @Override diff --git a/src/main/java/frc/robot/util/hardware/motion/PivotController.java b/src/main/java/frc/robot/util/hardware/motion/PivotController.java index 6842820b..e02fee61 100644 --- a/src/main/java/frc/robot/util/hardware/motion/PivotController.java +++ b/src/main/java/frc/robot/util/hardware/motion/PivotController.java @@ -3,8 +3,6 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; -import java.util.function.Supplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.ClosedLoopSlot; @@ -16,7 +14,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.team6962.lib.telemetry.Logger; import com.team6962.lib.telemetry.StatusChecks; - import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotState; @@ -24,11 +21,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants.MANIPULATOR_PIVOT; import frc.robot.util.hardware.SparkMaxUtil; +import java.util.function.Supplier; /** - * Uses a Spark Max motor controller with a NEO brushless motor and a Rev - * Through-Bore absolute encoder to control the angle of a pivoting arm. - * + * Uses a Spark Max motor controller with a NEO brushless motor and a Rev Through-Bore absolute + * encoder to control the angle of a pivoting arm. + * * @deprecated Do not use after the 2025 season. */ public class PivotController extends SubsystemBase { @@ -99,7 +97,9 @@ public PivotController( Logger.logNumber(this.getName() + "/duty/applied", () -> motor.getAppliedOutput()); Logger.logNumber(this.getName() + "/duty/pid", () -> motor.get()); - Logger.logNumber(this.getName() + "/angle/target", () -> targetPosition == null ? 0 : targetPosition.in(Rotations)); + Logger.logNumber( + this.getName() + "/angle/target", + () -> targetPosition == null ? 0 : targetPosition.in(Rotations)); Logger.logNumber(this.getName() + "/angle/relative", () -> getPosition().in(Rotations)); Logger.logNumber(this.getName() + "/angle/absolute", () -> getAbsolutePosition().in(Rotations)); Logger.logNumber(this.getName() + "/angle/raw", () -> getRawAbsolutePosition().in(Rotations)); @@ -113,17 +113,17 @@ public Angle getPosition() { public Command pivotTo(Supplier targetSupplier, Angle tolerance) { return startEnd( - () -> pivotTowards(targetSupplier.get()), - () -> { - if (doneMoving(tolerance)) { - targetPosition = targetSupplier.get(); - } else { - targetPosition = getPosition(); - } - - pivotTowards(targetPosition); - } - ).until(() -> doneMoving(tolerance)); + () -> pivotTowards(targetSupplier.get()), + () -> { + if (doneMoving(tolerance)) { + targetPosition = targetSupplier.get(); + } else { + targetPosition = getPosition(); + } + + pivotTowards(targetPosition); + }) + .until(() -> doneMoving(tolerance)); } public Command pivotTo(Supplier angleSupplier) { @@ -186,7 +186,8 @@ public Command down() { } public double calculateKG(Angle currentAngle) { - return kG * Math.cos(MANIPULATOR_PIVOT.CENTER_OF_MASS_OFFSET.in(Radians) + currentAngle.in(Radians)); + return kG + * Math.cos(MANIPULATOR_PIVOT.CENTER_OF_MASS_OFFSET.in(Radians) + currentAngle.in(Radians)); } private Angle wrapAngle(Angle angle) { diff --git a/src/main/java/frc/robot/vision/AprilTags.java b/src/main/java/frc/robot/vision/AprilTags.java index 39cd03c5..d45f5d06 100644 --- a/src/main/java/frc/robot/vision/AprilTags.java +++ b/src/main/java/frc/robot/vision/AprilTags.java @@ -22,7 +22,6 @@ import io.limelightvision.LimelightHelpers; import io.limelightvision.LimelightHelpers.PoseEstimate; import io.limelightvision.LimelightHelpers.RawFiducial; - import java.util.ArrayList; import java.util.HashMap; import java.util.List; diff --git a/src/main/java/frc/robot/vision/CoralDetection.java b/src/main/java/frc/robot/vision/CoralDetection.java index 18296375..5ee77667 100644 --- a/src/main/java/frc/robot/vision/CoralDetection.java +++ b/src/main/java/frc/robot/vision/CoralDetection.java @@ -4,13 +4,9 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Value; - -import java.util.concurrent.locks.ReentrantLock; import com.team6962.lib.swerve.SwerveDrive; import com.team6962.lib.telemetry.Logger; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,141 +23,167 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import io.limelightvision.LimelightHelpers; import io.limelightvision.LimelightHelpers.LimelightResults; -import io.limelightvision.LimelightHelpers.LimelightTarget_Classifier; import io.limelightvision.LimelightHelpers.LimelightTarget_Detector; +import java.util.concurrent.locks.ReentrantLock; public class CoralDetection extends SubsystemBase { - private static Time MAX_LATENCY = Seconds.of(0.5); - private static Distance MAX_DISTANCE = Meters.of(2); + private static Time MAX_LATENCY = Seconds.of(0.5); + private static Distance MAX_DISTANCE = Meters.of(2); - private String cameraName; + private String cameraName; - private Translation3d cameraPosition; - private Angle cameraPitch; + private Translation3d cameraPosition; + private Angle cameraPitch; - private Angle tx, ty; + private Angle tx, ty; - private SwerveDrive drivetrain; + private SwerveDrive drivetrain; - private Translation3d objectInView = new Translation3d(); - private Translation2d objectOnField = new Translation2d(); + private Translation3d objectInView = new Translation3d(); + private Translation2d objectOnField = new Translation2d(); - private Translation2d lastCoralPosition = null; - private Time lastCoralTimestamp = Seconds.of(-10000); + private Translation2d lastCoralPosition = null; + private Time lastCoralTimestamp = Seconds.of(-10000); - private Time lastMeasurementTimestamp = Seconds.of(-10000); + private Time lastMeasurementTimestamp = Seconds.of(-10000); - private final ReentrantLock loggingLock = new ReentrantLock(); - private StringEntry timestampEntry; + private final ReentrantLock loggingLock = new ReentrantLock(); + private StringEntry timestampEntry; - public CoralDetection(String cameraName, Translation3d cameraPosition, Angle cameraPitch, SwerveDrive drivetrain) { - this.cameraName = cameraName; - this.cameraPosition = cameraPosition; - this.cameraPitch = cameraPitch; - this.drivetrain = drivetrain; + public CoralDetection( + String cameraName, Translation3d cameraPosition, Angle cameraPitch, SwerveDrive drivetrain) { + this.cameraName = cameraName; + this.cameraPosition = cameraPosition; + this.cameraPitch = cameraPitch; + this.drivetrain = drivetrain; - Logger.log("CoralDetection/" + cameraName + "/cameraPosition/x", cameraPosition.getX()); - Logger.log("CoralDetection/" + cameraName + "/cameraPosition/y", cameraPosition.getY()); - Logger.log("CoralDetection/" + cameraName + "/cameraPosition/z", cameraPosition.getZ()); - - Logger.log("CoralDetection/" + cameraName + "/cameraPitch", cameraPitch.in(Degrees)); + Logger.log("CoralDetection/" + cameraName + "/cameraPosition/x", cameraPosition.getX()); + Logger.log("CoralDetection/" + cameraName + "/cameraPosition/y", cameraPosition.getY()); + Logger.log("CoralDetection/" + cameraName + "/cameraPosition/z", cameraPosition.getZ()); - Logger.logMeasure("CoralDetection/" + cameraName + "/tx", () -> tx); - Logger.logMeasure("CoralDetection/" + cameraName + "/ty", () -> ty); + Logger.log("CoralDetection/" + cameraName + "/cameraPitch", cameraPitch.in(Degrees)); - Logger.logMeasure("CoralDetection/" + cameraName + "/detectionTimestamp", () -> lastCoralTimestamp); - Logger.logNumber("CoralDetection/" + cameraName + "/coralPositionX", () -> getCoralLocation() == null ? 0 : getCoralLocation().getX()); - Logger.logNumber("CoralDetection/" + cameraName + "/coralPositionY", () -> getCoralLocation() == null ? 0 : getCoralLocation().getY()); - Logger.logPose("CoralDetection/" + cameraName + "/coralPosition", () -> new Pose2d(getCoralLocation() == null ? 0 : getCoralLocation().getX(), getCoralLocation() == null ? 0 : getCoralLocation().getY(), new Rotation2d())); + Logger.logMeasure("CoralDetection/" + cameraName + "/tx", () -> tx); + Logger.logMeasure("CoralDetection/" + cameraName + "/ty", () -> ty); - timestampEntry = LimelightHelpers.getLimelightNTTable(cameraName).getStringTopic("json").getEntry(""); - } + Logger.logMeasure( + "CoralDetection/" + cameraName + "/detectionTimestamp", () -> lastCoralTimestamp); + Logger.logNumber( + "CoralDetection/" + cameraName + "/coralPositionX", + () -> getCoralLocation() == null ? 0 : getCoralLocation().getX()); + Logger.logNumber( + "CoralDetection/" + cameraName + "/coralPositionY", + () -> getCoralLocation() == null ? 0 : getCoralLocation().getY()); + Logger.logPose( + "CoralDetection/" + cameraName + "/coralPosition", + () -> + new Pose2d( + getCoralLocation() == null ? 0 : getCoralLocation().getX(), + getCoralLocation() == null ? 0 : getCoralLocation().getY(), + new Rotation2d())); - private void processDetectorTarget(LimelightTarget_Detector target, Time timestamp, Time latency) { - if (!target.className.equals("Coral")) return; + timestampEntry = + LimelightHelpers.getLimelightNTTable(cameraName).getStringTopic("json").getEntry(""); + } - tx = Degrees.of(-target.tx_nocrosshair); - ty = Degrees.of(target.ty_nocrosshair); + private void processDetectorTarget( + LimelightTarget_Detector target, Time timestamp, Time latency) { + if (!target.className.equals("Coral")) return; - double focalLength = 1.0; + tx = Degrees.of(-target.tx_nocrosshair); + ty = Degrees.of(target.ty_nocrosshair); - double sensorX = Math.tan(tx.in(Radians)) * focalLength; - double sensorY = Math.tan(ty.in(Radians)) * focalLength; + double focalLength = 1.0; - double distanceForwardFromCameraToGround = -cameraPosition.getZ() / Math.sin(cameraPitch.in(Radians)); + double sensorX = Math.tan(tx.in(Radians)) * focalLength; + double sensorY = Math.tan(ty.in(Radians)) * focalLength; - double slopeOfGroundRelativeToCamera = -Math.tan(cameraPitch.in(Radians)); + double distanceForwardFromCameraToGround = + -cameraPosition.getZ() / Math.sin(cameraPitch.in(Radians)); - double cameraY = distanceForwardFromCameraToGround * sensorY / (focalLength - sensorY * slopeOfGroundRelativeToCamera); - double cameraZ = slopeOfGroundRelativeToCamera * cameraY + distanceForwardFromCameraToGround; - double cameraX = sensorX * cameraZ / focalLength; + double slopeOfGroundRelativeToCamera = -Math.tan(cameraPitch.in(Radians)); - objectInView = new Translation3d(cameraZ, cameraX, cameraY); + double cameraY = + distanceForwardFromCameraToGround + * sensorY + / (focalLength - sensorY * slopeOfGroundRelativeToCamera); + double cameraZ = slopeOfGroundRelativeToCamera * cameraY + distanceForwardFromCameraToGround; + double cameraX = sensorX * cameraZ / focalLength; - Transform3d objectInViewPose = new Transform3d(objectInView, new Rotation3d()); + objectInView = new Translation3d(cameraZ, cameraX, cameraY); - Rotation3d cameraRotation = new Rotation3d(Degrees.of(0), cameraPitch, Degrees.of(180)); - Transform3d cameraPose = new Transform3d(cameraPosition, cameraRotation); + Transform3d objectInViewPose = new Transform3d(objectInView, new Rotation3d()); - Pose3d robotPose = new Pose3d(drivetrain.getEstimatedPose(timestamp)); + Rotation3d cameraRotation = new Rotation3d(Degrees.of(0), cameraPitch, Degrees.of(180)); + Transform3d cameraPose = new Transform3d(cameraPosition, cameraRotation); - Transform3d robotToObject = cameraPose.plus(objectInViewPose); - Pose3d fieldToObject = robotPose.plus(robotToObject); - objectOnField = fieldToObject.getTranslation().toTranslation2d(); + Pose3d robotPose = new Pose3d(drivetrain.getEstimatedPose(timestamp)); - Distance distance = Meters.of(objectOnField.getDistance(drivetrain.getEstimatedPose().getTranslation())); - - // double confidence = 1 - latency.div(MAX_LATENCY).minus(distance.div(MAX_DISTANCE)).in(Value); + Transform3d robotToObject = cameraPose.plus(objectInViewPose); + Pose3d fieldToObject = robotPose.plus(robotToObject); + objectOnField = fieldToObject.getTranslation().toTranslation2d(); - loggingLock.lock(); - - try { - if (timestamp.minus(lastCoralTimestamp).gte(Seconds.of(0.25)) || objectOnField.getDistance(drivetrain.getEstimatedPose().getTranslation()) < lastCoralPosition.getDistance(drivetrain.getEstimatedPose().getTranslation())) { - lastCoralPosition = objectOnField; - lastCoralTimestamp = timestamp; - } - } finally { - loggingLock.unlock(); - } + Distance distance = + Meters.of(objectOnField.getDistance(drivetrain.getEstimatedPose().getTranslation())); + + // double confidence = 1 - latency.div(MAX_LATENCY).minus(distance.div(MAX_DISTANCE)).in(Value); + + loggingLock.lock(); + + try { + if (timestamp.minus(lastCoralTimestamp).gte(Seconds.of(0.25)) + || objectOnField.getDistance(drivetrain.getEstimatedPose().getTranslation()) + < lastCoralPosition.getDistance(drivetrain.getEstimatedPose().getTranslation())) { + lastCoralPosition = objectOnField; + lastCoralTimestamp = timestamp; + } + } finally { + loggingLock.unlock(); } + } - public Translation2d getCoralLocation() { - loggingLock.lock(); + public Translation2d getCoralLocation() { + loggingLock.lock(); - try { - return RobotController.getMeasureTime().minus(lastCoralTimestamp).lt(Seconds.of(1)) ? lastCoralPosition : null; - } finally { - loggingLock.unlock(); - } + try { + return RobotController.getMeasureTime().minus(lastCoralTimestamp).lt(Seconds.of(1)) + ? lastCoralPosition + : null; + } finally { + loggingLock.unlock(); } + } + + @Override + public void periodic() { + // LimelightTarget_Classifier target = new LimelightTarget_Classifier(); + // target.tx = LimelightHelpers.getTX(cameraName); + // target.ty = LimelightHelpers.getTY(cameraName); + // target.className = "Coral"; - @Override - public void periodic() { - // LimelightTarget_Classifier target = new LimelightTarget_Classifier(); - // target.tx = LimelightHelpers.getTX(cameraName); - // target.ty = LimelightHelpers.getTY(cameraName); - // target.className = "Coral"; + // processClassifierTarget(target, RobotController.getMeasureTime(), Seconds.of(0)); + TimestampedString timestampedString = timestampEntry.getAtomic(); + double internalLatencyMs = + LimelightHelpers.getLatency_Pipeline(cameraName) + + LimelightHelpers.getLatency_Capture(cameraName); + Time timestamp = + Seconds.of((timestampedString.timestamp / 1000000.0) - (internalLatencyMs / 1000.0)); - // processClassifierTarget(target, RobotController.getMeasureTime(), Seconds.of(0)); - TimestampedString timestampedString = timestampEntry.getAtomic(); - double internalLatencyMs = LimelightHelpers.getLatency_Pipeline(cameraName) + LimelightHelpers.getLatency_Capture(cameraName); - Time timestamp = Seconds.of((timestampedString.timestamp / 1000000.0) - (internalLatencyMs / 1000.0)); + // if (timestamp.isNear(lastMeasurementTimestamp, Seconds.of(0.001))) return; - // if (timestamp.isNear(lastMeasurementTimestamp, Seconds.of(0.001))) return; - - lastMeasurementTimestamp = timestamp; + lastMeasurementTimestamp = timestamp; - Time latency = Seconds.of(RobotController.getFPGATime()).minus(timestamp); + Time latency = Seconds.of(RobotController.getFPGATime()).minus(timestamp); - // if (latency.gt(MAX_LATENCY)) return; + // if (latency.gt(MAX_LATENCY)) return; - LimelightResults results = LimelightHelpers.getLatestResults(cameraName); - LimelightTarget_Detector[] classifierTargets = results.targets_Detector; + LimelightResults results = LimelightHelpers.getLatestResults(cameraName); + LimelightTarget_Detector[] classifierTargets = results.targets_Detector; - if (classifierTargets != null) { - for (LimelightTarget_Detector target : classifierTargets) { - processDetectorTarget(target, timestamp, latency); - } - } + if (classifierTargets != null) { + for (LimelightTarget_Detector target : classifierTargets) { + processDetectorTarget(target, timestamp, latency); + } } + } } diff --git a/src/test/java/frc/robot/auto/AutoPickupTest.java b/src/test/java/frc/robot/auto/AutoPickupTest.java index 5c54a78e..e71e068e 100644 --- a/src/test/java/frc/robot/auto/AutoPickupTest.java +++ b/src/test/java/frc/robot/auto/AutoPickupTest.java @@ -1,160 +1,164 @@ package frc.robot.auto; -import static edu.wpi.first.units.Units.Degrees; - -import org.junit.jupiter.api.Test; - import com.team6962.lib.utils.KinematicsUtils; - 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.kinematics.ChassisSpeeds; +import org.junit.jupiter.api.Test; public class AutoPickupTest { - private void testAlterTranslationalVelocityToReachTarget(Translation2d translationalVelocity, Translation2d relativeTargetPosition, double strength, Translation2d expectedOutput) { - Translation2d result = AutoPickup.alterTranslationalVelocityToReachTarget(translationalVelocity, relativeTargetPosition, strength); - - double tolerance = 1e-6; - assert Math.abs(result.getX() - expectedOutput.getX()) < tolerance : "X component mismatch: expected " + expectedOutput.getX() + ", got " + result.getX(); - assert Math.abs(result.getY() - expectedOutput.getY()) < tolerance : "Y component mismatch: expected " + expectedOutput.getY() + ", got " + result.getY(); - } - - @Test - public void testAlterTranslationalVelocityToReachTarget_Case1() { - testAlterTranslationalVelocityToReachTarget( - new Translation2d(1, 0), - new Translation2d(1, 1), - 0.5, - new Translation2d(1, 0.5) - ); - } - - @Test - public void testAlterTranslationalVelocityToReachTarget_Case2() { - testAlterTranslationalVelocityToReachTarget( - new Translation2d(0, -1), - new Translation2d(1, -1), - 0.25, - new Translation2d(0.25, -1) - ); - } - - @Test - public void testAlterTranslationalVelocityToReachTarget_Case3() { - testAlterTranslationalVelocityToReachTarget( - new Translation2d(0, -1).rotateBy(Rotation2d.fromDegrees(45)), - new Translation2d(1, -1).rotateBy(Rotation2d.fromDegrees(45)), - 0.25, - new Translation2d(0.25, -1).rotateBy(Rotation2d.fromDegrees(45)) - ); - } - - private void testAlterSpeedsToReachTarget(ChassisSpeeds speeds, Translation2d robotPose, Translation2d target, double strength, ChassisSpeeds expectedOutput) { - ChassisSpeeds result = AutoPickup.alterSpeedsToReachTarget(speeds, robotPose, target, strength); - - double tolerance = 1e-6; - assert Math.abs(result.vxMetersPerSecond - expectedOutput.vxMetersPerSecond) < tolerance : "vx mismatch: expected " + expectedOutput.vxMetersPerSecond + ", got " + result.vxMetersPerSecond; - assert Math.abs(result.vyMetersPerSecond - expectedOutput.vyMetersPerSecond) < tolerance : "vy mismatch: expected " + expectedOutput.vyMetersPerSecond + ", got " + result.vyMetersPerSecond; - assert Math.abs(result.omegaRadiansPerSecond - expectedOutput.omegaRadiansPerSecond) < tolerance : "omega mismatch: expected " + expectedOutput.omegaRadiansPerSecond + ", got " + result.omegaRadiansPerSecond; - } - - @Test - public void testAlterSpeedsToReachTarget_RobotAtOrigin() { - testAlterSpeedsToReachTarget( - new ChassisSpeeds(1, 0, 0), - new Translation2d(0, 0), - new Translation2d(1, 1), - 0.5, - new ChassisSpeeds(1, 0.5, 0) - ); - } - - @Test - public void testAlterSpeedsToReachTarget_RobotNotAtOrigin() { - testAlterSpeedsToReachTarget( - new ChassisSpeeds(0, -1, 0), - new Translation2d(1, 1), - new Translation2d(2, 0), - 0.25, - new ChassisSpeeds(0.25, -1, 0) - ); - } - - @Test - public void testAlterSpeedsToReachTarget_RobotRotated() { - testAlterSpeedsToReachTarget( - KinematicsUtils.rotateSpeeds(new ChassisSpeeds(0, -1, 0), Rotation2d.fromDegrees(45)), - new Translation2d(1, 1).rotateBy(Rotation2d.fromDegrees(45)), - new Translation2d(2, 0).rotateBy(Rotation2d.fromDegrees(45)), - 0.25, - KinematicsUtils.rotateSpeeds(new ChassisSpeeds(0.25, -1, 0), Rotation2d.fromDegrees(45)) - ); - } - - private void testIsTargetReachable(Pose2d robotPose, ChassisSpeeds robotSpeeds, Translation2d target, boolean expectedOutput) { - boolean result = AutoPickup.isTargetReachable(robotPose, robotSpeeds, target); - - assert result == expectedOutput : "Expected " + expectedOutput + ", got " + result; - } - - @Test - public void testIsTargetReachable_SimpleCase1() { - testIsTargetReachable( - new Pose2d(0, 0, Rotation2d.fromDegrees(0)), - new ChassisSpeeds(1, 0, 0), - new Translation2d(1, 0), - true - ); - } - - @Test - public void testIsTargetReachable_SimpleCase2() { - testIsTargetReachable( - new Pose2d(0, 0, Rotation2d.fromDegrees(0)), - new ChassisSpeeds(-1, 0, 0), - new Translation2d(1, 0), - false - ); - } - - @Test - public void testIsTargetReachable_RotatedCase1() { - testIsTargetReachable( - new Pose2d(0, 0, Rotation2d.fromDegrees(180)), - new ChassisSpeeds(-1, 0, 0), - new Translation2d(-1, 0), - true - ); - } - - @Test - public void testIsTargetReachable_RotatedCase2() { - testIsTargetReachable( - new Pose2d(0, 0, Rotation2d.fromDegrees(270)), - new ChassisSpeeds(0, -1, 0), - new Translation2d(0, -1), - true - ); - } - - @Test - public void testIsTargetReachable_RotatedCase3() { - testIsTargetReachable( - new Pose2d(-2, 0, Rotation2d.fromDegrees(180)), - new ChassisSpeeds(-1, 0, 0), - new Translation2d(-1, 0), - false - ); - } - - @Test - public void testIsTargetReachable_RotatedCase4() { - testIsTargetReachable( - new Pose2d(0.5, -4, Rotation2d.fromDegrees(270)), - new ChassisSpeeds(0, -1, 0), - new Translation2d(0, -1), - false - ); - } + private void testAlterTranslationalVelocityToReachTarget( + Translation2d translationalVelocity, + Translation2d relativeTargetPosition, + double strength, + Translation2d expectedOutput) { + Translation2d result = + AutoPickup.alterTranslationalVelocityToReachTarget( + translationalVelocity, relativeTargetPosition, strength); + + double tolerance = 1e-6; + assert Math.abs(result.getX() - expectedOutput.getX()) < tolerance + : "X component mismatch: expected " + expectedOutput.getX() + ", got " + result.getX(); + assert Math.abs(result.getY() - expectedOutput.getY()) < tolerance + : "Y component mismatch: expected " + expectedOutput.getY() + ", got " + result.getY(); + } + + @Test + public void testAlterTranslationalVelocityToReachTarget_Case1() { + testAlterTranslationalVelocityToReachTarget( + new Translation2d(1, 0), new Translation2d(1, 1), 0.5, new Translation2d(1, 0.5)); + } + + @Test + public void testAlterTranslationalVelocityToReachTarget_Case2() { + testAlterTranslationalVelocityToReachTarget( + new Translation2d(0, -1), new Translation2d(1, -1), 0.25, new Translation2d(0.25, -1)); + } + + @Test + public void testAlterTranslationalVelocityToReachTarget_Case3() { + testAlterTranslationalVelocityToReachTarget( + new Translation2d(0, -1).rotateBy(Rotation2d.fromDegrees(45)), + new Translation2d(1, -1).rotateBy(Rotation2d.fromDegrees(45)), + 0.25, + new Translation2d(0.25, -1).rotateBy(Rotation2d.fromDegrees(45))); + } + + private void testAlterSpeedsToReachTarget( + ChassisSpeeds speeds, + Translation2d robotPose, + Translation2d target, + double strength, + ChassisSpeeds expectedOutput) { + ChassisSpeeds result = AutoPickup.alterSpeedsToReachTarget(speeds, robotPose, target, strength); + + double tolerance = 1e-6; + assert Math.abs(result.vxMetersPerSecond - expectedOutput.vxMetersPerSecond) < tolerance + : "vx mismatch: expected " + + expectedOutput.vxMetersPerSecond + + ", got " + + result.vxMetersPerSecond; + assert Math.abs(result.vyMetersPerSecond - expectedOutput.vyMetersPerSecond) < tolerance + : "vy mismatch: expected " + + expectedOutput.vyMetersPerSecond + + ", got " + + result.vyMetersPerSecond; + assert Math.abs(result.omegaRadiansPerSecond - expectedOutput.omegaRadiansPerSecond) < tolerance + : "omega mismatch: expected " + + expectedOutput.omegaRadiansPerSecond + + ", got " + + result.omegaRadiansPerSecond; + } + + @Test + public void testAlterSpeedsToReachTarget_RobotAtOrigin() { + testAlterSpeedsToReachTarget( + new ChassisSpeeds(1, 0, 0), + new Translation2d(0, 0), + new Translation2d(1, 1), + 0.5, + new ChassisSpeeds(1, 0.5, 0)); + } + + @Test + public void testAlterSpeedsToReachTarget_RobotNotAtOrigin() { + testAlterSpeedsToReachTarget( + new ChassisSpeeds(0, -1, 0), + new Translation2d(1, 1), + new Translation2d(2, 0), + 0.25, + new ChassisSpeeds(0.25, -1, 0)); + } + + @Test + public void testAlterSpeedsToReachTarget_RobotRotated() { + testAlterSpeedsToReachTarget( + KinematicsUtils.rotateSpeeds(new ChassisSpeeds(0, -1, 0), Rotation2d.fromDegrees(45)), + new Translation2d(1, 1).rotateBy(Rotation2d.fromDegrees(45)), + new Translation2d(2, 0).rotateBy(Rotation2d.fromDegrees(45)), + 0.25, + KinematicsUtils.rotateSpeeds(new ChassisSpeeds(0.25, -1, 0), Rotation2d.fromDegrees(45))); + } + + private void testIsTargetReachable( + Pose2d robotPose, ChassisSpeeds robotSpeeds, Translation2d target, boolean expectedOutput) { + boolean result = AutoPickup.isTargetReachable(robotPose, robotSpeeds, target); + + assert result == expectedOutput : "Expected " + expectedOutput + ", got " + result; + } + + @Test + public void testIsTargetReachable_SimpleCase1() { + testIsTargetReachable( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new ChassisSpeeds(1, 0, 0), + new Translation2d(1, 0), + true); + } + + @Test + public void testIsTargetReachable_SimpleCase2() { + testIsTargetReachable( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new ChassisSpeeds(-1, 0, 0), + new Translation2d(1, 0), + false); + } + + @Test + public void testIsTargetReachable_RotatedCase1() { + testIsTargetReachable( + new Pose2d(0, 0, Rotation2d.fromDegrees(180)), + new ChassisSpeeds(-1, 0, 0), + new Translation2d(-1, 0), + true); + } + + @Test + public void testIsTargetReachable_RotatedCase2() { + testIsTargetReachable( + new Pose2d(0, 0, Rotation2d.fromDegrees(270)), + new ChassisSpeeds(0, -1, 0), + new Translation2d(0, -1), + true); + } + + @Test + public void testIsTargetReachable_RotatedCase3() { + testIsTargetReachable( + new Pose2d(-2, 0, Rotation2d.fromDegrees(180)), + new ChassisSpeeds(-1, 0, 0), + new Translation2d(-1, 0), + false); + } + + @Test + public void testIsTargetReachable_RotatedCase4() { + testIsTargetReachable( + new Pose2d(0.5, -4, Rotation2d.fromDegrees(270)), + new ChassisSpeeds(0, -1, 0), + new Translation2d(0, -1), + false); + } }