diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ba77933..994e29a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,14 +13,20 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; + import java.util.Set; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.subsystems.shooter.ShooterConstants; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -39,9 +45,32 @@ public final class Constants { private static final RobotType robotType = RobotType.SIMBOT; public static final boolean spawnLessFuelInSim = true; + public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. + //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub + //but I think it will work so yeah trust me butch + public static final double shootOnMoveMaxSpeed = 2.0/5.0; + public static final double aimOnMoveMaxSpeed = 2.0/3.0; // obsolete rn, but change if we ever add a aim mode again public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; + + public static final Distance fieldLength = Inches.of(651.22); + public static final Distance fieldWidth = Inches.of(317.69); + + public static final Distance allianceZoneBlue = Inches.of(156.61); + public static final Distance allianceZoneRed = fieldLength.minus(allianceZoneBlue); + + public static final Translation2d blueLeftFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(5.0/6.0)); + public static final Translation2d blueRightFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(1.0/6.0)); + public static final Translation2d redLeftFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(5.0/6.0)); + public static final Translation2d redRightFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(1.0/6.0)); + public static Translation2d getHubTranslation(Alliance alliance){ + return ( + alliance == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose + ); + } } public static class FieldConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 57846d6..8becd29 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,6 @@ import frc.robot.subsystems.shooter.HoodIOServo; import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.RobotCommands; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; @@ -250,7 +249,6 @@ public RobotContainer() { configureBindings(); configureDriveBindings(); } - // Bind robot actions to commands here. private void configureBindings() { // Manually deploying and undeploying the intake. @@ -270,15 +268,22 @@ private void configureBindings() { ) ); - // While the right trigger is held, we will shoot into the hub or ferry. - gamepad_.rightTrigger().whileTrue(RobotCommands.shoot(shooter_, hopper_, drivebase_)); - - // When the hopper isnt shooting, set it to run its idle velocity. // hopper_.setDefaultCommand(hopper_.idleScrambler()); // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); + shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, drivebase_::getVirtualTarget)); + // while in alliance zone, point drivebase at virtual target, but still allow translational driving + //lowkey we are not gonna need this but like maybe so idk imma just keep it + + // While the right trigger is held, we will shoot into the hub. + // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. + //CHANGE BACK TO RIGHT TRIGGER! + // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. + // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk + gamepad_.rightTrigger().whileTrue( + shooter_.shootCmd(drivebase_, hopper_, gamepad_, Constants.shootOnMove) + ); //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. gamepad_.a().whileTrue(intake_.ejectSequence()); diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 8cd1a78..4586af5 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -58,11 +58,15 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.MapleSimUtil; +import frc.robot.util.VirtualTarget; public class DriveCommands { private static final double kStoppedVelocity = 0.15 ; @@ -238,6 +242,45 @@ public static Command joystickDriveAtAngle( // Reset PID controller when command starts .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + + public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier target, boolean shootOnMove){ + return joystickDriveAtAngle( + drive, + () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, + () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, + () -> { + Logger.recordOutput("Drive/Target", target.get()); + var translation = target.get().minus(drive.getPose().getTranslation()); + var rotation = new Rotation2d(translation.getX(), translation.getY()); + + return rotation; + }); + } + + private static Translation2d getTarget(Drive drive) { + Translation2d target; + + boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; + + boolean robotInAllianceZone = blueDS ? + drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue): + drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); + + if(robotInAllianceZone){ + target = DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)); + }else if(drive.getPose().getMeasureY().gt(DriveConstants.fieldWidth.div(2.0))){ + target = blueDS ? DriveConstants.blueLeftFerryTarget : DriveConstants.redLeftFerryTarget ; + }else{ + target = blueDS ? DriveConstants.blueRightFerryTarget : DriveConstants.redRightFerryTarget ; + } + + return target; + } + + // make sure this isnt bad with intellisense later + public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ + return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot) : getTarget(drive)), shootOnMove); + } /** * Measures the velocity feedforward constants for the drive motors. diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5c4550f..0c37fa8 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -12,7 +12,6 @@ // GNU General Public License for more details. package frc.robot.subsystems.drive; - import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -61,9 +60,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.util.LocalADStarAK; +import frc.robot.util.VirtualTarget; public class Drive extends SubsystemBase { // These Constants should be the same for every drivebase, so just use the comp bot constants. @@ -219,10 +221,8 @@ public void periodic() { for (var module : modules) { module.stop(); } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { + // Log empty setpoint states when disabled Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } @@ -466,6 +466,12 @@ public double getMaxAngularSpeedRadPerSec() { public RobotConfig getPathplannerConfig() { return PP_CONFIG; } + + + + public Translation2d getVirtualTarget() { + return VirtualTarget.getVirtualTargetFromTarget(this, DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)), ShooterConstants.hangTimeOnShot); + } /** Returns an array of module translations. */ public Translation2d[] getModuleTranslations() { diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index e032a89..11e2f8c 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -141,6 +141,9 @@ public Command feed(AngularVelocity feeder, AngularVelocity scrambler) { * Runs the scrambler at its active speed, and the feeder. * @return */ + + // idk how to do this but like we should run the feeder motor backwards for about 0.5 secs to get arid of the balls stuck in there after we stop shooting + // theoretically this would improve consistency public Command forwardFeed() { return feed(HopperConstants.scramblerShootingVelocity, HopperConstants.feedingVelocity); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 49d6906..77ac6c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -17,7 +16,6 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; 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.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -31,20 +29,18 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; +import frc.robot.commands.drive.DriveCommands; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; -import frc.robot.subsystems.drive.Drive; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.subsystems.intake.IntakeSubsystem; -import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { @@ -142,12 +138,15 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { * @param pos * @return */ - // public Command shootCmd(Hopper hopper) { - // return Commands.parallel( - // runDynamicSetpoints(() -> RPM.of(5000), () -> Degrees.of(30)), - // hopper.forwardFeed() - // ); - // } + public Command shootCmd(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { + return Commands.parallel( + DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), + Commands.sequence( + Commands.waitTime(ShooterConstants.dbRotationDelay), + shooterSetpointSupplier(() -> drive.getVirtualTarget().minus(drive.getPose().getTranslation()), hopper) + ) + ); + } /** * The command that the shooter can run whenever its not shooting to manage @@ -155,6 +154,33 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { * or lowering the hood under the trench. * @return A command that does so. */ + public Command awaitShooting(Supplier robotPose, Supplier targetPose) { + return runDynamicSetpoints(() -> RadiansPerSecond.zero(), () -> { + Pose2d pose = robotPose.get(); + Pose2d nearestTrench = pose.nearest(FieldConstants.trenches); + Distance nearestDistance = Meters.of(pose.getTranslation().getDistance(nearestTrench.getTranslation())); + + if (nearestDistance.lte(ShooterConstants.allowedTrenchDistance)) { + return Degrees.zero(); + } + + Translation2d hubTranslation = targetPose.get(); + + Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation)); + + switch(HubDistance.fromDistance(distanceToHub)) { + case LOW: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + case MEDIUM: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + case HIGH: + return Degrees.of(ShooterConstants.Positions.hoodHIGH); + default: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + + } + }); + } public Command awaitShooting(Supplier robotPose) { return runDynamicSetpoints(() -> { @@ -174,19 +200,7 @@ public Command awaitShooting(Supplier robotPose) { var vel = 0.0; if ((Math.abs(pose.getX() - zone.magnitude()) < ShooterConstants.Positions.spinUpZone.magnitude())) { - switch(HubDistance.fromDistance(distanceToHub)) { - case LOW: - vel = ShooterConstants.Positions.distMapLow.get(distanceToHub.magnitude()); - - case MEDIUM: - vel = ShooterConstants.Positions.distMapMed.get(distanceToHub.magnitude()); - - case HIGH: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - - default: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - } + vel = ShooterConstants.Positions.distMap.get(distanceToHub.magnitude()); } return RotationsPerSecond.of(vel); }, @@ -247,7 +261,9 @@ public Command runVoltageCmd(Voltage vol) { public Command hoodToPosCmd(Angle pos) { return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position"); } - + public Command runSetpoints(AngularVelocity vel, Angle pos) { + return startEnd(() -> setSetpoints(vel, pos), this::stopShooter); + } /** * Calculates Velocity and Hood Angle based on distance and Shoots * @@ -269,26 +285,57 @@ public Command shoot(Supplier pose, Hopper hopper) { return runDynamicSetpoints(() -> { Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); var vel = 0.0; + vel = ShooterConstants.Positions.distMap.get(distanceToHub.magnitude()); + return RotationsPerSecond.of(vel); + }, + + () -> { + // periodic + Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + switch(HubDistance.fromDistance(distanceToHub)) { case LOW: - vel = ShooterConstants.Positions.distMapLow.get(distanceToHub.magnitude()); + return Degrees.of(ShooterConstants.Positions.hoodLOW); case MEDIUM: - vel = ShooterConstants.Positions.distMapMed.get(distanceToHub.magnitude()); + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); case HIGH: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); + return Degrees.of(ShooterConstants.Positions.hoodHIGH); default: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - } + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + } + }); + }), + hopper.forwardFeed() + ); + } + + /** + * Calculates Velocity and Hood Angle based on distance and Shoots + * + * + */ + public Command shooterSetpointSupplier(Supplier dist, Hopper hopper) { + + return Commands.parallel( + defer(() -> { + + // constructing + + ShooterConstants.Positions.initMap(); + + return runDynamicSetpoints(() -> { + Distance distanceToHub = Meters.of(Math.sqrt(dist.get().getX() * dist.get().getX() + dist.get().getY() * dist.get().getY())); + var vel = ShooterConstants.Positions.distMap.get(distanceToHub.baseUnitMagnitude()); return RotationsPerSecond.of(vel); }, () -> { // periodic - Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + Distance distanceToHub = Meters.of(Math.sqrt(dist.get().getX() * dist.get().getX() + dist.get().getY() * dist.get().getY())); switch(HubDistance.fromDistance(distanceToHub)) { case LOW: @@ -379,16 +426,16 @@ public Command hoodCalibration() { } - /** - * Shoot balls from the shooter until the command ends. - * @return - */ - public Command shootCmd(Hopper hopper) { - return Commands.parallel( - runDynamicSetpoints(() -> RPM.of(5000), () -> Degrees.of(30)), - hopper.forwardFeed() - ); - } + // /** + // * Shoot balls from the shooter until the command ends. + // * @return + // */ + // public Command shootCmd(Hopper hopper) { + // return Commands.parallel( + // runDynamicSetpoints(() -> RevolutionsPerSecond.of(5000.0/60.0), () -> Degrees.of(30)), + // hopper.forwardFeed() + // ); + // } /** * The command that the shooter can run whenever its not shooting to manage @@ -409,46 +456,4 @@ public Command shootCmd(Hopper hopper) { // return Degrees.of(45); // TODO: replace this with whatever determines shooter angle // }); // } - - public Command ferryToOutpost(Drive drive, Hopper hopper, IntakeSubsystem intake, DoubleSupplier xSupplier, DoubleSupplier ySupplier){ - return new ConditionalCommand( - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - ()-> 0, - () -> 0, - () -> { - Translation2d ferryTarget = ShooterConstants.FerryPositions.blueOutpostTarget; - - var targetTranslation= ferryTarget.minus(drive.getPose().getTranslation()); - var targetRotation= new Rotation2d(targetTranslation.getX(), targetTranslation.getY()); - return targetRotation; - } - ), - shootCmd(hopper) - ), - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - ()-> 0, - () -> 0, - () -> { - Translation2d ferryTarget= ShooterConstants.FerryPositions.redOutpostTarget; - - var targetTranslation= ferryTarget.minus(drive.getPose().getTranslation()); - var targetRotation= new Rotation2d(targetTranslation.getX(), targetTranslation.getY()); - return targetRotation; - } - ), - shootCmd(hopper) - ), - - () -> { - if(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue){ - return true; - } - return false; - } - ); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index d70d466..6dd273c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -31,6 +31,8 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); + public static final Time hangTimeOnShot = Seconds.of(8/4.5);//number taken from video, 8 frames / 4.5 frames/sec seems to be about the right hang time. this should be tuned better later tho + public static final Time dbRotationDelay = Seconds.of(0.5); public static final double timeBeforeShoot = 0.2; public static final Angle hoodParkedAngle = Degrees.of(5.0) ; @@ -53,7 +55,7 @@ public class PID { public static final double shooterkA = 0.0; public static final double shooterkG = 0.0; public static final double shooterkS = 0.0; - } + } public class MotionMagic { @@ -61,10 +63,15 @@ public class MotionMagic { public static final double shooterkMaxVelocity = 1000.0; public static final double shooterkMaxAcceleration = 3000.0; public static final double shooterkJerk = 0.0; + + // hood + public static final double hoodkMaxVelocity = 0.0; + public static final double hoodkMaxAcceleration = 300.0; + public static final double hoodkJerk = 0.0; } public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; + public static final double hoodMaxAngle = Positions.hoodHIGH; public static final double hoodMinAngle = 0.0; } @@ -91,6 +98,8 @@ public class Positions { public static final double hoodLOW = 0; public static final double hoodMEDIUM = 1; public static final double hoodHIGH = 2; + + //why are these not all Distances/Angles? // Tested Distances public static final double dist1 = 1; @@ -105,6 +114,9 @@ public class Positions { public static final double dist8 = 8; public static final double dist9 = 9; + public static final double distLowMid = 3.5; + public static final double distMidHigh = 6.5; + // Tested Velocities public static final double low1 = 10; public static final double low2 = 20; @@ -118,25 +130,23 @@ public class Positions { public static final double high2 = 80; public static final double high3 = 90; - public static final InterpolatingDoubleTreeMap distMapLow = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap distMapMed = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap distMapHigh = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap distMap = new InterpolatingDoubleTreeMap(); public static void initMap() { // Hood Low - distMapLow.put(dist1, low1); - distMapLow.put(dist2, low2); - distMapLow.put(dist3, low3); + distMap.put(dist1, low1); + distMap.put(dist2, low2); + distMap.put(dist3, low3); // Hood Med - distMapMed.put(dist4, med1); - distMapMed.put(dist5, med2); - distMapMed.put(dist6, med3); + distMap.put(dist4, med1); + distMap.put(dist5, med2); + distMap.put(dist6, med3); // Hood High - distMapHigh.put(dist7, high1); - distMapHigh.put(dist8, high2); - distMapHigh.put(dist9, high3); + distMap.put(dist7, high1); + distMap.put(dist8, high2); + distMap.put(dist9, high3); } public enum HubDistance { diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java new file mode 100644 index 0000000..f53f315 --- /dev/null +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -0,0 +1,18 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Time; +import frc.robot.subsystems.drive.Drive; + +public class VirtualTarget { + public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, Time hangTime){ + return target.minus( + new Translation2d( + MetersPerSecond.of(drive.getFieldChassisSpeeds().vxMetersPerSecond).times(hangTime), + MetersPerSecond.of(drive.getFieldChassisSpeeds().vyMetersPerSecond).times(hangTime) + ) + ); + } +}