Shuttling

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));

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);

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.

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;}

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)));