From a5ef2c117c79ef7428b93f2c37eb1ac997647673 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Sat, 27 Jan 2024 20:02:39 -0800 Subject: [PATCH 01/10] Small bugfixes for pathing --- SeriouslyCommonLib | 2 +- src/main/java/competition/Robot.java | 100 ++++++++++++++++-- .../subsystems/oracle/DynamicOracle.java | 41 ++++++- .../schoocher/ScoocherSubsystem.java | 2 +- 4 files changed, 132 insertions(+), 13 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index ad70313f..e110f0d4 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit ad70313fff96e1b8edd1c1080dbcb4343a5f7da1 +Subproject commit e110f0d4e2930b25a7e6b2c30927f24b46170104 diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 8297f3f4..4f0098a7 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -8,17 +8,19 @@ 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.subsystems.pose.BasePoseSubsystem; +import java.util.LinkedList; +import java.util.Queue; + public class Robot extends BaseRobot { @Override @@ -75,6 +77,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() { @@ -96,17 +99,102 @@ 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); } + + public class MovingAverage { + private static final int SIZE = 15; + private Queue queue; + private T sum; + private SumFunction sumFunction; + + public MovingAverage(SumFunction sumFunction, T initialValue) { + this.queue = new LinkedList<>(); + this.sum = initialValue; + this.sumFunction = sumFunction; + } + + public void add(T value) { + sum = sumFunction.add(sum, value); + queue.add(value); + if (queue.size() > SIZE) { + sum = sumFunction.subtract(sum, queue.remove()); + } + } + + public T getAverage() { + if (queue.isEmpty()) { + return sum; + } + return sumFunction.divide(sum, queue.size()); + } + + public interface SumFunction { + T add(T a, T b); + T subtract(T a, T b); + T divide(T a, int b); + } + } + + MovingAverage translationAverageCalculator = new MovingAverage<>( + new MovingAverage.SumFunction() { + @Override + public Translation2d add(Translation2d a, Translation2d b) { + return a.plus(b); + } + + @Override + public Translation2d subtract(Translation2d a, Translation2d b) { + return a.minus(b); + } + + @Override + public Translation2d divide(Translation2d a, int b) { + return new Translation2d(a.getX() / b, a.getY() / b); + } + }, + new Translation2d() + ); + + MovingAverage rotationAverageCalculator = new MovingAverage<>( + new MovingAverage.SumFunction() { + @Override + public Double add(Double a, Double b) { + return a + b; + } + + @Override + public Double subtract(Double a, Double b) { + return a - b; + } + + @Override + public Double divide(Double a, int b) { + return a / b; + } + }, + 0.0 + ); } diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 0071fd02..7fe5fd21 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -4,6 +4,7 @@ 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 org.littletonrobotics.junction.Logger; import xbot.common.command.BaseSubsystem; import xbot.common.trajectory.LowResField; @@ -12,6 +13,7 @@ import javax.inject.Inject; import javax.inject.Singleton; +import java.util.ArrayList; @Singleton public class DynamicOracle extends BaseSubsystem { @@ -82,17 +84,21 @@ private void createRobotObstacle(Translation2d location, double sideLength, Stri private void 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)); + field.addObstacle(new Obstacle(x, y, width+robotWidth, height+robotWidth, name)); } 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); + + createObstacleWithRobotWidth(3.34, 4.122, 0.254,0.254, .914, "BlueLeftStageColumn", field); + createObstacleWithRobotWidth(5.58, 5.42, 0.3469,0.3469, .914, "BlueTopStageColumn", field); + createObstacleWithRobotWidth(5.58, 2.82,0.3469, 0.3469, .914, "BlueBottomStageColumn", field); return field; } @@ -186,6 +192,31 @@ 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 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(bottomLeftCorner); + wpiStates.add(bottomRightCorner); + + return new Trajectory(wpiStates); } Note targetNote; diff --git a/src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java b/src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java index 47894774..534741ea 100644 --- a/src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java +++ b/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); From 92e2cc9653a39f07f54a983d10ae8d0efe692722 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Sat, 27 Jan 2024 23:42:47 -0800 Subject: [PATCH 02/10] Let the robot "catch up" to the ghost --- .../java/competition/operator_interface/OperatorCommandMap.java | 2 +- .../subsystems/oracle/SwerveAccordingToOracleCommand.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index efe71089..af90f42b 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/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); diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 20c49b2b..e3630b17 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/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); From 9cd18285b4f0181d1575985195bd029c7135f022 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Sun, 28 Jan 2024 14:35:39 -0800 Subject: [PATCH 03/10] Add speaker and ways to visualize collision boxes --- .../subsystems/oracle/DynamicOracle.java | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 7fe5fd21..a7db3e3c 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -64,27 +64,31 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri 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(double x, double y, double width, double height, double robotWidth, String name, LowResField field) { - field.addObstacle(new Obstacle(x, y, width+robotWidth, height+robotWidth, name)); + var obstacle = new Obstacle(x, y, width+robotWidth, height+robotWidth, name); + field.addObstacle(obstacle); + return obstacle; } private LowResField setupLowResField() { @@ -99,6 +103,11 @@ private LowResField setupLowResField() { createObstacleWithRobotWidth(3.34, 4.122, 0.254,0.254, .914, "BlueLeftStageColumn", field); createObstacleWithRobotWidth(5.58, 5.42, 0.3469,0.3469, .914, "BlueTopStageColumn", field); createObstacleWithRobotWidth(5.58, 2.82,0.3469, 0.3469, .914, "BlueBottomStageColumn", field); + + var speaker = createObstacleWithRobotWidth(0.415, 5.57, 0.95, 1.1, 0, "BlueSpeaker", field); + speaker.defaultBottomLeft = false; + speaker.defaultTopLeft = false; + return field; } @@ -213,8 +222,9 @@ private Trajectory obstacleToTrajectory(Obstacle o) { wpiStates.add(topLeftcorner); wpiStates.add(topRightCorner); - wpiStates.add(bottomLeftCorner); wpiStates.add(bottomRightCorner); + wpiStates.add(bottomLeftCorner); + wpiStates.add(topLeftcorner); return new Trajectory(wpiStates); } From 0545ce8a4261fe9f103e6070257447feb5f08e6f Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Sun, 28 Jan 2024 15:32:10 -0800 Subject: [PATCH 04/10] Use newer commonlib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index e110f0d4..71e9bdf9 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit e110f0d4e2930b25a7e6b2c30927f24b46170104 +Subproject commit 71e9bdf9d4cfccc7f7a7af3b2521f02c4b64a858 From f0c416994226bed634ce8d3f8ab587e473484e2b Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Sun, 28 Jan 2024 16:21:59 -0800 Subject: [PATCH 05/10] Updates to Oracle --- .../subsystems/oracle/DynamicOracle.java | 40 +++++++++++++++---- .../competition/subsystems/oracle/Note.java | 13 +++--- .../subsystems/oracle/NoteMap.java | 12 +++--- .../subsystems/pose/PoseSubsystem.java | 10 +++++ 4 files changed, 56 insertions(+), 19 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index a7db3e3c..4f4b8824 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -5,11 +5,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; -import org.littletonrobotics.junction.Logger; 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; @@ -56,8 +55,8 @@ 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)); @@ -84,6 +83,13 @@ private void createRobotObstacle(Translation2d location, double sideLength, Stri field.addObstacle(new Obstacle(location.getX(), location.getY(), sideLength, sideLength, name)); } + 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) { var obstacle = new Obstacle(x, y, width+robotWidth, height+robotWidth, name); @@ -100,11 +106,29 @@ private LowResField setupLowResField() { //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(3.34, 4.122, 0.254,0.254, .914, "BlueLeftStageColumn", field); - createObstacleWithRobotWidth(5.58, 5.42, 0.3469,0.3469, .914, "BlueTopStageColumn", field); - createObstacleWithRobotWidth(5.58, 2.82,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); - var speaker = createObstacleWithRobotWidth(0.415, 5.57, 0.95, 1.1, 0, "BlueSpeaker", field); + speaker = createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), + PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "RedSubwoofer", field); speaker.defaultBottomLeft = false; speaker.defaultTopLeft = false; diff --git a/src/main/java/competition/subsystems/oracle/Note.java b/src/main/java/competition/subsystems/oracle/Note.java index ffa36d63..134851fc 100644 --- a/src/main/java/competition/subsystems/oracle/Note.java +++ b/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; diff --git a/src/main/java/competition/subsystems/oracle/NoteMap.java b/src/main/java/competition/subsystems/oracle/NoteMap.java index d54e9ff3..fc1d8f3a 100644 --- a/src/main/java/competition/subsystems/oracle/NoteMap.java +++ b/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) { diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index d440852f..f58a5676 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/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 From c4c55d5bd13ede85c0c5d13e587b465de8a4fdab Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Mon, 29 Jan 2024 18:36:52 -0800 Subject: [PATCH 06/10] use newer commonlib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 71e9bdf9..ff3cf0bb 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 71e9bdf9d4cfccc7f7a7af3b2521f02c4b64a858 +Subproject commit ff3cf0bbf4eacc70975feecb82b0fb69b54e8ea6 From bea7c426b2d84c6764780539a2a002f54c49c6a7 Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Mon, 29 Jan 2024 18:46:35 -0800 Subject: [PATCH 07/10] Use newer commonlib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index ff3cf0bb..2fdadcf4 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit ff3cf0bbf4eacc70975feecb82b0fb69b54e8ea6 +Subproject commit 2fdadcf44c16416aa9080c99c9bfd3922d72710f From 60d2a0c19661309f55660453d9e9978f76ec9abb Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Tue, 30 Jan 2024 19:15:02 -0800 Subject: [PATCH 08/10] Use newer SCL --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 2fdadcf4..3e5005a2 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 2fdadcf44c16416aa9080c99c9bfd3922d72710f +Subproject commit 3e5005a27c4cb9490dcd445f7323aa2604c015d2 From f67e094997b3907576fa48b54f466b6d9d0adece Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Tue, 30 Jan 2024 20:13:13 -0800 Subject: [PATCH 09/10] Use newer SCL --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 3e5005a2..75e79a36 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 3e5005a27c4cb9490dcd445f7323aa2604c015d2 +Subproject commit 75e79a36aa59981baabe1bc5af30c4f93e932639 From ec34ba8395718ea5bb39de864f71a29a9e2191ce Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Tue, 30 Jan 2024 20:54:00 -0800 Subject: [PATCH 10/10] Use newer commonlib, which has moving average code --- SeriouslyCommonLib | 2 +- src/main/java/competition/Robot.java | 85 ++++------------------------ 2 files changed, 12 insertions(+), 75 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 75e79a36..1db4a346 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 75e79a36aa59981baabe1bc5af30c4f93e932639 +Subproject commit 1db4a34670e8291b34f1eabdb4c39c6a922e2af4 diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 4f0098a7..a91e0e5c 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -16,6 +16,9 @@ import edu.wpi.first.wpilibj.simulation.DriverStationSim; import xbot.common.command.BaseRobot; import xbot.common.math.FieldPose; +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; @@ -23,6 +26,9 @@ public class Robot extends BaseRobot { + public Robot() { + } + @Override protected void initializeSystems() { super.initializeSystems(); @@ -91,6 +97,11 @@ private FieldPose getFieldOrigin() { ); } + MovingAverageForTranslation2d translationAverageCalculator = + new MovingAverageForTranslation2d(15); + MovingAverageForDouble rotationAverageCalculator = + new MovingAverageForDouble(15); + @Override public void simulationPeriodic() { super.simulationPeriodic(); @@ -123,78 +134,4 @@ public void simulationPeriodic() { currentPose.getRotation().plus(Rotation2d.fromDegrees(currentRotationAverage * headingAdjustmentFactorForSimulation))); pose.setCurrentPoseInMeters(updatedPose); } - - public class MovingAverage { - private static final int SIZE = 15; - private Queue queue; - private T sum; - private SumFunction sumFunction; - - public MovingAverage(SumFunction sumFunction, T initialValue) { - this.queue = new LinkedList<>(); - this.sum = initialValue; - this.sumFunction = sumFunction; - } - - public void add(T value) { - sum = sumFunction.add(sum, value); - queue.add(value); - if (queue.size() > SIZE) { - sum = sumFunction.subtract(sum, queue.remove()); - } - } - - public T getAverage() { - if (queue.isEmpty()) { - return sum; - } - return sumFunction.divide(sum, queue.size()); - } - - public interface SumFunction { - T add(T a, T b); - T subtract(T a, T b); - T divide(T a, int b); - } - } - - MovingAverage translationAverageCalculator = new MovingAverage<>( - new MovingAverage.SumFunction() { - @Override - public Translation2d add(Translation2d a, Translation2d b) { - return a.plus(b); - } - - @Override - public Translation2d subtract(Translation2d a, Translation2d b) { - return a.minus(b); - } - - @Override - public Translation2d divide(Translation2d a, int b) { - return new Translation2d(a.getX() / b, a.getY() / b); - } - }, - new Translation2d() - ); - - MovingAverage rotationAverageCalculator = new MovingAverage<>( - new MovingAverage.SumFunction() { - @Override - public Double add(Double a, Double b) { - return a + b; - } - - @Override - public Double subtract(Double a, Double b) { - return a - b; - } - - @Override - public Double divide(Double a, int b) { - return a / b; - } - }, - 0.0 - ); }