diff --git a/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json new file mode 100644 index 00000000..53eb4e72 --- /dev/null +++ b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json @@ -0,0 +1,585 @@ +{ + "tags" : [ + { + "ID" : 1, + "pose" : { + "translation" : { + "x" : 12.050434112548828, + "y" : 6.9188642501831055, + "z" : 1.1512539386749268 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0319787934422493, + "X" : -0.0173358041793108, + "Y" : 0.0013840891188010573, + "Z" : 0.9993372559547424 + } + } + } + }, + { + "ID" : 2, + "pose" : { + "translation" : { + "x" : 11.923489570617676, + "y" : 4.128251552581787, + "z" : 1.3825604915618896 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7207949161529541, + "X" : -0.015484735369682312, + "Y" : -0.0014652509707957506, + "Z" : 0.6929739117622375 + } + } + } + }, + { + "ID" : 3, + "pose" : { + "translation" : { + "x" : 11.331823348999023, + "y" : 3.782963275909424, + "z" : 1.3673444986343384 + }, + "rotation" : { + "quaternion" : { + "W" : 0.01351415179669857, + "X" : -0.021491853520274162, + "Y" : 0.004888125695288181, + "Z" : 0.9996656775474548 + } + } + } + }, + { + "ID" : 4, + "pose" : { + "translation" : { + "x" : 11.300000190734863, + "y" : 3.435410976409912, + "z" : 1.371805191040039 + }, + "rotation" : { + "quaternion" : { + "W" : 0.02158663608133793, + "X" : -0.022888774052262306, + "Y" : 0.004296557977795601, + "Z" : 0.9994957447052002 + } + } + } + }, + { + "ID" : 5, + "pose" : { + "translation" : { + "x" : 11.903764724731445, + "y" : 2.7956435680389404, + "z" : 1.383049488067627 + }, + "rotation" : { + "quaternion" : { + "W" : 0.6885595917701721, + "X" : 0.016876405104994774, + "Y" : -0.004065195098519325, + "Z" : -0.7249720096588135 + } + } + } + }, + { + "ID" : 6, + "pose" : { + "translation" : { + "x" : 11.706083297729492, + "y" : 0.14464473724365234, + "z" : 1.157674789428711 + }, + "rotation" : { + "quaternion" : { + "W" : 0.025160714983940125, + "X" : -0.019202761352062225, + "Y" : 0.002425266895443201, + "Z" : 0.9994960427284241 + } + } + } + }, + { + "ID" : 7, + "pose" : { + "translation" : { + "x" : 11.779255867004395, + "y" : 0.1305246353149414, + "z" : 1.1589089632034302 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9994612336158752, + "X" : -0.0020718644373118877, + "Y" : -0.01980898156762123, + "Z" : -0.02608805149793625 + } + } + } + }, + { + "ID" : 8, + "pose" : { + "translation" : { + "x" : 12.259369850158691, + "y" : 2.77305269241333, + "z" : 1.3927721977233887 + }, + "rotation" : { + "quaternion" : { + "W" : 0.6856842041015625, + "X" : 0.019558781757950783, + "Y" : -0.001896911533549428, + "Z" : -0.7276337742805481 + } + } + } + }, + { + "ID" : 9, + "pose" : { + "translation" : { + "x" : 12.518342018127441, + "y" : 3.0062994956970215, + "z" : 1.4028586149215698 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9991946220397949, + "X" : 0.0011942394776269794, + "Y" : -0.01824851520359516, + "Z" : -0.03571748360991478 + } + } + } + }, + { + "ID" : 10, + "pose" : { + "translation" : { + "x" : 12.526363372802734, + "y" : 3.5018177032470703, + "z" : 1.410685658454895 + }, + "rotation" : { + "quaternion" : { + "W" : 0.999525785446167, + "X" : -0.0003090753161814064, + "Y" : -0.007112081162631512, + "Z" : -0.0299605093896389 + } + } + } + }, + { + "ID" : 11, + "pose" : { + "translation" : { + "x" : 12.401211738586426, + "y" : 4.053260326385498, + "z" : 1.4090445041656494 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7207784652709961, + "X" : -0.015011371113359928, + "Y" : -0.010919488966464996, + "Z" : 0.6929170489311218 + } + } + } + }, + { + "ID" : 12, + "pose" : { + "translation" : { + "x" : 12.1367826461792, + "y" : 6.854067802429199, + "z" : 1.1314808130264282 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9998978972434998, + "X" : 0.0032891076989471912, + "Y" : 0.0008480203105136752, + "Z" : -0.013879206962883472 + } + } + } + }, + { + "ID" : 13, + "pose" : { + "translation" : { + "x" : 16.69558334350586, + "y" : 6.610563278198242, + "z" : 0.9037705659866333 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0234692245721817, + "X" : -0.002002944238483906, + "Y" : -0.0003050622472073883, + "Z" : 0.9997224807739258 + } + } + } + }, + { + "ID" : 14, + "pose" : { + "translation" : { + "x" : 16.67595863342285, + "y" : 6.18171501159668, + "z" : 0.9039492607116699 + }, + "rotation" : { + "quaternion" : { + "W" : 0.010960479266941547, + "X" : 0.0005166280898265541, + "Y" : -0.002506508957594633, + "Z" : 0.999936580657959 + } + } + } + }, + { + "ID" : 15, + "pose" : { + "translation" : { + "x" : 16.564498901367188, + "y" : 3.5477375984191895, + "z" : 0.9101831316947937 + }, + "rotation" : { + "quaternion" : { + "W" : 0.029302222654223442, + "X" : 0.003682132810354233, + "Y" : 0.0026310565881431103, + "Z" : 0.9995603561401367 + } + } + } + }, + { + "ID" : 16, + "pose" : { + "translation" : { + "x" : 16.539405822753906, + "y" : 3.1178951263427734, + "z" : 0.9153234958648682 + }, + "rotation" : { + "quaternion" : { + "W" : 0.007331775967031717, + "X" : -0.008358882740139961, + "Y" : 0.00232889992184937, + "Z" : 0.9999354481697083 + } + } + } + }, + { + "ID" : 17, + "pose" : { + "translation" : { + "x" : 4.522510051727295, + "y" : 0.4364204406738281, + "z" : 0.9622411131858826 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9995527267456055, + "X" : -0.0019414271228015423, + "Y" : -0.010864048264920712, + "Z" : -0.027793319895863533 + } + } + } + }, + { + "ID" : 18, + "pose" : { + "translation" : { + "x" : 4.58988618850708, + "y" : 3.195493221282959, + "z" : 1.1940523386001587 + }, + "rotation" : { + "quaternion" : { + "W" : 0.691650390625, + "X" : 0.01303744874894619, + "Y" : -0.00376962311565876, + "Z" : -0.7221049666404724 + } + } + } + }, + { + "ID" : 19, + "pose" : { + "translation" : { + "x" : 5.226935386657715, + "y" : 3.45327091217041, + "z" : 1.2011194229125977 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9997949600219727, + "X" : 0.0005262811318971217, + "Y" : -0.010933312587440014, + "Z" : -0.01703987829387188 + } + } + } + }, + { + "ID" : 20, + "pose" : { + "translation" : { + "x" : 5.234817981719971, + "y" : 3.8026344776153564, + "z" : 1.2003945112228394 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9998764991760254, + "X" : 0.00011397890921216458, + "Y" : -0.008693280629813671, + "Z" : -0.013094102963805199 + } + } + } + }, + { + "ID" : 21, + "pose" : { + "translation" : { + "x" : 4.664792537689209, + "y" : 4.426998615264893, + "z" : 1.1946265697479248 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7123200297355652, + "X" : -0.002563168527558446, + "Y" : -0.011971132829785347, + "Z" : 0.7017481327056885 + } + } + } + }, + { + "ID" : 22, + "pose" : { + "translation" : { + "x" : 4.825745105743408, + "y" : 7.208614349365234, + "z" : 0.9605111479759216 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9997665882110596, + "X" : -0.0008584200986661017, + "Y" : -0.021474771201610565, + "Z" : -0.002250437159091234 + } + } + } + }, + { + "ID" : 23, + "pose" : { + "translation" : { + "x" : 4.734621047973633, + "y" : 7.202859878540039, + "z" : 0.9770094156265259 + }, + "rotation" : { + "quaternion" : { + "W" : 0.034675028175115585, + "X" : -0.018650785088539124, + "Y" : 0.0029087455477565527, + "Z" : 0.9992203116416931 + } + } + } + }, + { + "ID" : 24, + "pose" : { + "translation" : { + "x" : 4.310214519500732, + "y" : 4.4429521560668945, + "z" : 1.1934773921966553 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7160341739654541, + "X" : -0.011152027174830437, + "Y" : -0.0041417754255235195, + "Z" : 0.6979638934135437 + } + } + } + }, + { + "ID" : 25, + "pose" : { + "translation" : { + "x" : 4.0436787605285645, + "y" : 4.20996618270874, + "z" : 1.1983577013015747 + }, + "rotation" : { + "quaternion" : { + "W" : 0.01692536473274231, + "X" : -0.02695719338953495, + "Y" : -0.0008292849524877965, + "Z" : 0.9994929432868958 + } + } + } + }, + { + "ID" : 26, + "pose" : { + "translation" : { + "x" : 4.030979633331299, + "y" : 3.853869915008545, + "z" : 1.1997509002685547 + }, + "rotation" : { + "quaternion" : { + "W" : 0.017164066433906555, + "X" : -0.031823523342609406, + "Y" : 0.001335379434749484, + "Z" : 0.9993451833724976 + } + } + } + }, + { + "ID" : 27, + "pose" : { + "translation" : { + "x" : 4.239171028137207, + "y" : 3.2177231311798096, + "z" : 1.1884853839874268 + }, + "rotation" : { + "quaternion" : { + "W" : 0.6884703040122986, + "X" : 0.014258901588618755, + "Y" : 0.0001352958206553012, + "Z" : -0.7251242399215698 + } + } + } + }, + { + "ID" : 28, + "pose" : { + "translation" : { + "x" : 4.380957126617432, + "y" : 0.46189260482788086, + "z" : 0.9888477325439453 + }, + "rotation" : { + "quaternion" : { + "W" : 0.018142135813832283, + "X" : -0.009404327720403671, + "Y" : 0.0032862841617316008, + "Z" : 0.9997857809066772 + } + } + } + }, + { + "ID" : 29, + "pose" : { + "translation" : { + "x" : -0.17474985122680664, + "y" : 0.7096297740936279, + "z" : 0.5477728247642517 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9998412728309631, + "X" : -0.00282590021379292, + "Y" : -0.006864454131573439, + "Z" : -0.01619541272521019 + } + } + } + }, + { + "ID" : 30, + "pose" : { + "translation" : { + "x" : -0.15256865322589874, + "y" : 1.1327531337738037, + "z" : 0.548723578453064 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9996415376663208, + "X" : -0.002256404608488083, + "Y" : -0.012892674654722214, + "Z" : -0.0233586598187685 + } + } + } + }, + { + "ID" : 31, + "pose" : { + "translation" : { + "x" : -0.006732862442731857, + "y" : 3.744476795196533, + "z" : 0.5510860085487366 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999865889549255, + "X" : -0.001071035978384316, + "Y" : -0.0008199909934774041, + "Z" : -0.004985099658370018 + } + } + } + }, + { + "ID" : 32, + "pose" : { + "translation" : { + "x" : 0.008077199570834637, + "y" : 4.1775126457214355, + "z" : 0.5524500012397766 + }, + "rotation" : { + "quaternion" : { + "W" : 1.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.0 + } + } + } + } + ], + "field" : { + "length" : 16.541, + "width" : 8.069 + }, + "_comment" : "generated by PractiCal 1.0.5 (3) on 3/13/2026" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f5c3cfde..ab411dfa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -144,7 +144,7 @@ public enum RobotEdition { * This is for when we're testing shot and extension numbers and should be FALSE once bring up is * complete */ - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; public boolean hasZeroedSinceStartup = false; @@ -925,6 +925,8 @@ public void autonomousExit() { @Override public void teleopInit() { + Superstructure.matchStartTime = Timer.getFPGATimestamp(); + intake.slapdownInit(); } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index fedf75e7..80efa4c2 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -86,16 +86,40 @@ public enum FixedShotTarget { @AutoLogOutput(key = "Superstructure/State") private static SuperState state = SuperState.IDLE; - @AutoLogOutput(key = "Scoring/Scoring Active") - public boolean isScoringActive = - isOurShift(); // assuming we want the dashboard to show if the time allows us to score not if - - // its litterly possible - private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); + private double getFPGATimestamp() { + return Timer.getFPGATimestamp(); + } + + @AutoLogOutput(key = "Superstructure/match starttime") + public static double matchStartTime; + + private double getTimeElapsed() { + return getFPGATimestamp() - matchStartTime; + } + + private double timeLeftinMatch() { + return 140.00 - getTimeElapsed(); + } + + @AutoLogOutput(key = "Superstructure/Shift Timer") + private double getTimeStampLeftInShift() { + return getTimeLeftInShift(); + } + + @AutoLogOutput(key = "Superstructure/Current Shift") + private String getCurrentShiftName() { + return getCurrentShift(); + } + + @AutoLogOutput(key = "Scoring/Scoring Active") + public boolean isScoringActive() { + return isOurShift(); + } + private final SwerveSubsystem swerve; private final Indexer indexer; private final Intake intake; @@ -270,7 +294,7 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_SCORE, SuperState.SCORE, readyTrigger); - bindTransition(SuperState.SCORE, SuperState.SPIN_UP_SCORE, readyTrigger.negate()); + // bindTransition(SuperState.SCORE, SuperState.SPIN_UP_SCORE, readyTrigger.negate()); bindTransition(SuperState.SPIN_UP_SCORE, SuperState.IDLE, shootReq.negate()); @@ -758,28 +782,41 @@ private Alliance getStartingAlliance() { } } - private int getCurrentShift() { - double timeLeftinMatch = Timer.getMatchTime(); - // may be a nicer way to do this - if (105.00 <= timeLeftinMatch && timeLeftinMatch <= 130.00) { - return 1; - } else if (80.00 <= timeLeftinMatch && timeLeftinMatch <= 105.00) { - return 2; - } else if ((55.00 <= timeLeftinMatch && timeLeftinMatch <= 80.00)) { - return 3; - } else if ((30.00 <= timeLeftinMatch && timeLeftinMatch <= 55.00)) { - return 4; + private String getCurrentShift() { + if (130.00 < timeLeftinMatch() && timeLeftinMatch() <= 140.00) { + return "Transition"; + } else if (105.00 < timeLeftinMatch() && timeLeftinMatch() <= 130.00) { + return "Shift 1"; + } else if (80.00 < timeLeftinMatch() && timeLeftinMatch() <= 105.00) { + return "Shift 2"; + } else if ((55.00 < timeLeftinMatch() && timeLeftinMatch() <= 80.00)) { + return "Shift 3"; + } else if ((30.00 < timeLeftinMatch() && timeLeftinMatch() <= 55.00)) { + return "Shift 4"; } else { - return 0; + return "End Game"; } } + private double getTimeLeftInShift() { + double offset = + switch (getCurrentShift()) { + case "Transition" -> 130.00; + case "Shift 1" -> 105.00; + case "Shift 2" -> 80.00; + case "Shift 3" -> 55.00; + case "Shift 4" -> 30.00; + default -> 0.00; + }; + return timeLeftinMatch() - offset; + } + public boolean isOurShift() { // only cant score when its the others turn, otherwise everyone can if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) { - return !(getCurrentShift() == 2 || getCurrentShift() == 4); + return !(getCurrentShift() == "Shift 2" || getCurrentShift() == "Shift 4"); } else { - return !(getCurrentShift() == 1 || getCurrentShift() == 3); + return !(getCurrentShift() == "Shift 1" || getCurrentShift() == "Shift 3"); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index e854fd6a..5ca16bde 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.swerve.constants; +import java.io.File; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -12,6 +14,7 @@ import edu.wpi.first.units.measure.Mass; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Filesystem; import frc.robot.components.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.module.Module.ModuleConstants; @@ -24,19 +27,19 @@ public abstract class SwerveConstants { protected AprilTagFieldLayout fieldTags; public SwerveConstants() { - // try { - // fieldTags = - // new AprilTagFieldLayout( - // Filesystem.getDeployDirectory() - // .toPath() - // .resolve("tagmaps" + File.separator + "2026-rebuilt-welded.json")); - // System.out.println("Successfully loaded tag map"); - // } catch (Exception e) { - // System.err.println("Failed to load custom tag map"); - // tagLoadFailureAlert.set(true); - // fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - // } - fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + try { + fieldTags = + new AprilTagFieldLayout( + Filesystem.getDeployDirectory() + .toPath() + .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json")); + System.out.println("Successfully loaded tag map"); + } catch (Exception e) { + System.err.println("Failed to load custom tag map"); + tagLoadFailureAlert.set(true); + fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + } + // fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); } public AprilTagFieldLayout getFieldTagLayout() { diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 6a12c504..103975b1 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -19,7 +19,8 @@ public class AutoAim { private static boolean outOfRange = false; // TODO not sure if this should be true by default public static double LATENCY_COMPENSATION_SECS = - new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp + // new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp + 0.3; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree();