diff --git a/src/main/java/com/team1701/robot/Constants.java b/src/main/java/com/team1701/robot/Constants.java index 851d8880..c2d6a155 100644 --- a/src/main/java/com/team1701/robot/Constants.java +++ b/src/main/java/com/team1701/robot/Constants.java @@ -234,6 +234,9 @@ public static final class Drive { public static final KinematicLimits kShootMoveKinematicLimits; public static final KinematicLimits kMediumTrapezoidalKinematicLimits; + public static final double kShootWhileMoveSpeedCap = 0.5; + public static final double kShootWhileMoveDistanceCap = 3.5; + public static final LoggedTunableNumber kDriveKs = new LoggedTunableNumber("Drive/Module/DriveKs"); public static final LoggedTunableNumber kDriveKv = new LoggedTunableNumber("Drive/Module/DriveKv"); public static final LoggedTunableNumber kDriveKa = new LoggedTunableNumber("Drive/Module/DriveKa"); @@ -433,10 +436,10 @@ public static final class Shooter { new LoggedTunableNumber("Shooter/Rotation/TunableAngleRadians", 1.0); public static final LoggedTunableNumber kLowerAmpSpeed = - new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 70); + new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 75); public static final LoggedTunableNumber kUpperAmpSpeed = - new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 70); + new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 75); public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp"); public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd"); @@ -495,10 +498,11 @@ public static final class Shooter { }; public static final double[][] kShooterForwardVelocityToHeadingOffset = { - {66, 13}, - {73, 9}, - {89, 2}, - {100, 1} + {315 * Math.cos(.975), 9}, + {370 * Math.cos(.689), 6}, + {437.2 * Math.cos(.483), 4}, + {493 * Math.cos(.4), 2}, + {560 * Math.cos(.306), 0} }; public static final double[][] kPassingDistanceToAngleValues = { diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java index d225b609..e6493324 100644 --- a/src/main/java/com/team1701/robot/RobotContainer.java +++ b/src/main/java/com/team1701/robot/RobotContainer.java @@ -387,7 +387,7 @@ private void setupControllerBindings() { mDriverController .leftBumper() .and(() -> mRobotState.getScoringMode().equals(ScoringMode.SPEAKER)) - .whileTrue(ShootCommands.passANote( + .whileTrue(DriveCommands.passANote( mDrive, mShooter, mIndexer, diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index 9069842f..3d7eca3d 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -120,6 +120,10 @@ private Command setNoteToSeek(AutoNote note) { .withName("SetNoteToSeek"); } + private Command clearNoteToSeek() { + return runOnce(() -> mAutoNoteSeeker.clear()).ignoringDisable(true).withName("ClearNoteToSeek"); + } + private Command timedDriveWithVelocity(ChassisSpeeds speeds, double seconds) { return DriveCommands.driveWithVelocity(() -> speeds, mDrive) .withTimeout(seconds) @@ -141,7 +145,7 @@ private Command driveToPoseAndPreWarm(Pose2d pose) { return stopRoutine(); } - var setpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation())); + var setpoint = ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation())); mPathBuilder.addPose(pose); return DriveCommands.driveToPose( mDrive, @@ -203,26 +207,39 @@ private Command driveBackPreWarmAndShoot(String pathName) { runOnce(() -> mRobotState.setUseAutonFallback(false))); } - private Command driveToNextPiece(AutoNote nextNote) { - return new DriveAndSeekNote( - mDrive, - mRobotState, - new DriveToPose( - mDrive, - mRobotState, - () -> new Pose2d( - nextNote.pose().getTranslation(), + private class NextNote { + Pose2d notePose; + + public NextNote() { + notePose = GeometryUtil.kPoseIdentity; + } + } + + private Command driveToNextPiece(AutoNote note) { + NextNote nextNote = new NextNote(); + return loggedSequence( + setNoteToSeek(note), + runOnce(() -> nextNote.notePose = new Pose2d( + note.pose().getTranslation(), mRobotState .getPose2d() .getTranslation() - .minus(nextNote.pose().getTranslation()) - .getAngle()), - mRobotState::getPose2d, - kAutoTrapezoidalKinematicLimits, - true, - true), - mAutoNoteSeeker::getDetectedNoteToSeek, - kAutoTrapezoidalKinematicLimits); + .minus(note.pose().getTranslation()) + .getAngle()) + .transformBy(Constants.Robot.kIntakeToRobot)), + new DriveAndSeekNote( + mDrive, + mRobotState, + new DriveToPose( + mDrive, + mRobotState, + () -> nextNote.notePose, + mRobotState::getPose2d, + kAutoTrapezoidalKinematicLimits, + true, + true), + mAutoNoteSeeker::getDetectedNoteToSeek, + kAutoTrapezoidalKinematicLimits)); } private Pose2d getFirstPose(String pathName) { @@ -266,12 +283,13 @@ private Command followChoreoPathAndSeekNote(String pathName) { mPathBuilder.addPath(trajectory.getPoses()); var eventMarkers = ChoreoEventMarker.loadFromFile(pathName); - return new DriveAndSeekNote( + return clearNoteToSeek() + .andThen(new DriveAndSeekNote( mDrive, mRobotState, new DriveChoreoTrajectory(mDrive, mRobotState, trajectory, eventMarkers, false), mAutoNoteSeeker::getDetectedNoteToSeek, - kAutoTrapezoidalKinematicLimits) + kAutoTrapezoidalKinematicLimits)) .deadlineWith(index(), idleShooter()) .finallyDo(mAutoNoteSeeker::clear) .withName("FollowChoreoAndSeekNote"); @@ -289,7 +307,8 @@ private Command followChoreoPathAndPreWarm(String pathName, boolean resetPose, b mPathBuilder.addPath(trajectory.getPoses()); - var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose())); + var shooterSetpoint = + ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose())); var eventMarkers = ChoreoEventMarker.loadFromFile(pathName); return new DriveChoreoTrajectory( mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose) @@ -305,7 +324,8 @@ private Command followChoreoPathAndShoot(String pathName, boolean resetPose, dou mPathBuilder.addPath(trajectory.getPoses()); - var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose())); + var shooterSetpoint = + ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose())); var eventMarkers = ChoreoEventMarker.loadFromFile(pathName); return new DriveChoreoTrajectory( mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose) diff --git a/src/main/java/com/team1701/robot/commands/DriveCommands.java b/src/main/java/com/team1701/robot/commands/DriveCommands.java index bf82961a..03fef410 100644 --- a/src/main/java/com/team1701/robot/commands/DriveCommands.java +++ b/src/main/java/com/team1701/robot/commands/DriveCommands.java @@ -229,4 +229,26 @@ public static Command driveWithVelocity(Supplier velocity, Drive .finallyDo(() -> drive.stop()) .withName("DriveWithVelocity"); } + + public static Command passANote( + Drive drive, + Shooter shooter, + Indexer indexer, + RobotState robotState, + DoubleSupplier throttleSupplier, + DoubleSupplier strafeSupplier) { + return new PassANote( + drive, + shooter, + indexer, + robotState, + () -> calculateDriveWithJoysticksVelocities( + throttleSupplier.getAsDouble(), + strafeSupplier.getAsDouble(), + drive.getFieldRelativeHeading(), + Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity()) + .rotateBy(robotState.getHeading()), + () -> new Rotation2d(.03)) + .withName("PassANote"); + } } diff --git a/src/main/java/com/team1701/robot/commands/PassANote.java b/src/main/java/com/team1701/robot/commands/PassANote.java index e0b44485..b9c1e103 100644 --- a/src/main/java/com/team1701/robot/commands/PassANote.java +++ b/src/main/java/com/team1701/robot/commands/PassANote.java @@ -1,16 +1,30 @@ package com.team1701.robot.commands; +import java.util.function.Supplier; + +import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits; import com.team1701.lib.util.GeometryUtil; +import com.team1701.lib.util.TimeLockedBoolean; +import com.team1701.lib.util.Util; import com.team1701.lib.util.tuning.LoggedTunableBoolean; import com.team1701.lib.util.tuning.LoggedTunableNumber; +import com.team1701.lib.util.tuning.LoggedTunableValue; import com.team1701.robot.Constants; import com.team1701.robot.states.RobotState; import com.team1701.robot.states.ShootingState; +import com.team1701.robot.subsystems.drive.Drive; import com.team1701.robot.subsystems.indexer.Indexer; import com.team1701.robot.subsystems.shooter.Shooter; import com.team1701.robot.subsystems.shooter.Shooter.ShooterSetpoint; +import com.team1701.robot.util.FieldUtil; import com.team1701.robot.util.ShooterUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import org.littletonrobotics.junction.Logger; @@ -21,48 +35,143 @@ public class PassANote extends Command { new LoggedTunableBoolean(kLoggingPrefix + "TuningEnabled", false); private static final LoggedTunableNumber kAngleToleranceRadians = - new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.01); + new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.02); + + private static final TrapezoidProfile.State kZeroState = new TrapezoidProfile.State(0.0, 0.0); + private static final KinematicLimits kKinematicLimits = Constants.Drive.kFastTrapezoidalKinematicLimits; + + private static final LoggedTunableNumber kMaxAngularVelocity = new LoggedTunableNumber( + kLoggingPrefix + "MaxAngularVelocity", + Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius); + private static final LoggedTunableNumber kMaxAngularAcceleration = + new LoggedTunableNumber(kLoggingPrefix + "MaxAngularAcceleration", kMaxAngularVelocity.get() / 2.0); + + private Supplier mHeadingTolerance; + private final Drive mDrive; private final Shooter mShooter; private final Indexer mIndexer; private final RobotState mRobotState; + private final Supplier mFieldRelativeSpeeds; + private final PIDController mRotationController; - private boolean mShooting; + private TrapezoidProfile mRotationProfile; + private TrapezoidProfile.State mRotationState = kZeroState; - public PassANote(Shooter shooter, Indexer indexer, RobotState robotState) { + private boolean mShooting; + private TimeLockedBoolean mLockedReadyToShoot; + + public PassANote( + Drive drive, + Shooter shooter, + Indexer indexer, + RobotState robotState, + Supplier fieldRelativeSpeeds, + Supplier headingTolerance) { + mDrive = drive; mShooter = shooter; mIndexer = indexer; mRobotState = robotState; + mFieldRelativeSpeeds = fieldRelativeSpeeds; + mRotationController = new PIDController(6.0, 0, 0, Constants.kLoopPeriodSeconds); + mRotationController.enableContinuousInput(-Math.PI, Math.PI); + mRotationProfile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(kMaxAngularVelocity.get(), kMaxAngularAcceleration.get())); + mHeadingTolerance = headingTolerance; + + mLockedReadyToShoot = new TimeLockedBoolean(.1, Timer.getFPGATimestamp()); - addRequirements(shooter, indexer); + addRequirements(mDrive, shooter, indexer); } @Override public void initialize() { mShooting = false; + mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits); + + mRotationController.reset(); + mRotationController.enableContinuousInput(-Math.PI, Math.PI); + + var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity(); + var headingError = mRobotState.getHeading().minus(mRobotState.getPassingHeading()); + mRotationState = new TrapezoidProfile.State( + MathUtil.angleModulus(headingError.getRadians()), fieldRelativeChassisSpeeds.omegaRadiansPerSecond); + + mLockedReadyToShoot.update(false, Timer.getFPGATimestamp()); + + LoggedTunableValue.ifChanged( + hashCode(), + () -> { + mRotationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints( + Math.min( + kMaxAngularVelocity.get(), + kKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius), + Math.min( + kMaxAngularAcceleration.get(), + kKinematicLimits.maxDriveAcceleration() / Constants.Drive.kModuleRadius))); + mRotationController.setPID(6, 0, 0); + }, + kMaxAngularVelocity, + kMaxAngularAcceleration); } @Override public void execute() { - var setpoint = mTuningEnabled.get() + var currentPose = mRobotState.getPose2d(); + var fieldRelativeSpeeds = mFieldRelativeSpeeds.get(); + + var droppedVelocity = ShooterUtil.calculatePassingSpeed(mRobotState.getDistanceToSpeaker()) + * Constants.Shooter.kRollerSpeedToNoteSpeed; + var robotVelocityTowardsTarget = fieldRelativeSpeeds + .rotateBy(mRobotState.getPassingHeading().unaryMinus()) + .getX(); + + var timeInAir = mRobotState.getPassingDistance() / (robotVelocityTowardsTarget + droppedVelocity); + + var endTranslation = new Translation2d( + currentPose.getX() + fieldRelativeSpeeds.getX() * timeInAir, + currentPose.getY() + fieldRelativeSpeeds.getY() * timeInAir); + + var shooterSetpoint = mTuningEnabled.get() ? new ShooterSetpoint( Constants.Shooter.kTunableShooterSpeedRadiansPerSecond.get(), Rotation2d.fromRadians(Constants.Shooter.kTunableShooterAngleRadians.get())) - : ShooterUtil.calculatePassingSetpoint(mRobotState); + : ShooterUtil.calculatePassingSetpoint(FieldUtil.getDistanceToPassTarget(endTranslation)); + + mShooter.setSetpoint(shooterSetpoint); + + var targetHeading = mRobotState.getPassingHeading().minus(shooterSetpoint.releaseAngle()); + var headingError = mRobotState.getHeading().minus(targetHeading); + + Rotation2d headingSetpoint; + double rotationalVelocity; + if (GeometryUtil.isNear(GeometryUtil.kRotationIdentity, headingError, mHeadingTolerance.get()) + && Util.epsilonEquals(fieldRelativeSpeeds.getX(), 0) + && Util.epsilonEquals(fieldRelativeSpeeds.getY(), 0)) { + rotationalVelocity = 0; + mRotationState = kZeroState; + headingSetpoint = targetHeading; + } else { + var rotationPidOutput = mRotationController.calculate(headingError.getRadians(), 0); + mRotationState = mRotationProfile.calculate(Constants.kLoopPeriodSeconds, mRotationState, kZeroState); + rotationalVelocity = mRotationState.velocity + rotationPidOutput; + headingSetpoint = Rotation2d.fromRadians(targetHeading.getRadians() + mRotationState.position); + } - mShooter.setSetpoint(setpoint); + mDrive.setVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeeds.getX(), fieldRelativeSpeeds.getY(), rotationalVelocity, currentPose.getRotation())); - var atSpeed = setpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0); + var atSpeed = shooterSetpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0); var atAngle = GeometryUtil.isNear( - mShooter.getAngle(), setpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get())); + mShooter.getAngle(), shooterSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get())); var atHeading = GeometryUtil.isNear( - mRobotState.getPassingHeading(), mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance); + targetHeading, mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance); var atPose = mRobotState.getPassingDistance() > 6 && !mRobotState.inOpponentWing(); - if (atSpeed && atAngle && atHeading && atPose) { + if (mLockedReadyToShoot.update(atSpeed && atAngle && atHeading && atPose, Timer.getFPGATimestamp())) { mIndexer.setForwardShoot(); mShooting = true; } @@ -75,13 +184,18 @@ public void execute() { } } - mRobotState.setShootingState(new ShootingState(setpoint, true, atAngle, atSpeed, atHeading, mShooting)); + mRobotState.setShootingState(new ShootingState(shooterSetpoint, true, atAngle, atSpeed, atHeading, mShooting)); Logger.recordOutput(kLoggingPrefix + "AtPose", atPose); + Logger.recordOutput(kLoggingPrefix + "HeadingSetpoint", headingSetpoint); } @Override public void end(boolean interrupted) { + mShooting = false; + + mRobotState.setShootingState(ShootingState.kDefault); mShooter.stop(); mIndexer.stop(); + mDrive.stop(); } } diff --git a/src/main/java/com/team1701/robot/commands/Shoot.java b/src/main/java/com/team1701/robot/commands/Shoot.java index 8a40c5a6..45f8d952 100644 --- a/src/main/java/com/team1701/robot/commands/Shoot.java +++ b/src/main/java/com/team1701/robot/commands/Shoot.java @@ -87,7 +87,7 @@ public void execute() { currentPose.getY() + fieldRelativeSpeeds.vyMetersPerSecond * timeInAir); var setpoint = mRobotState.isSpeakerMode() - ? ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation)) + ? ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation)) : ShooterUtil.calculateStationarySetpoint(mRobotState); mShooter.setSetpoint(setpoint); diff --git a/src/main/java/com/team1701/robot/commands/ShootAndMove.java b/src/main/java/com/team1701/robot/commands/ShootAndMove.java index 2556f3fd..c3385607 100644 --- a/src/main/java/com/team1701/robot/commands/ShootAndMove.java +++ b/src/main/java/com/team1701/robot/commands/ShootAndMove.java @@ -50,9 +50,19 @@ public class ShootAndMove extends Command { new LoggedTunableNumber(kLoggingPrefix + "SpeedToleranceRadiansPerSecond", 25.0); private Rotation2d headingTolerance; - private static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber(kLoggingPrefix + "RotationKp", 8.0); - private static final LoggedTunableNumber kRotationKi = new LoggedTunableNumber(kLoggingPrefix + "RotationKi", 0.0); - private static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber(kLoggingPrefix + "RotationKd", 0.75); + private static final LoggedTunableNumber kStationaryRotationKp = + new LoggedTunableNumber(kLoggingPrefix + "StationaryRotationKp", 11.0); + private static final LoggedTunableNumber kStationaryRotationKi = + new LoggedTunableNumber(kLoggingPrefix + "StationaryRotationKi", 0.0); + private static final LoggedTunableNumber kStationaryRotationKd = + new LoggedTunableNumber(kLoggingPrefix + "StationaryRotationKd", 1.0); + + private static final LoggedTunableNumber kMovingRotationKp = + new LoggedTunableNumber(kLoggingPrefix + "MovingRotationKp", 8.0); + private static final LoggedTunableNumber kMovingRotationKi = + new LoggedTunableNumber(kLoggingPrefix + "MovingRotationKi", 0.0); + private static final LoggedTunableNumber kMovingRotationKd = + new LoggedTunableNumber(kLoggingPrefix + "MovingRotationKd", 0.5); private final Drive mDrive; private final Shooter mShooter; @@ -60,7 +70,8 @@ public class ShootAndMove extends Command { private final RobotState mRobotState; private final Supplier mFieldRelativeSpeeds; private final boolean mEndAfterShooting; - private final PIDController mRotationController; + private final PIDController mStationaryRotationController; + private final PIDController mMovingRotationController; private TrapezoidProfile mRotationProfile; private TrapezoidProfile.State mRotationState = kZeroState; @@ -68,6 +79,10 @@ public class ShootAndMove extends Command { private TimeLockedBoolean mLockedReadyToShoot; private boolean mShooting; + private ShooterSetpoint mLastSetpoint = + new ShooterSetpoint(Double.POSITIVE_INFINITY, GeometryUtil.kRotationIdentity); + private Rotation2d mLastTargetHeading = GeometryUtil.kRotationIdentity; + ShootAndMove( Drive drive, Shooter shooter, @@ -81,12 +96,22 @@ public class ShootAndMove extends Command { mRobotState = robotState; mFieldRelativeSpeeds = fieldRelativeSpeeds; mEndAfterShooting = endAfterShooting; - mRotationController = new PIDController( - kRotationKp.get(), kRotationKi.get(), kRotationKd.get(), Constants.kLoopPeriodSeconds); - mRotationController.enableContinuousInput(-Math.PI, Math.PI); + mStationaryRotationController = new PIDController( + kStationaryRotationKp.get(), + kStationaryRotationKi.get(), + kStationaryRotationKd.get(), + Constants.kLoopPeriodSeconds); + + mMovingRotationController = new PIDController( + kMovingRotationKp.get(), + kMovingRotationKi.get(), + kMovingRotationKd.get(), + Constants.kLoopPeriodSeconds); + mStationaryRotationController.enableContinuousInput(-Math.PI, Math.PI); + mMovingRotationController.enableContinuousInput(-Math.PI, Math.PI); mRotationProfile = new TrapezoidProfile( new TrapezoidProfile.Constraints(kMaxAngularVelocity.get(), kMaxAngularAcceleration.get())); - mLockedReadyToShoot = new TimeLockedBoolean(.2, Timer.getFPGATimestamp()); + mLockedReadyToShoot = new TimeLockedBoolean(.1, Timer.getFPGATimestamp()); addRequirements(mDrive, mShooter, mIndexer); } @@ -95,8 +120,10 @@ public class ShootAndMove extends Command { public void initialize() { mDrive.setKinematicLimits(kKinematicLimits); - mRotationController.reset(); - mRotationController.enableContinuousInput(-Math.PI, Math.PI); + mStationaryRotationController.reset(); + mMovingRotationController.reset(); + mStationaryRotationController.enableContinuousInput(-Math.PI, Math.PI); + mMovingRotationController.enableContinuousInput(-Math.PI, Math.PI); var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity(); var headingError = mRobotState.getHeading().minus(mRobotState.getSpeakerHeading()); @@ -104,6 +131,8 @@ public void initialize() { MathUtil.angleModulus(headingError.getRadians()), fieldRelativeChassisSpeeds.omegaRadiansPerSecond); mLockedReadyToShoot.update(false, Timer.getFPGATimestamp()); + mLastSetpoint = new ShooterSetpoint(Double.POSITIVE_INFINITY, GeometryUtil.kRotationIdentity); + mLastTargetHeading = GeometryUtil.kRotationIdentity; LoggedTunableValue.ifChanged( hashCode(), @@ -113,13 +142,19 @@ public void initialize() { Math.min( kMaxAngularAcceleration.get(), kKinematicLimits.maxDriveAcceleration() / kModuleRadius))); - mRotationController.setPID(kRotationKp.get(), kRotationKi.get(), kRotationKd.get()); + if (Util.epsilonEquals(mDrive.getSpeedMetersPerSecond(), 0.0)) { + mStationaryRotationController.setPID( + kStationaryRotationKp.get(), kStationaryRotationKi.get(), kStationaryRotationKd.get()); + } else { + mMovingRotationController.setPID( + kMovingRotationKp.get(), kMovingRotationKi.get(), kMovingRotationKd.get()); + } }, kMaxAngularVelocity, kMaxAngularAcceleration, - kRotationKp, - kRotationKi, - kRotationKd); + kStationaryRotationKp, + kStationaryRotationKi, + kStationaryRotationKd); } @Override @@ -140,7 +175,7 @@ public void execute() { ? new ShooterSetpoint( Constants.Shooter.kTunableShooterSpeedRadiansPerSecond.get(), Rotation2d.fromRadians(Constants.Shooter.kTunableShooterAngleRadians.get())) - : ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation)); + : ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation)); mShooter.setSetpoint(shooterSetpoint); @@ -164,7 +199,7 @@ public void execute() { mRotationState = kZeroState; setpoint = targetHeading; } else { - var rotationPidOutput = mRotationController.calculate(headingError.getRadians(), 0); + var rotationPidOutput = getPIDController().calculate(headingError.getRadians(), 0); mRotationState = mRotationProfile.calculate(Constants.kLoopPeriodSeconds, mRotationState, kZeroState); rotationalVelocity = mRotationState.velocity + rotationPidOutput; setpoint = Rotation2d.fromRadians(targetHeading.getRadians() + mRotationState.position); @@ -174,15 +209,22 @@ public void execute() { fieldRelativeSpeeds.getX(), fieldRelativeSpeeds.getY(), rotationalVelocity, currentPose.getRotation())); var atAngle = GeometryUtil.isNear( - mShooter.getAngle(), shooterSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get())); + mShooter.getAngle(), mLastSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get())); + + var stationaryTargetHeading = FieldUtil.getHeadingToSpeaker(currentPose); - var atHeading = GeometryUtil.isNear(targetHeading, mRobotState.getHeading(), headingTolerance); + var atHeading = GeometryUtil.isNear(mLastTargetHeading, mRobotState.getHeading(), headingTolerance) + || GeometryUtil.isNear(stationaryTargetHeading, mRobotState.getHeading(), headingTolerance); - var atSpeed = shooterSetpoint + var atSpeed = mLastSetpoint .speeds() .allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), kSpeedToleranceRadiansPerSecond.get()); - if (mLockedReadyToShoot.update(atAngle && atHeading && atSpeed, Timer.getFPGATimestamp())) { + var atAcceptableRobotSpeed = mRobotState.getHorizontalToSpeaker() < Constants.Drive.kShootWhileMoveDistanceCap + || mDrive.getSpeedMetersPerSecond() < Constants.Drive.kShootWhileMoveSpeedCap; + + if (mLockedReadyToShoot.update(atAngle && atHeading && atSpeed, Timer.getFPGATimestamp()) + && atAcceptableRobotSpeed) { mIndexer.setForwardShoot(); mShooting = true; } @@ -195,11 +237,16 @@ public void execute() { } } + mLastSetpoint = shooterSetpoint; + mLastTargetHeading = targetHeading; + mRobotState.setShootingState(new ShootingState(shooterSetpoint, true, atAngle, atSpeed, atHeading, mShooting)); Logger.recordOutput(kLoggingPrefix + "Setpoint", new Pose2d(endTranslation, setpoint)); Logger.recordOutput(kLoggingPrefix + "TimeInAir", timeInAir); Logger.recordOutput(kLoggingPrefix + "VelocityTowardsSpeaker", robotVelocityTowardsSpeaker); + Logger.recordOutput(kLoggingPrefix + "TargetHeading", targetHeading); + Logger.recordOutput(kLoggingPrefix + "AtRobotSpeed", atAcceptableRobotSpeed); } @Override @@ -216,4 +263,10 @@ public void end(boolean interrupted) { public boolean isFinished() { return mEndAfterShooting && mShooting && !mRobotState.hasNote(); } + + public PIDController getPIDController() { + return Util.epsilonEquals(mFieldRelativeSpeeds.get().getNorm(), 0.0) + ? mStationaryRotationController + : mMovingRotationController; + } } diff --git a/src/main/java/com/team1701/robot/commands/ShootCommands.java b/src/main/java/com/team1701/robot/commands/ShootCommands.java index 38f937ee..a3f71fca 100644 --- a/src/main/java/com/team1701/robot/commands/ShootCommands.java +++ b/src/main/java/com/team1701/robot/commands/ShootCommands.java @@ -19,9 +19,10 @@ public class ShootCommands { public static Command idleShooterCommand(Shooter shooter, RobotState robotState) { return Commands.runEnd( - () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)), - shooter::stopRotation, - shooter); + () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)), + shooter::stopRotation, + shooter) + .withName("IdleShooterCommand"); } public static Command stop(Shooter shooter) { @@ -51,23 +52,6 @@ public static Command scoreInAmp(Shooter shooter, Indexer indexer, RobotState ro return new Shoot(shooter, indexer, robotState, false); } - public static Command passANote( - Drive drive, - Shooter shooter, - Indexer indexer, - RobotState robotState, - DoubleSupplier throttleSupplier, - DoubleSupplier strafeSupplier) { - return new PassANote(shooter, indexer, robotState) - .deadlineWith(DriveCommands.rotateToPassTarget( - drive, - throttleSupplier, - strafeSupplier, - robotState, - Constants.Drive.kFastTrapezoidalKinematicLimits)) - .withName("PassANote"); - } - public static Command passLow( Drive drive, Shooter shooter, diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java index 6307d17a..817643de 100644 --- a/src/main/java/com/team1701/robot/states/RobotState.java +++ b/src/main/java/com/team1701/robot/states/RobotState.java @@ -22,7 +22,6 @@ import com.team1701.robot.subsystems.intake.Intake; import com.team1701.robot.subsystems.shooter.Shooter; import com.team1701.robot.util.FieldUtil; -import com.team1701.robot.util.ShooterUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -228,10 +227,12 @@ public double getPassingDistance() { @AutoLogOutput public Rotation2d getPassingHeading() { - var difference = getPassingTarget().minus(getPose2d().getTranslation()); - return ShooterUtil.calculatePassingSetpoint(this) - .applyReleaseAngle(new Pose2d(difference, difference.getAngle())) - .getRotation(); + return FieldUtil.getHeadingToPassTarget(getPose2d().getTranslation()); + } + + @AutoLogOutput + public Rotation2d getPassingHeading(Translation2d translation) { + return FieldUtil.getHeadingToPassTarget(translation); } @AutoLogOutput @@ -257,18 +258,14 @@ public Translation3d getAmpPose() { } @AutoLogOutput - public Rotation2d getSpeakerHeading() { - return FieldUtil.getHeadingToSpeaker(getPose2d().getTranslation()); + public double getHorizontalToSpeaker() { + return Math.abs( + getSpeakerPose().toTranslation2d().getDistance(getPose2d().getTranslation())); } - public Rotation2d getMovingSpeakerHeading() { - var fieldRelativeSpeeds = getFieldRelativeSpeeds(); - var projectedTranslation = getPose2d() - .getTranslation() - .plus(new Translation2d( - fieldRelativeSpeeds.vxMetersPerSecond * Constants.kLoopPeriodSeconds, - fieldRelativeSpeeds.vyMetersPerSecond * Constants.kLoopPeriodSeconds)); - return FieldUtil.getHeadingToSpeaker(projectedTranslation); + @AutoLogOutput + public Rotation2d getSpeakerHeading() { + return FieldUtil.getHeadingToSpeaker(getPose2d().getTranslation()); } public Rotation2d getToleranceSpeakerHeading() { @@ -282,8 +279,8 @@ public Rotation2d getToleranceSpeakerHeading(Translation2d translation) { .minus(translation) .getAngle(); - var toleranceRadians = Math.abs( - MathUtil.angleModulus(heading.getRadians() - getSpeakerHeading().getRadians())); + var toleranceRadians = Math.abs(MathUtil.angleModulus(heading.getRadians() + - FieldUtil.getHeadingToSpeaker(translation).getRadians())); return Rotation2d.fromRadians(Math.max(0.017, toleranceRadians / 2)); } diff --git a/src/main/java/com/team1701/robot/util/FieldUtil.java b/src/main/java/com/team1701/robot/util/FieldUtil.java index 91002d53..82c8507e 100644 --- a/src/main/java/com/team1701/robot/util/FieldUtil.java +++ b/src/main/java/com/team1701/robot/util/FieldUtil.java @@ -29,6 +29,20 @@ public static double getDistanceToSpeaker(Pose2d pose) { return getDistanceToSpeaker(pose.getTranslation()); } + public static double getDistanceToPassTarget(Translation2d translation) { + return translation.getDistance( + Configuration.isBlueAlliance() + ? FieldConstants.kBluePassingTarget.getTranslation() + : FieldConstants.kRedPassingTarget.getTranslation()); + } + + public static Rotation2d getHeadingToPassTarget(Translation2d translation) { + var target = Configuration.isBlueAlliance() + ? FieldConstants.kBluePassingTarget.getTranslation() + : FieldConstants.kRedPassingTarget.getTranslation(); + return target.minus(translation).getAngle(); + } + public static Rotation2d getHeadingToSpeaker(Translation2d translation) { var speakerTranslation = Configuration.isBlueAlliance() ? FieldConstants.kBlueSpeakerOpeningCenter diff --git a/src/main/java/com/team1701/robot/util/ShooterUtil.java b/src/main/java/com/team1701/robot/util/ShooterUtil.java index 738444b9..31883edb 100644 --- a/src/main/java/com/team1701/robot/util/ShooterUtil.java +++ b/src/main/java/com/team1701/robot/util/ShooterUtil.java @@ -20,13 +20,17 @@ public static double calculateSpeakerSpeed(double distanceToSpeaker) { : Constants.Shooter.kShooterSpeedInterpolator.get(distanceToSpeaker); } + public static double calculatePassingSpeed(double distanceToTarget) { + return Constants.Shooter.kPassingSpeedInterpolator.get(distanceToTarget); + } + public static double calculateSpeakerAngle(double distanceToSpeaker) { return Constants.Shooter.kUseNewCurves ? Constants.Shooter.kAngleRegression.predict(calculateTheoreticalAngle(distanceToSpeaker)) : Constants.Shooter.kShooterAngleInterpolator.get(distanceToSpeaker); } - public static ShooterSetpoint calculateSetpoint(double distanceToSpeaker) { + public static ShooterSetpoint calculateShooterSetpoint(double distanceToSpeaker) { return new ShooterSetpoint( new ShooterSpeeds(calculateSpeakerSpeed(distanceToSpeaker)), GeometryUtil.clampRotation( @@ -40,11 +44,13 @@ public static ShooterSetpoint calculateStationarySetpoint(RobotState robotState) calculateStationaryRollerSpeeds(robotState), calculateStationaryDesiredAngle(robotState)); } - public static ShooterSetpoint calculatePassingSetpoint(RobotState robotState) { + public static ShooterSetpoint calculatePassingSetpoint(double distanceToTarget) { return new ShooterSetpoint( - Constants.Shooter.kPassingSpeedInterpolator.get(robotState.getPassingDistance()), - Rotation2d.fromRadians( - Constants.Shooter.kPassingAngleInterpolator.get(robotState.getPassingDistance()))); + calculatePassingSpeed(distanceToTarget), + GeometryUtil.clampRotation( + Rotation2d.fromRadians(Constants.Shooter.kPassingAngleInterpolator.get(distanceToTarget)), + Constants.Shooter.kShooterLowerLimit, + Constants.Shooter.kShooterUpperLimit)); } public static ShooterSetpoint calculateIdleSetpoint(RobotState robotState) { diff --git a/src/main/ts/dashboard/src/components/incrementable-number.ts b/src/main/ts/dashboard/src/components/incrementable-number.ts index 92b71d20..86143418 100644 --- a/src/main/ts/dashboard/src/components/incrementable-number.ts +++ b/src/main/ts/dashboard/src/components/incrementable-number.ts @@ -52,7 +52,7 @@ export class IncrementableNumber extends LitElement {
${this.label}
-
${this.value.toFixed(2)}
+
${this.value.toFixed(3)}
diff --git a/src/main/ts/dashboard/src/components/top-bar.ts b/src/main/ts/dashboard/src/components/top-bar.ts index 83e782e7..4af9028a 100644 --- a/src/main/ts/dashboard/src/components/top-bar.ts +++ b/src/main/ts/dashboard/src/components/top-bar.ts @@ -42,7 +42,7 @@ export class TopBar extends LitElement {