diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index def5d1c..c849df7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,7 +208,6 @@ private void configureBindings() { new ShooterHoodDown(shooterHood), new AutoAim(shooterHood, driveTrain))); - // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. //operatorController.back().and(operatorController.y()).whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); diff --git a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java index edd474d..52bf295 100644 --- a/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/Chassis/TeleopDrive.java @@ -32,6 +32,8 @@ public class TeleopDrive extends Command { private final SwerveRequest.FieldCentric drive; private Translation2d hubVector = new Translation2d(0, 0); + private Translation2d upShuttleVector = new Translation2d(0,0); + private Translation2d downShuttleVector = new Translation2d(0,0); /** Creates a new TeleopDrive. */ public TeleopDrive(CommandSwerveDrivetrain driveTrain, CommandPS5Controller controller, SwerveRequest.FieldCentric drive, @@ -56,6 +58,16 @@ public void initialize() { hubVector = new Translation2d(Units.inchesToMeters(181.56 + 287),Units.inchesToMeters(158.32)); } } + + if(DriverStation.getAlliance().isPresent()) { + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84-90.95)); + } else { + upShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84+90.95)); + downShuttleVector = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84-90.95)); + } + } } // Called every time the scheduler runs while the command is scheduled. @@ -65,7 +77,11 @@ public void execute() { ChassisSpeeds targetSpeeds = driveTrain.accelLimitVectorDrive(driveTrain.getHIDspeedsMPS(controller)); if(Math.abs(controller.getRightY()) > 0.7) { shooter.interpolTargetSpeed(driveTrain, shooterHood); //bandaid fix - targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); + if(driveTrain.getStatePose().getX() > Units.inchesToMeters(182.11) && driveTrain.getStatePose().getX() < Units.inchesToMeters(469.11)) { //If the robot is in the neutral Zone shuttle + targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocityWhileShuttlingNotHub(shooter, upShuttleVector, downShuttleVector, controller); + } else { //Else it must be in the alliance zone so toggle hub + targetSpeeds.omegaRadiansPerSecond = driveTrain.getRotationalVelocity(shooter, hubVector, controller); + } } else { driveTrain.driveLimiter.setMaxAccel(Constants.Swerve.maxAccelerationFromRest); driveTrain.driveLimiter.setNegativeRateLimit(-5); diff --git a/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java b/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java index 7267565..857773e 100644 --- a/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java +++ b/src/main/java/frc/robot/commands/ShooterHood/PID/AutoAim.java @@ -29,7 +29,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooterHood.autoAim(drivetrain.getDistanceFromHub()); + shooterHood.autoAim(drivetrain.getDistanceForAutoAim()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d7f3da9..6f8b311 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -286,7 +286,7 @@ public ChassisSpeeds getHIDspeedsMPS(CommandPS5Controller driverController) { yAxis = MathUtil.applyDeadband(yAxis, Constants.Swerve.kDeadband); rotation = MathUtil.applyDeadband(rotation, Constants.Swerve.kDeadband); double maxSpeed = 0; - if(Math.abs(driverController.getRightY()) > 0.7) { + if(Math.abs(driverController.getRightY()) > 0.7 && getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { maxSpeed = 1; } else { maxSpeed = Constants.Swerve.maxSpeed; @@ -319,9 +319,47 @@ public Translation2d getTranslationToHub() { } } - public double getDistanceFromHub() { + public Translation2d getTranslationToShuttle() { + if(DriverStation.getAlliance().isEmpty()) {return new Translation2d(0, 0);} + if(getStatePose().getY() >= Units.inchesToMeters(158.84)) {//if the robot is above the hub then this is the translation + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + Translation2d originToShuttleUpBlue = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d blue = getStatePose().getTranslation().minus(originToShuttleUpBlue).unaryMinus(); + return blue; + } else { + Translation2d originToShuttleUpRed = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84+90.95)); + Translation2d red = getStatePose().getTranslation().minus(originToShuttleUpRed).unaryMinus(); + return red; + } + } else {//If not above, then below so get below translation + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + Translation2d originToShuttleDownBlue = new Translation2d(Units.inchesToMeters(181.56-39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d blue = getStatePose().getTranslation().minus(originToShuttleDownBlue).unaryMinus(); + return blue; + } else { + Translation2d originToShuttleDownRed = new Translation2d(Units.inchesToMeters(181.56+287+39.37),Units.inchesToMeters(158.84-90.95)); + Translation2d red = getStatePose().getTranslation().minus(originToShuttleDownRed).unaryMinus(); + return red; + } + } + + } + + public Translation2d getTranslationForAutoAim() { + if(getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { + return getTranslationToShuttle(); + } else { + return getTranslationToHub(); + } + } + + public double getDistanceForAutoAim() { + if(getStatePose().getX() > Units.inchesToMeters(182.11) && getStatePose().getX() < Units.inchesToMeters(469.11)) { + return getTranslationToShuttle().getNorm(); + } else { return getTranslationToHub().getNorm(); } +} /** @@ -469,39 +507,58 @@ public ChassisSpeeds getSpeeds() { return this.getState().Speeds; } - public double getRotationalVelocity(Shooter shooter, Translation2d hubVector, CommandPS5Controller controller) { - Translation2d robotVector = getState().Pose.getTranslation(); - Translation2d targetVector = hubVector.minus(robotVector); - double targetAngle = targetVector.getAngle().getDegrees(); - double robotAngle = getState().Pose.getRotation().getDegrees(); - //correct for movement and face hub - targetAngle = getAngleSetpoint(); - driveLimiter.setMaxAccel(3); - driveLimiter.setNegativeRateLimit(-3); - // Keep angles in the range (-180, 180) - if(targetAngle - robotAngle > 180) { - targetAngle -= 360; - } else if(targetAngle - robotAngle < -180) { - targetAngle += 360; - } - // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s - if(Math.abs(robotAngle - targetAngle) < 5 && getState().Speeds.omegaRadiansPerSecond < 1) { - setFacingHub(true); - } else { - setFacingHub(false); - } - double angleInput = angularPIDController.calculate(robotAngle, targetAngle); - - Translation2d robotFieldVel = new Translation2d( - getState().Speeds.vxMetersPerSecond, - getState().Speeds.vyMetersPerSecond - ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity - // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector - Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); - // Angle correct the opposite direction of movement using w = -v/R - double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); - - return angleInput + angleOutput; + public double getRotationalVelocity(Shooter shooter, Translation2d targetSpotVector, CommandPS5Controller controller) { + Translation2d robotVector = getState().Pose.getTranslation(); + Translation2d targetVector = targetSpotVector.minus(robotVector); + double targetAngle = targetVector.getAngle().getDegrees(); + double robotAngle = getState().Pose.getRotation().getDegrees(); + // If shooting correct for movement, otherwise just target hub regardless of where you are + if (shooter.getIsShooting()) { + targetAngle = getAngleSetpoint(); + driveLimiter.setMaxAccel(3); + driveLimiter.setNegativeRateLimit(-3); + } else { + targetAngle = targetSpotVector.minus(robotVector).getAngle().getDegrees(); + } + // Keep angles in the range (-180, 180) + if(targetAngle - robotAngle > 180) { + targetAngle -= 360; + } else if(targetAngle - robotAngle < -180) { + targetAngle += 360; + } + // Robot angle is within 3 degrees of target angle and rotational velocityy is less than 0.2 rad/s + if(Math.abs(robotAngle - targetAngle) < 3 && getState().Speeds.omegaRadiansPerSecond < 1) { + setFacingHub(true); + } else { + setFacingHub(false); + } + double angleInput = angularPIDController.calculate(robotAngle, targetAngle); + + Translation2d robotFieldVel = new Translation2d( + getState().Speeds.vxMetersPerSecond, + getState().Speeds.vyMetersPerSecond + ).rotateBy(getState().Pose.getRotation()); // Field Relative Robot Velocity + // Calculation of unit tangent vector. Take vector to hub, rotate by 90 degrees to get tangential vector, normalize tangential vector + Translation2d unitTangent = targetVector.rotateBy(new Rotation2d(Math.PI/2)).div(targetVector.getNorm()); + // Angle correct the opposite direction of movement using w = -v/R + double angleOutput = -unitTangent.dot(robotFieldVel)/targetVector.getNorm(); + + return angleInput + angleOutput; + } + + public double getRotationalVelocityHub(Shooter shooter, Translation2d hubVector, CommandPS5Controller controller) { + return getRotationalVelocity(shooter, hubVector, controller); + } + + public double getRotationalVelocityWhileShuttlingNotHub(Shooter shooter, Translation2d upShuttleVector, Translation2d downShuttleVector, CommandPS5Controller controller) { + if(getStatePose().getY() >= Units.inchesToMeters(158.84)){ + return getRotationalVelocity(shooter, upShuttleVector, controller); + + } if (getStatePose().getY() < Units.inchesToMeters(158.84)) {//If robot is below the hub on the map + return getRotationalVelocity(shooter, downShuttleVector, controller); + } else { //If robot is out of field + return 0; + } } // public double getkPAngular() {return angularkP;} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0408d82..eca8009 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -157,7 +157,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public void revAtVelocity(double velocityMetersPerSec, CommandSwerveDrivetrain driveTrain, ShooterHood shooterHood) { Translation2d ballVelocityVector = driveTrain.getTranslationToHub(); ballVelocityVector = ballVelocityVector.times(0.45 * velocityMetersPerSec/ballVelocityVector.getNorm()); - ballVelocityVector = ballVelocityVector.times(Math.cos(Math.toRadians(360 * shooterHood.getTableAutoAimValue(driveTrain.getDistanceFromHub()) + 9))); + ballVelocityVector = ballVelocityVector.times(Math.cos(Math.toRadians(360 * shooterHood.getTableAutoAimValue(driveTrain.getDistanceForAutoAim()) + 9))); Translation2d robotVelocityVector = new Translation2d(driveTrain.getRobotRelativeSpeeds().vxMetersPerSecond, driveTrain.getRobotRelativeSpeeds().vyMetersPerSecond); Translation2d velocityVector = ballVelocityVector.minus(robotVelocityVector); double newVelocityMetersPerSec = velocityVector.getNorm(); @@ -259,13 +259,15 @@ public double getInterPolVel(double distance) { return Math.sqrt(velmps); } + // Interpolation Request for Velocity public double interpolTargetSpeed(CommandSwerveDrivetrain driveTrain, ShooterHood shooterHood) { - double distance = driveTrain.getDistanceFromHub(); + + double distance = driveTrain.getDistanceForAutoAim(); double velmps = tableVel.get(distance); // 1. Get the direction to the hub (Must point AT the hub) - Translation2d toHubDir = driveTrain.getTranslationToHub(); + Translation2d toHubDir = driveTrain.getTranslationForAutoAim(); // 2. Calculate the "Static" components (what the ball does if robot is still) double hoodAngleRads = Math.toRadians(81 - (360 * shooterHood.getTableAutoAimValue(distance)));