a

diff --git a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path
index 587ab10aa6a5f3d20dee952767f7e7a7a7d0daef..fa5e4eb13b7ad1904b898784849493d7a72cca22 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path
@@ -119,6 +119,28 @@
           ]
         }
       }
+    },
+    {
+      "name": "Hood Down",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Hood Down"
+        }
+      }
+    },
+    {
+      "name": "Stop Hood Down",
+      "waypointRelativePos": 0.5534308211473522,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Stop Hood Down"
+        }
+      }
     }
   ],
   "globalConstraints": {

diff --git a/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path b/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path
index 4470532c9f140efb7df25e0639468ef3751d0ed4..d1bb09c17c5a81050dcba7af6fccb1199afb633a 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path
@@ -3,13 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5467141162514837,
-        "y": 0.6942704626334513
+        "x": 4.396690391459075,
+        "y": 0.43604982206405685
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.83681954887218,
-        "y": 0.7017406015037613
+        "x": 6.580806642941875,
+        "y": 0.823380782918149
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.301672597864768,
+        "y": 0.823380782918149
+      },
+      "prevControl": {
+        "x": 6.72798848994848,
+        "y": 0.776996631189148
+      },
+      "nextControl": {
+        "x": 7.911232317280483,
+        "y": 0.8726655898756941
       },
       "isLocked": false,
       "linkedName": null
@@ -20,12 +36,12 @@
         "y": 1.5158029801324506
       },
       "prevControl": {
-        "x": 8.742932937101976,
-        "y": 0.1380740879492448
+        "x": 8.57960288807054,
+        "y": 0.5564572982632667
       },
       "nextControl": {
-        "x": 8.5023178807947,
-        "y": 2.1111175496688723
+        "x": 8.571257413997628,
+        "y": 2.265112692763938
       },
       "isLocked": false,
       "linkedName": null
@@ -36,8 +52,8 @@
         "y": 3.57
       },
       "prevControl": {
-        "x": 8.519890563503097,
-        "y": 2.960165562913907
+        "x": 8.62505338078292,
+        "y": 2.275871886120997
       },
       "nextControl": null,
       "isLocked": false,
@@ -46,19 +62,19 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.42628774422735277,
+      "waypointRelativePos": 0.85,
       "rotationDegrees": 29.999999999999996
     },
     {
-      "waypointRelativePos": 0.6975023126734518,
+      "waypointRelativePos": 1.4,
       "rotationDegrees": 90.84924216679515
     }
   ],
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 0.7792207792207767,
-      "maxWaypointRelativePos": 1.6989130434782616,
+      "minWaypointRelativePos": 1.55,
+      "maxWaypointRelativePos": 2.6989130434782616,
       "constraints": {
         "maxVelocity": 3.0,
         "maxAcceleration": 3.0,
@@ -71,7 +87,7 @@
     {
       "name": "Constraints Zone",
       "minWaypointRelativePos": 0,
-      "maxWaypointRelativePos": 0.7151943462897536,
+      "maxWaypointRelativePos": 1.45,
       "constraints": {
         "maxVelocity": 6.0,
         "maxAcceleration": 5.0,
@@ -83,7 +99,49 @@
     }
   ],
   "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Extend Intake"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "Spin Intake Rollers"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "Hood Down"
+              }
+            }
+          ]
+        }
+      }
+    },
+    {
+      "name": "Stop Hood Down",
+      "waypointRelativePos": 0.65,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Stop Hood Down"
+        }
+      }
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,

diff --git a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
index 0ae719259f04ee04e9b9ddfbdc99eae899c17ece..0e4cf18ddd235f281a4388dbf01615449386d095 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
@@ -114,6 +114,28 @@
           ]
         }
       }
+    },
+    {
+      "name": "Hood Down",
+      "waypointRelativePos": 0.9111361079864995,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Hood Down"
+        }
+      }
+    },
+    {
+      "name": "Stop Hood Down",
+      "waypointRelativePos": 1.7885264341957245,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Stop Hood Down"
+        }
+      }
     }
   ],
   "globalConstraints": {

diff --git a/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path b/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path
index d854b6873af1fb81fe93ec0b596052f4b18b5826..2e7f0803c5344aa09737080a585f13c23bd9f5c3 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path
@@ -66,7 +66,32 @@
     }
   ],
   "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Retract Intake",
+      "waypointRelativePos": 0.5354330708661407,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Retract Intake"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "Hood Down"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,

diff --git a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path
index 53e7d7795cd369a3ca91d1becfd0b9c4c19f8bc6..0bec0ed9705d5c740e545a61a859259892763b98 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.9893708609271523,
-        "y": 6.002997790234502
+        "x": 0.925765932523468,
+        "y": 5.968318426038064
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 5.966
       },
       "prevControl": {
-        "x": 1.3625978647686838,
-        "y": 5.944756820877817
+        "x": 1.497518014658156,
+        "y": 5.9695580132966395
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 35478b047cd36d20dcfb25e207adafcefd7889d8..926877bf4cc603b56864435f170b1cdf63b38563 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -92,7 +92,7 @@ public class RobotContainer {
     SmartDashboard.putString("RobotID", robotId.toString());
 
     // Filling the SendableChooser on SmartDashboard
-    autoChooserInit();
+    //autoChooserInit();
 
     // dispatch on the robot
     switch (robotId) {
@@ -141,12 +141,13 @@ public class RobotContainer {
         PathGroupLoader.loadPathGroups();
         // Load the auto command
         try {
-          String leftSideAuto = "Left Side Auto";
+          String leftSideAuto = "Left(No SOTM) - Under Trench";
           PathPlannerAuto.getPathGroupFromAutoFile(leftSideAuto);
           auto = new PathPlannerAuto(leftSideAuto);
         } catch (IOException | ParseException e) {
           e.printStackTrace();
         }
+        
         if(turret != null){
           turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
         }
@@ -218,9 +219,8 @@ public class RobotContainer {
     }
 
     if (hood != null){
-      Command hoodDown = new InstantCommand(()-> {hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0);}, hood);
-      NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hoodDown.schedule();}));
-      NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hoodDown.cancel();}));
+      NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hood.forceHoodDown(true);}));
+      NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hood.forceHoodDown(false);}));
     }
 
 
@@ -268,11 +268,8 @@ public class RobotContainer {
     }
   }
 
-  public Command getAutoCommand() {
-    // get the selected Command
-    Command autoSelected = autoChooser.getSelected();
-
-    return autoSelected;
+  public Command getAutoCommand(){
+    return auto;
   }
 
   public void logComponents() {

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 3aa1ee9ad0f4f8daa2548c8b927e2f6a63aced2d..7cae9fc5fe5bc907e4d85916b0066ea6c83f623f 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -190,9 +190,9 @@ public class AutoShootCommand extends Command {
         hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
         shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
-        SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
-        SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
-        SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
+        Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
+        Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
+        Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
 
         /** Spindexer Stuff!! */
         if(spindexer != null){

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
new file mode 100644 (file)
index 0000000..7ffc366
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -0,0 +1,31 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+
+public class RunSpindexer extends Command {
+    private Spindexer spindexer;
+    private Turret turret;
+
+    public RunSpindexer(Spindexer spindexer, Turret turret){
+        this.spindexer = spindexer;
+        this.turret = turret;
+        
+        addRequirements(spindexer);
+    }
+
+    @Override
+    public void execute() {
+        //if (turret.atSetpoint()){
+            spindexer.maxSpindexer();
+        // } else{
+        //     spindexer.stopSpindexer();
+        // }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        spindexer.stopSpindexer();
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 0de26731ac75a7346796f2423ab525af9867541b..6112b51ffc4515f82f041fdb7b6d336981df8a30 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -124,7 +124,7 @@ public class Superstructure extends Command {
                                        Translation2d.kZero,
                                target3d.minus(lookahead3d),
                 target == FieldConstants.getHubTranslation().toTranslation2d() ?
-                               2.0 : 5.0);
+                               2.0 : 2.0);
 
             double TOFAdjustment = 0.75;
             timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
@@ -213,7 +213,7 @@ public class Superstructure extends Command {
         SmartDashboard.putNumber("Turret Offset", turretOffset);
         // Phase manager stuff
         phaseManager.update(drivepose, shooter, turret);
-        target = phaseManager.getTarget();
+        target = phaseManager.getTarget(drivepose);
 
         updateDrivePose();
         updateSetpoints(drivepose);
@@ -223,11 +223,11 @@ public class Superstructure extends Command {
         } else {
             turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
             
-            if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
-                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
-            } else{
+            // 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);
-            }
+            // }
 
             shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
@@ -238,9 +238,9 @@ public class Superstructure extends Command {
             // }
         }
 
-        SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
-        SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
-        SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
+        Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
+        Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
+        Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
 
         SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
     }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index ecc2bd1185f023a2537b8fbb8187031b08bba728..aa53754468b62f0d34a10b8896a5fb1a9e7110af 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -16,6 +16,7 @@ import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
@@ -83,7 +84,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 () -> false, getDrivetrain()).withTimeout(2));
 
         // Trench align
-        controller.get(DPad.LEFT).whileTrue(new StartEndCommand(
+        controller.get(PS5Button.CIRCLE).whileTrue(new StartEndCommand(
                 () -> {
                     getDrivetrain().setTrenchAssist(true);
                     getDrivetrain().setTrenchAlign(true);
@@ -95,7 +96,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
         // Reverse motors
         if (intake != null && spindexer != null) {
-            controller.get(PS5Button.CIRCLE).whileTrue(new ReverseMotors(intake, spindexer));
             controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake, spindexer));
         }
 
@@ -141,8 +141,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         // Spindexer
         if (spindexer != null) {
             // Will only run if we are not calling default shoot command
-            controller.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
-                    .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
+            controller.get(PS5Button.LEFT_TRIGGER).whileTrue(new RunSpindexer(spindexer, turret));
         }
 
         // Auto shoot

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index ebe389f0a5da796942951ffa2ec25b2dfaecff3d..9b1a45a2a4c0ba193a8ef9ebf1217abe431f8242 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 = .75;
+    double powerModifier = 1.0;
 
     public Shooter(){
         updateInputs();

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 3a83e8dc0f74a35834ea638e1f7de539863eaf4a..773cb78a8bd666ce6265356a19020086f40e452f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -146,10 +146,10 @@ public class Turret extends SubsystemBase implements TurretIO{
        }
 
        /**
-        * @return If the turret is at setpoint with tolerance of 5 degrees
+        * @return If the turret is at setpoint with tolerance of 10 degrees
         */
        public boolean atSetpoint() {
-               return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(5.0);
+               return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(10.0);
        }
 
        /**
@@ -210,7 +210,7 @@ public class Turret extends SubsystemBase implements TurretIO{
                double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO;
 
                // Clamp position setpoint to min and max angles
-               motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(180) * TurretConstants.GEAR_RATIO);
+               motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO);
                        
                // Multiply goal velocity by kV
                double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV;

diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java
index 9eb3931db16ac7ba692f543067ee01684255fe0d..c5e445191f1c9a66fb9adbdf80215d35371a4436 100644 (file)
--- a/src/main/java/frc/robot/util/PhaseManager.java
+++ b/src/main/java/frc/robot/util/PhaseManager.java
@@ -87,7 +87,8 @@ public class PhaseManager {
     public Translation2d getTarget(Pose2d drivePose) {
         return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d()
                 : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation())
-                        ? FieldConstants.getAllianceSideTranslation(true).toTranslation2d()
-                        : FieldConstants.getAllianceSideTranslation(false).toTranslation2d());
+                //TODO: reversed for sm reason
+                        ? FieldConstants.getAllianceSideTranslation(false).toTranslation2d()
+                        : FieldConstants.getAllianceSideTranslation(true).toTranslation2d());
     }
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index c6e12083ffa3c9b7203ccaafe35732057fc7f569..f54bf47bf4f068ddcc7527ea26d2ead47ee4af97 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -22,8 +22,8 @@ public class TrenchAssistConstants {
     };
 
     public static final double[] SLIDE_LATITUDES = new double[] {
-            FieldConstants.FIELD_WIDTH - Units.inchesToMeters(30.0),
-            Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small
+            FieldConstants.FIELD_WIDTH - Units.inchesToMeters(27.0),
+            Units.inchesToMeters(27.0), // should be accurate, i think our field is slightly too small
             // 6.550,
             // 0.668,