Enrich DynamicOracle with more obstacle/note utilities for automatic pathing

src/main/java/competition/Robot.java

@@ -8,19 +8,27 @@
 import competition.injection.components.DaggerSimulationComponent;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.pose.PoseSubsystem;
+import edu.wpi.first.hal.AllianceStationID;
 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.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import xbot.common.command.BaseRobot;
 import xbot.common.math.FieldPose;
-import xbot.common.subsystems.drive.BaseDriveSubsystem;
+import xbot.common.math.MovingAverage;
+import xbot.common.math.MovingAverageForDouble;
+import xbot.common.math.MovingAverageForTranslation2d;
 import xbot.common.subsystems.pose.BasePoseSubsystem;
 
+import java.util.LinkedList;
+import java.util.Queue;
+
 public class Robot extends BaseRobot {
 
+    public Robot() {
+    }
+
     @Override
     protected void initializeSystems() {
         super.initializeSystems();
@@ -75,6 +83,7 @@ public void simulationInit() {
         // hassle of navigating to the DS window and re-enabling the simulated robot.
         DriverStationSim.setEnabled(true);
         //webots.setFieldPoseOffset(getFieldOrigin());
+        DriverStationSim.setAllianceStationId(AllianceStationID.Blue1);
     }
 
     private FieldPose getFieldOrigin() {
@@ -88,6 +97,11 @@ private FieldPose getFieldOrigin() {
             );
     }
 
+    MovingAverageForTranslation2d translationAverageCalculator =
+            new MovingAverageForTranslation2d(15);
+    MovingAverageForDouble rotationAverageCalculator =
+            new MovingAverageForDouble(15);
+
     @Override
     public void simulationPeriodic() {
         super.simulationPeriodic();
@@ -96,17 +110,28 @@ public void simulationPeriodic() {
         double robotLoopPeriod = 0.02;
         double poseAdjustmentFactorForSimulation = robotTopSpeedInMetersPerSecond * robotLoopPeriod;
 
-        double robotTopAngularSpeedInDegreesPerSecond = 180.0;
+        double robotTopAngularSpeedInDegreesPerSecond = 360;
         double headingAdjustmentFactorForSimulation = robotTopAngularSpeedInDegreesPerSecond * robotLoopPeriod;
 
         var pose = (PoseSubsystem)getInjectorComponent().poseSubsystem();
         var currentPose = pose.getCurrentPose2d();
         DriveSubsystem drive = (DriveSubsystem)getInjectorComponent().driveSubsystem();
+
+        // Extremely simple physics simulation. We want to give the robot some very basic translational and rotational
+        // inertia. We can take the moving average of the last second or so of robot commands and apply that to the
+        // robot's pose. This is a very simple way to simulate the robot's movement without having to do any real physics.
+
+        translationAverageCalculator.add(drive.lastRawCommandedDirection);
+        var currentAverage = translationAverageCalculator.getAverage();
+
+        rotationAverageCalculator.add(drive.lastRawCommandedRotation);
+        var currentRotationAverage = rotationAverageCalculator.getAverage();
+
         var updatedPose = new Pose2d(
                 new Translation2d(
-                        currentPose.getTranslation().getX() + drive.lastRawCommandedDirection.x * poseAdjustmentFactorForSimulation,
-                        currentPose.getTranslation().getY() + drive.lastRawCommandedDirection.y * poseAdjustmentFactorForSimulation),
-                currentPose.getRotation().plus(Rotation2d.fromDegrees(drive.lastRawCommandedRotation * headingAdjustmentFactorForSimulation)));
+                        currentPose.getTranslation().getX() + currentAverage.getX() * poseAdjustmentFactorForSimulation,
+                        currentPose.getTranslation().getY() + currentAverage.getY() * poseAdjustmentFactorForSimulation),
+                currentPose.getRotation().plus(Rotation2d.fromDegrees(currentRotationAverage * headingAdjustmentFactorForSimulation)));
         pose.setCurrentPoseInMeters(updatedPose);
     }
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -68,7 +68,7 @@ public void setupMyCommands(
 
 
         oracleSwerve.logic.setEnableConstantVelocity(true);
-        oracleSwerve.logic.setConstantVelocity(3);
+        oracleSwerve.logic.setConstantVelocity(2.8);
         oracleSwerve.logic.setFieldWithObstacles(oracle.getFieldWithObstacles());
 
         operatorInterface.driverGamepad.getXboxButton(XboxButton.Back).whileTrue(oracleSwerve);

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

@@ -4,14 +4,15 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.math.trajectory.Trajectory;
 import xbot.common.command.BaseSubsystem;
+import xbot.common.subsystems.pose.BasePoseSubsystem;
 import xbot.common.trajectory.LowResField;
 import xbot.common.trajectory.Obstacle;
-import xbot.common.trajectory.XbotSwervePoint;
 
 import javax.inject.Inject;
 import javax.inject.Singleton;
+import java.util.ArrayList;
 
 @Singleton
 public class DynamicOracle extends BaseSubsystem {
@@ -54,45 +55,83 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri
         firstRunInNewGoal = true;
         setupLowResField();
 
-        reserveNote(Note.KeyNoteNames.SpikeMiddle);
-        reserveNote(Note.KeyNoteNames.SpikeBottom);
+        reserveNote(Note.KeyNoteNames.BlueSpikeMiddle);
+        reserveNote(Note.KeyNoteNames.BlueSpikeBottom);
 
         Pose2d scoringPositionTop = new Pose2d(1.3, 6.9, Rotation2d.fromDegrees(0));
         Pose2d scoringPositionMiddle = new Pose2d(1.5, 5.5, Rotation2d.fromDegrees(0));
         Pose2d scoringPositionBottom = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(0));
 
         activeScoringPosition = scoringPositionTop;
-        createRobotObstacle(scoringPositionMiddle.getTranslation(), 1.5, "PartnerA");
-        createRobotObstacle(scoringPositionBottom.getTranslation(), 1.5, "PartnerB");
+        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);
         // create an obstacle at the same location.
         field.addObstacle(new Obstacle(reserved.getLocation().getTranslation().getX(),
-                reserved.getLocation().getTranslation().getY(), 1.25, 1.25, "ReservedNote"));
+                reserved.getLocation().getTranslation().getY(), 1.25, 1.25, "ReservedNote" + noteCount));
+        noteCount++;
     }
 
     private void createRobotObstacle(Translation2d location, double sideLength, String name) {
         field.addObstacle(new Obstacle(location.getX(), location.getY(), sideLength, sideLength, name));
     }
 
-    private void createObstacleWithRobotWidth(double x, double y, double width, double height,
+    private Obstacle createObstacleWithRobotWidth(Translation2d location, double width, double height,
+                                                  double robotWidth, String name, LowResField field) {
+        return createObstacleWithRobotWidth(location.getX(), location.getY(), width, height, robotWidth, name, field);
+    }
+
+
+
+    private Obstacle createObstacleWithRobotWidth(double x, double y, double width, double height,
                                               double robotWidth, String name, LowResField field) {
-        double calculatedWidth = (robotWidth / 2) + width;
-        field.addObstacle(new Obstacle(x, y, calculatedWidth, height, name));
+        var obstacle = new Obstacle(x, y, width+robotWidth, height+robotWidth, name);
+        field.addObstacle(obstacle);
+        return obstacle;
     }
 
     private LowResField setupLowResField() {
         field = new LowResField();
         // For now, just add the three columns in the middle.
-        createObstacleWithRobotWidth(3.2004, 4.105656, 0.254,0.254, .914, "BlueLeftStageColumn", field);
+        // !!These values seem incorrect on simulator!!.
+        // createObstacleWithRobotWidth(3.2004, 4.105656, 0.254,0.254, .914, "BlueLeftStageColumn", field);
         // widths and height are different to account for angle differences
-        createObstacleWithRobotWidth(5.8129, 5.553456, 0.3469,0.3469, .914, "BlueTopStageColumn", field);
-        createObstacleWithRobotWidth(5.8129,  2.657856,0.3469, 0.3469, .914, "BlueBottomStageColumn", field);
+        //createObstacleWithRobotWidth(5.8129, 5.553456, 0.3469,0.3469, .914, "BlueTopStageColumn", field);
+        //createObstacleWithRobotWidth(5.8129,  2.657856,0.3469, 0.3469, .914, "BlueBottomStageColumn", field);
+
+        // Blue obstacles
+        createObstacleWithRobotWidth(PoseSubsystem.BlueLeftStageColumn,
+                PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, .914, "BlueLeftStageColumn", field);
+        createObstacleWithRobotWidth(PoseSubsystem.BlueTopStageColumn,
+                PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914,"BlueTopStageColumn", field);
+        createObstacleWithRobotWidth(PoseSubsystem.BlueBottomStageColumn,
+                PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "BlueBottomStageColumn", field);
+
+        var speaker = createObstacleWithRobotWidth(PoseSubsystem.BlueSubwoofer,
+                PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "BlueSubwoofer", field);
+        speaker.defaultBottomLeft = false;
+        speaker.defaultTopLeft = false;
+
+        // Red obstacles
+        createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueLeftStageColumn),
+                PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, .914, "RedLeftStageColumn", field);
+        createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueTopStageColumn),
+                PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "RedTopStageColumn", field);
+        createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueBottomStageColumn),
+                PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "RedBottomStageColumn", field);
+
+        speaker = createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer),
+                PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "RedSubwoofer", field);
+        speaker.defaultBottomLeft = false;
+        speaker.defaultTopLeft = false;
+
         return field;
     }
 
@@ -186,6 +225,32 @@ public void periodic() {
                 targetNote == null ? new Pose2d(-100, -100, new Rotation2d(0)) : getTargetNote().getLocation());
         aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose());
         aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber());
+
+        // Let's show some major obstacles
+        field.getObstacles().forEach(obstacle -> {
+            aKitLog.record(obstacle.getName(), obstacleToTrajectory(obstacle));
+        });
+    }
+
+    private Trajectory obstacleToTrajectory(Obstacle o) {
+        // create a trajectory using the 4 corners of the obstacle.
+        ArrayList<Trajectory.State> wpiStates = new ArrayList<>();
+        var topLeftcorner = new Trajectory.State();
+        topLeftcorner.poseMeters = new Pose2d(o.topLeft, Rotation2d.fromDegrees(0));
+        var topRightCorner = new Trajectory.State();
+        topRightCorner.poseMeters = new Pose2d(o.topRight, Rotation2d.fromDegrees(0));
+        var bottomLeftCorner = new Trajectory.State();
+        bottomLeftCorner.poseMeters = new Pose2d(o.bottomLeft, Rotation2d.fromDegrees(0));
+        var bottomRightCorner = new Trajectory.State();
+        bottomRightCorner.poseMeters = new Pose2d(o.bottomRight, Rotation2d.fromDegrees(0));
+
+        wpiStates.add(topLeftcorner);
+        wpiStates.add(topRightCorner);
+        wpiStates.add(bottomRightCorner);
+        wpiStates.add(bottomLeftCorner);
+        wpiStates.add(topLeftcorner);
+
+        return new Trajectory(wpiStates);
     }
 
     Note targetNote;

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

@@ -13,16 +13,17 @@ public enum NoteAvailability {
     }
 
     public enum KeyNoteNames{
-        SpikeTop,
-        SpikeMiddle,
-        SpikeBottom,
+        BlueSpikeTop,
+        BlueSpikeMiddle,
+        BlueSpikeBottom,
+        RedSpikeTop,
+        RedSpikeMiddle,
+        RedSpikeBottom,
         CenterLine1,
         CenterLine2,
         CenterLine3,
         CenterLine4,
-        CenterLine5,
-        BlueSource,
-        RedSource
+        CenterLine5
     }
 
     private int priority;

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

@@ -1,9 +1,8 @@
 package competition.subsystems.oracle;
 
 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 xbot.common.subsystems.pose.BasePoseSubsystem;
 
 import javax.inject.Singleton;
 import java.util.Arrays;
@@ -19,14 +18,17 @@ public NoteMap() {
     }
 
     private void initializeNotes() {
-        addNote(Note.KeyNoteNames.SpikeTop,    new Note(PoseSubsystem.SpikeTop));
-        addNote(Note.KeyNoteNames.SpikeMiddle, new Note(PoseSubsystem.SpikeMiddle));
-        addNote(Note.KeyNoteNames.SpikeBottom, new Note(PoseSubsystem.SpikeBottom));
+        addNote(Note.KeyNoteNames.BlueSpikeTop,    new Note(PoseSubsystem.SpikeTop));
+        addNote(Note.KeyNoteNames.BlueSpikeMiddle, new Note(PoseSubsystem.SpikeMiddle));
+        addNote(Note.KeyNoteNames.BlueSpikeBottom, new Note(PoseSubsystem.SpikeBottom));
         addNote(Note.KeyNoteNames.CenterLine1, new Note(PoseSubsystem.CenterLine1));
         addNote(Note.KeyNoteNames.CenterLine2, new Note(PoseSubsystem.CenterLine2));
         addNote(Note.KeyNoteNames.CenterLine3, new Note(PoseSubsystem.CenterLine3));
         addNote(Note.KeyNoteNames.CenterLine4, new Note(PoseSubsystem.CenterLine4));
         addNote(Note.KeyNoteNames.CenterLine5, new Note(PoseSubsystem.CenterLine5));
+        addNote(Note.KeyNoteNames.RedSpikeTop,    new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeTop)));
+        addNote(Note.KeyNoteNames.RedSpikeMiddle, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeMiddle)));
+        addNote(Note.KeyNoteNames.RedSpikeBottom, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeBottom)));
     }
 
     public void addNote(String key, Note note) {

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

@@ -110,7 +110,7 @@ public void execute() {
             setNewInstruction();
         }
 
-        Twist2d powers = logic.calculatePowers(pose.getCurrentPose2d(), drive.getPositionalPid(), headingModule);
+        Twist2d powers = logic.calculatePowers(pose.getCurrentPose2d(), drive.getPositionalPid(), headingModule, drive.getMaxTargetSpeedMetersPerSecond());
 
         aKitLog.record("Powers", powers);
 

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -62,6 +62,16 @@ public class PoseSubsystem extends BasePoseSubsystem {
     public static Pose2d CenterLine4 = new Pose2d(8.2956, 2.4292, new Rotation2d());
     public static Pose2d CenterLine5 = new Pose2d(8.2956, 0.7528, new Rotation2d());
 
+    public static double closeColumnWidth = 0.254;
+    public static double farColumnWidths = 0.3469;
+    public static Translation2d BlueLeftStageColumn = new Translation2d(3.34, 4.122);
+    public static Translation2d BlueTopStageColumn = new Translation2d(5.58, 5.42);
+    public static Translation2d BlueBottomStageColumn = new Translation2d(5.58,  2.82);
+
+    public static double SubwooferWidth = 0.95;
+    public static double SubwooferHeight = 1.1;
+    public static Translation2d BlueSubwoofer = new Translation2d(0.415, 5.57);
+
     private DoubleProperty matchTime;
 
     @Inject

src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java

@@ -21,7 +21,7 @@ public class ScoocherSubsystem extends BaseSubsystem implements DataFrameRefresh
     public ScoocherSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory,
                              ElectricalContract electricalContract, XDigitalInput.XDigitalInputFactory xDigitalInputFactory) {
         this.contract = electricalContract;
-        this.scoocherMotor = sparkMaxFactory.create(contract.getScoocherMotor(), getPrefix(), "ScoocherMotor", null);
+        this.scoocherMotor = sparkMaxFactory.createWithoutProperties(contract.getScoocherMotor(), getPrefix(), "Scoocher");
 
         pf.setPrefix(this);
         sendingPower = pf.createPersistentProperty("sendingPower", 0.1);