Dynamic Auto Upgrades

AdvantageScopeModels/Robot_Xbot2024/config.json

@@ -8,7 +8,7 @@
     },
     {
       "axis": "z",
-      "degrees": -90
+      "degrees": 90
     }
   ],
   "position": [
@@ -58,15 +58,15 @@
         },
         {
           "axis": "z",
-          "degrees": -90
+          "degrees": 90
         },
         {
           "axis": "y",
           "degrees": 0
         }
 	  ],
       "zeroedPosition": [
-        -0.3,
+        0.3,
         0,
         0.435
       ]

src/main/java/competition/auto_programs/TwoNoteGriefAuto.java

@@ -30,7 +30,7 @@
 import javax.inject.Provider;
 import java.util.ArrayList;
 
-//am i cookin with this?? 🤫🧏‍♂️🤑
+//am i cookin with this??
 //this auto starts in front of the subwoofer
 //scores the note we are holding, then goes to the center and steals the notes to our side via both the scoocher and the collector
 //at the end, it grabs the last centerline note and scores it at the subwoofer.

src/main/java/competition/simulation/Simulator2024.java

@@ -4,6 +4,7 @@
 import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.drive.DriveSubsystem;
+import competition.subsystems.oracle.Availability;
 import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.oracle.Note;
 import competition.subsystems.pose.PoseSubsystem;
@@ -77,7 +78,7 @@ private void visualizeNotes() {
 
     private void simulateCollector() {
         // If we are running the collector and near a note,
-        var nearestNote = oracle.getNoteMap().getClosestNote(pose.getCurrentPose2d().getTranslation(), 0.1, Note.NoteAvailability.Available);
+        var nearestNote = oracle.getNoteMap().getClosest(pose.getCurrentPose2d().getTranslation(), 0.1, Availability.Available, Availability.AgainstObstacle);
         if (nearestNote != null && collector.collectorMotor.getAppliedOutput() > 0.05) {
             // Trigger our sensors to say we have one.
             ((MockDigitalInput)collector.inControlNoteSensor).setValue(true);
@@ -187,18 +188,18 @@ private void initializeNewFiredNote() {
         // create a translation3d representing note start point.
         noteStartPoint = new Translation3d(
                 currentPose.getTranslation().getX()
-                        + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 0.33,
+                        + Math.cos(currentPose.getRotation().getRadians()) * 0.33,
                 currentPose.getTranslation().getY()
-                        + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 0.33,
+                        + Math.sin(currentPose.getRotation().getRadians()) * 0.33,
                 0.33
         );
 
         // create a translation3d representing note finish point.
         noteFinishPoint = new Translation3d(
                 currentPose.getTranslation().getX()
-                        + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 3,
+                        + Math.cos(currentPose.getRotation().getRadians()) * 3,
                 currentPose.getTranslation().getY()
-                        + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 3,
+                        + Math.sin(currentPose.getRotation().getRadians()) * 3,
                 3
         );
     }

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -81,6 +81,14 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
     private int totalLoops = 0;
     private int loopsWhereCompressorRunning = 0;
 
+    //private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136};
+    //private static double[] experimentalArmExtensionsInMm = new double[]{0, 0,  20.0, 26, 41, 57,  64};
+    // TODO: For now, this was a very ugly and quick way to force the arm low, given that all our scoring is coming
+    // from the subwoofer. This will need to be revisited.
+    private static double[] experimentalRangesInInches = new double[]{0, 63};
+    private static double[] experimentalArmExtensionsInMm = new double[]{0, 0};
+
+
 
     public enum ArmState {
         EXTENDING,
@@ -202,19 +210,17 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
             .append(new MechanismLigament2d("box-top", 2, 90))
             .append(new MechanismLigament2d("box-left", 1, 90));
 
-
-        double[] rangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136};
-        double[] rangesInMeters = new double[7];
+        double[] rangesInMeters = new double[experimentalRangesInInches.length];
         //PoseSubsystem.INCHES_IN_A_METER
         // Convert the rangesInInches array to meters
-        for (int i = 0; i < rangesInInches.length; i++) {
-            rangesInMeters[i] = rangesInInches[i] / PoseSubsystem.INCHES_IN_A_METER;
+        for (int i = 0; i < experimentalRangesInInches.length; i++) {
+            rangesInMeters[i] = experimentalRangesInInches[i] / PoseSubsystem.INCHES_IN_A_METER;
         }
 
         speakerDistanceToExtensionInterpolator =
                 new DoubleInterpolator(
                         rangesInMeters,
-                        new double[]{0, 0,  20.0, 26, 41, 57,  64});
+                        experimentalArmExtensionsInMm);
     }
 
     public double constrainPowerIfNearLimit(double power, double actualPosition) {
@@ -643,7 +649,16 @@ public double getRecommendedExtensionForSpeaker() {
 
     @Override
     protected boolean areTwoTargetsEquivalent(Double target1, Double target2) {
-        return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2);
+        return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2, 1);
+    }
+
+    /**
+     * Returns our maximum scoring range. Is useful if the arm is "at target"
+     * but we know there's no way it will actually score.
+     * @return the maximum scoring range in meters
+     */
+    public double getMaximumRangeForAnyShotMeters() {
+        return experimentalRangesInInches[experimentalRangesInInches.length - 1] / PoseSubsystem.INCHES_IN_A_METER;
     }
 
     public void periodic() {

src/main/java/competition/subsystems/collector/CollectorSubsystem.java

@@ -69,6 +69,10 @@ public IntakeState getIntakeState(){
         return intakeState;
     }
     public void intake(){
+        if (shouldCommitToFiring()){
+            return;
+        }
+
         double power = intakePower.get();
         if (getGamePieceInControl()) {
             power *= intakePowerInControlMultiplier.get();
@@ -80,10 +84,22 @@ public void intake(){
         intakeState = IntakeState.INTAKING;
     }
     public void eject(){
+        if (shouldCommitToFiring()){
+            return;
+        }
+
         setPower(ejectPower.get());
         intakeState = IntakeState.EJECTING;
     }
     public void stop(){
+        if (shouldCommitToFiring()){
+            return;
+        }
+        setPower(0);
+        intakeState = IntakeState.STOPPED;
+    }
+
+    public void emergencyStopBypassingJammingPrevention() {
         setPower(0);
         intakeState = IntakeState.STOPPED;
     }
@@ -134,6 +150,15 @@ public boolean confidentlyHasFiredNote() {
         return getSecondsSinceFiringBegan() > waitTimeAfterFiring.get();
     }
 
+    /**
+     * If we start firing for any reason, we have to commit to running the intake at full power
+     * for a few moments to avoid jamming the system.
+     * @return Returns true if we have started, but not finished, the firing process.
+     */
+    public boolean shouldCommitToFiring() {
+        return intakeState == IntakeState.FIRING && !confidentlyHasFiredNote();
+    }
+
     @Override
     public void periodic() {
         if (contract.isCollectorReady()) {

src/main/java/competition/subsystems/oracle/Availability.java

@@ -0,0 +1,10 @@
+package competition.subsystems.oracle;
+
+public enum Availability {
+    Available,
+    AgainstObstacle,
+    ReservedByOthersInAuto,
+    SuggestedByVision,
+    SuggestedByDriver,
+    Unavailable
+}

src/main/java/competition/subsystems/oracle/DynamicOracle.java

@@ -1,10 +1,12 @@
 package competition.subsystems.oracle;
 
+import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.pose.PoseSubsystem;
 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.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.DriverStation;
 import xbot.common.command.BaseSubsystem;
 import xbot.common.subsystems.pose.BasePoseSubsystem;
 import xbot.common.trajectory.LowResField;
@@ -25,54 +27,56 @@ public enum HighLevelGoal {
 
     public enum ScoringSubGoals {
         IngestNote,
+        IngestNoteAgainstObstacle,
         MoveToScoringRange,
         EarnestlyLaunchNote
     }
 
     NoteCollectionInfoSource noteCollectionInfoSource;
     NoteFiringInfoSource noteFiringInfoSource;
     NoteMap noteMap;
+    ScoringLocationMap scoringLocationMap;
     LowResField field;
 
     HighLevelGoal currentHighLevelGoal;
     ScoringSubGoals currentScoringSubGoal;
     boolean firstRunInNewGoal;
 
     PoseSubsystem pose;
+    ArmSubsystem arm;
 
     int instructionNumber = 0;
 
     @Inject
     public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiringInfoSource noteFiringInfoSource,
-                         PoseSubsystem pose) {
+                         PoseSubsystem pose, ArmSubsystem arm) {
         this.noteCollectionInfoSource = noteCollectionInfoSource;
         this.noteFiringInfoSource = noteFiringInfoSource;
         this.noteMap = new NoteMap();
+        this.scoringLocationMap = new ScoringLocationMap();
+
+        // TODO: adjust this during autonomous init
+        noteMap.markAllianceNotesAsUnavailable(DriverStation.Alliance.Red);
+        scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Red);
+
         this.pose = pose;
+        this.arm = arm;
 
         this.currentHighLevelGoal = HighLevelGoal.CollectNote;
-        this.currentScoringSubGoal = ScoringSubGoals.IngestNote;
+        this.currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote;
         firstRunInNewGoal = true;
         setupLowResField();
 
         //reserveNote(Note.KeyNoteNames.BlueSpikeMiddle);
         //reserveNote(Note.KeyNoteNames.BlueSpikeBottom);
-
-        Pose2d scoringPositionTop = new Pose2d(1.3, 6.9, Rotation2d.fromDegrees(180));
-        Pose2d scoringPositionMiddle = new Pose2d(1.5, 5.5, Rotation2d.fromDegrees(180));
-        Pose2d scoringPositionBottom = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(180));
-
-        activeScoringPosition = scoringPositionMiddle;
-        //createRobotObstacle(scoringPositionMiddle.getTranslation(), 1.75, "PartnerA");
-        //createRobotObstacle(scoringPositionBottom.getTranslation(), 1.75, "PartnerB");
     }
 
     Pose2d activeScoringPosition;
 
     int noteCount = 1;
     private void reserveNote(Note.KeyNoteNames specificNote) {
-        Note reserved = noteMap.getNote(specificNote);
-        reserved.setAvailability(Note.NoteAvailability.ReservedByOthersInAuto);
+        Note reserved = noteMap.get(specificNote);
+        reserved.setAvailability(Availability.ReservedByOthersInAuto);
         // create an obstacle at the same location.
         field.addObstacle(new Obstacle(reserved.getLocation().getTranslation().getX(),
                 reserved.getLocation().getTranslation().getY(), 1.25, 1.25, "ReservedNote" + noteCount));
@@ -155,15 +159,29 @@ public void requestReevaluation() {
         reevaluationRequested = true;
     }
 
+    /**
+     * Should be called in AutonomousInit.
+     * Idea: the operator will use the NeoTrellis to lay out a plan, and then either press a button to
+     * lock in that plan (or make updates to the plan), or the plan will automatically lock in at the start of auto.
+     */
+    public void freezeConfigurationForAutonomous() {
+    }
+
     @Override
     public void periodic() {
 
+        // Need to spend a moment figuring out what alliance we are on and what state of the game we are in.
+        // For example,
+
+
         switch (currentHighLevelGoal) {
             case ScoreInAmp: // For now keeping things simple
             case ScoreInSpeaker:
                 if (firstRunInNewGoal || reevaluationRequested) {
                     setTargetNote(null);
-                    setTerminatingPoint(activeScoringPosition);
+                    setTerminatingPoint(scoringLocationMap.getClosest(pose.getCurrentPose2d().getTranslation(),
+                            Availability.Available).getLocation());
+
                     currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange;
                     setSpecialAimTarget(new Pose2d(0, 5.5, Rotation2d.fromDegrees(0)));
                     // Choose a good speaker scoring location
@@ -184,18 +202,35 @@ public void periodic() {
             case CollectNote:
                 if (firstRunInNewGoal || reevaluationRequested) {
                     // Choose a good note collection location
-                    Note suggestedNote = noteMap.getClosestNote(pose.getCurrentPose2d().getTranslation(),
-                            Note.NoteAvailability.Available,
-                            Note.NoteAvailability.SuggestedByDriver,
-                            Note.NoteAvailability.SuggestedByVision);
+                    Note suggestedNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(),
+                            Availability.Available,
+                            Availability.AgainstObstacle,
+                            Availability.SuggestedByDriver,
+                            Availability.SuggestedByVision);
                     setTargetNote(suggestedNote);
-                    setSpecialAimTarget(null);
+
                     if (suggestedNote == null) {
                         // No notes on the field! Let's suggest going to the source and hope something turns up.
-                        setTerminatingPoint(new Pose2d(14, 1.2, Rotation2d.fromDegrees(0)));
-                    } else {
+                        setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.NearbySource));
+                    }
+                    else if (suggestedNote.getAvailability() == Availability.AgainstObstacle) {
+                        // Take the note's pose2d and extend it in to the super far distance so the robot
+                        // will effectively aim at the "wall" of the obstacle as it approaches the note.
+                        Pose2d superExtendedIntoTheDistancePose = new Pose2d(
+                                new Translation2d(
+                                        suggestedNote.getLocation().getTranslation().getX()
+                                                + Math.cos(suggestedNote.getLocation().getRotation().getRadians()) * 10000000,
+                                        suggestedNote.getLocation().getTranslation().getY()
+                                                + Math.sin(suggestedNote.getLocation().getRotation().getRadians()) * 10000000),
+                                suggestedNote.getLocation().getRotation()
+                                );
+                        setSpecialAimTarget(superExtendedIntoTheDistancePose);
+                        setTerminatingPoint(suggestedNote.getLocation());
+                    }
+                    else {
                         setTerminatingPoint(getTargetNote().getLocation());
                     }
+
                     // Publish a route from current position to that location
                     firstRunInNewGoal = false;
                     reevaluationRequested = false;
@@ -205,9 +240,9 @@ public void periodic() {
 
                 if (noteCollectionInfoSource.confidentlyHasControlOfNote()) {
                     // Mark the nearest note as being unavailable, if we are anywhere near it
-                    Note nearestNote = noteMap.getClosestNote(pose.getCurrentPose2d().getTranslation(), 1.0);
+                    Note nearestNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), 1.0);
                     if (nearestNote != null) {
-                        nearestNote.setAvailability(Note.NoteAvailability.Unavailable);
+                        nearestNote.setAvailability(Availability.Unavailable);
                     }
 
                     // Since we have a note, let's go score it.
@@ -223,8 +258,11 @@ public void periodic() {
         aKitLog.record("Current Goal", currentHighLevelGoal);
         aKitLog.record("Current Note",
                 targetNote == null ? new Pose2d(-100, -100, new Rotation2d(0)) : getTargetNote().getLocation());
-        aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose());
-        aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber());
+
+        if (getTerminatingPoint() != null) {
+            aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose());
+            aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber());
+        }
         aKitLog.record("Current SubGoal", currentScoringSubGoal);
 
         // Let's show some major obstacles
@@ -296,15 +334,18 @@ public ScoringSubGoals getScoringSubgoal() {
 
     private void determineScoringSubgoal() {
         double acceptableRangeBeforeScoringMeters = 0;
+        boolean inUnderstoodRange = false;
         if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) {
-            acceptableRangeBeforeScoringMeters = 2;
+            acceptableRangeBeforeScoringMeters = 0.2;
+            inUnderstoodRange = pose.getDistanceFromSpeaker() < arm.getMaximumRangeForAnyShotMeters();
+
         } else if (currentHighLevelGoal == HighLevelGoal.ScoreInAmp) {
             // in the future we'll do something more like "get near amp, then drive into the wall for a few moments
             // before scoring"
             acceptableRangeBeforeScoringMeters = 0.05;
         }
 
-        if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters)) {
+        if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters) && inUnderstoodRange) {
             currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote;
         } else {
             currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange;