a

diff --git a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto
index db0b7a00574efe13191da97762c35dbda99d3996..82643b45be5cf310ee9669298dcb9e745f656b07 100644 (file)
--- a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto
+++ b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto
@@ -13,7 +13,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "#2 Left(No SOTM) under the trench"
+            "pathName": "Duplicated Not Working"
           }
         },
         {

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
index eb884e16b02a9450751bb6fcf80c34a4f49891fe..05ae1b66121ce9688dc3c7f207c98b80208c5fee 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -30,7 +30,8 @@ public class ReverseMotors extends Command {
 
     @Override
     public void end(boolean interrupted){
-        intake.spinStop();
+        intake.extend();
+        intake.spinStart();
         //spindexer.maxSpindexer();
     }
 

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 6112b51ffc4515f82f041fdb7b6d336981df8a30..e4aaf459f36821be346b301b55536d6e4a1f2ea0 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -1,5 +1,7 @@
 package frc.robot.commands.gpm;
 
+import static edu.wpi.first.units.Units.Rotation;
+
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.MathUtil;
@@ -68,6 +70,8 @@ public class Superstructure extends Command {
 
     private double turretOffset = 0.0;
 
+    private double distanceFromTarget = 0.0;
+
     public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
         this.drivetrain = drivetrain;
@@ -126,7 +130,7 @@ public class Superstructure extends Command {
                 target == FieldConstants.getHubTranslation().toTranslation2d() ?
                                2.0 : 2.0);
 
-            double TOFAdjustment = 0.75;
+            double TOFAdjustment = 0.85;
             timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
@@ -175,6 +179,7 @@ public class Superstructure extends Command {
         hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
         lastHoodAngle = hoodAngle;
 
+        distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
     }
 
     public void updateDrivePose(){
@@ -226,10 +231,13 @@ public class Superstructure extends Command {
             // if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
             //     hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
             // } else{
-                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity);
+                //hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity);
             // }
 
-            shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
+            hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+            shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+            Logger.recordOutput("Distance From Target", distanceFromTarget);
+            //shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
             // if (phaseManager.shouldFeed()) {
             //     spindexer.maxSpindexer();

diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java
index 4f8ea0d2c27e0ba61c76e44cfe0c81a3392c0cd4..fc6c2b6d019a31f3d65e585f4f21f86e1ad545b9 100644 (file)
--- a/src/main/java/frc/robot/constants/ShotInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShotInterpolation.java
@@ -11,6 +11,10 @@ public class ShotInterpolation {
     
     public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap();
 
+    public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap();
+
+    public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap();
+
     static{
         timeOfFlightMap.put(0.0, 0.67);
         timeOfFlightMap.put(1.0, 0.67);
@@ -41,5 +45,26 @@ public class ShotInterpolation {
         exitVelocityMap.put(11.0, 26.0);
         exitVelocityMap.put(25.0, 25.0* 3.2);
         //exitVelocityMap.put(null, null);
+
+        shooterVelocityMap.put(1.49, 11.5);
+        shooterVelocityMap.put(2.09, 12.5);
+        shooterVelocityMap.put(2.95, 13.5);
+        shooterVelocityMap.put(5.05, 16.0);
+        shooterVelocityMap.put(5.79, 17.0);
+        shooterVelocityMap.put(4.07, 15.5);
+
+        shooterVelocityMap.put(0.0, 9.55);
+        shooterVelocityMap.put(25.0, 43.44);
+
+        newHoodMap.put(1.49, 72.0);
+        newHoodMap.put(2.09, 70.0);
+        newHoodMap.put(2.95, 68.0);
+        newHoodMap.put(5.05, 60.0);
+        newHoodMap.put(5.79, 59.0);
+        newHoodMap.put(4.07, 65.0);
+
+        newHoodMap.put(0.0, 75.9);
+        newHoodMap.put(27.99, 0.0);
+
     }
 }

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index ade7e350dc728ed11ba5533aebe1b2a6a7f13c1d..d19db1b99b4162e85d55f64dd039c4e5378ce644 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -99,6 +99,9 @@ public class Hood extends SubsystemBase implements HoodIO{
                updateInputs();
                Logger.processInputs("Hood", inputs);
 
+               // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees()));
+               // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees());
+
                if (forceHoodDown){
                        goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE);
                        goalVelocityRadPerSec = 0.0;

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 9b1a45a2a4c0ba193a8ef9ebf1217abe431f8242..7b8ff5e41f60ce1214e22ba32ff8fb30e643c915 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -30,7 +30,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
-    double powerModifier = 1.0;
+    double powerModifier = 1.3;
 
     public Shooter(){
         updateInputs();
@@ -59,6 +59,9 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     public void periodic(){
         updateInputs();
 
+        // shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed);
+        // SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed);
+
         powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier);
         SmartDashboard.putNumber("shooter power modifier", powerModifier);