Merge remote-tracking branch 'origin/main' into FrcCompIntakeManual

lib/.classpath

@@ -16,5 +16,16 @@
 	<classpathentry kind="src" path="build/generated/sources/annotationProcessor/java/main"/>
 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-17/"/>
 	<classpathentry kind="con" path="org.eclipse.buildship.core.gradleclasspathcontainer"/>
+	<classpathentry kind="src" path="bin/generated-sources/annotations">
+		<attributes>
+			<attribute name="optional" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="src" output="bin/test" path="bin/generated-test-sources/annotations">
+		<attributes>
+			<attribute name="optional" value="true"/>
+			<attribute name="test" value="true"/>
+		</attributes>
+	</classpathentry>
 	<classpathentry kind="output" path="bin/default"/>
 </classpath>

lib/build.gradle

@@ -32,20 +32,36 @@ repositories {
 
 dependencies {
     // Use JUnit Jupiter for testing.
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    // testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    // testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
     // WPI Lib
-    implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
-    implementation "edu.wpi.first.cameraserver:cameraserver-java:$wpiVersion"
-    implementation "edu.wpi.first.ntcore:ntcore-java:$wpiVersion"
-    implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpiVersion"
-    implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpiVersion"
-    implementation "edu.wpi.first.wpimath:wpimath-java:$wpiVersion"
-    implementation "edu.wpi.first.wpiunits:wpiunits-java:$wpiVersion"
-    implementation "edu.wpi.first.hal:hal-java:$wpiVersion"
+    // implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
+    // implementation "edu.wpi.first.cameraserver:cameraserver-java:$wpiVersion"
+    // implementation "edu.wpi.first.ntcore:ntcore-java:$wpiVersion"
+    // implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpiVersion"
+    // implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpiVersion"
+    // implementation "edu.wpi.first.wpimath:wpimath-java:$wpiVersion"
+    // implementation "edu.wpi.first.wpiunits:wpiunits-java:$wpiVersion"
+    // implementation "edu.wpi.first.hal:hal-java:$wpiVersion"
+
     implementation "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:$wpiVersion"
 
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
     // CTRE Phoenix 6
     implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}"
 
@@ -58,7 +74,6 @@ dependencies {
 
     // AdvantageKit
     implementation "org.littletonrobotics.akit:akit-java:${advantageKitVersion}"
-
 }
 
 tasks.named('test') {

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java

@@ -7,6 +7,7 @@ import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -95,7 +96,7 @@ public class SwerveDriveSystem implements Subsystem{
         double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
 
         if(mLastSimTime != 0){
-            Pose3d oldPose = getRobotPose();
+            Pose3d oldPose = getRobotPose3d();
             poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond))))));
         }
 
@@ -159,7 +160,7 @@ public class SwerveDriveSystem implements Subsystem{
      * @param chassisSpeeds
      * @param centerOfRotation
      */
-    private void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
+    public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
         this.chassisSpeeds = chassisSpeeds;
         this.centerOfRotation = centerOfRotation;
     }
@@ -194,14 +195,42 @@ public class SwerveDriveSystem implements Subsystem{
         ), new Translation2d());
     }
 
+    public ChassisSpeeds getRobotRelativeSpeeds(){
+        return this.chassisSpeeds;
+    }
+
     /**
      * Get the current field location of the robot
      * @return The current pose as a Pose3d
      */
-    public Pose3d getRobotPose(){
+    public Pose3d getRobotPose3d(){
         return poseEstimator.getEstimatedPosition();
     }
 
+    /**
+     * Get the current field location of the robot
+     * @return The current pose as a Pose2d
+     */
+    public Pose2d getRobotPose2d(){
+        return poseEstimator.getEstimatedPosition().toPose2d();
+    }
+
+    /**
+     * Set pose estimation to a specific pose
+     * @param pose The new pose as a Pose3d
+     */
+    public void setPose3d(Pose3d pose){
+        poseEstimator.resetPose(pose);
+    }
+
+    /**
+     * Set pose estimation to a specific pose
+     * @param pose The new pose as a Pose2d
+     */
+    public void setPose2d(Pose2d pose){
+        this.setPose3d(new Pose3d(pose));
+    }
+
     private SwerveModulePosition[] getModulePositions(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new);
     }

lib/src/main/java/org/fairportrobotics/frc/robolib/simulator/apriltag/LimelightAprilTagSimulator.java

@@ -0,0 +1,43 @@
+package org.fairportrobotics.frc.robolib.simulator.apriltag;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTableValue;
+
+public class LimelightAprilTagSimulator {
+    
+    private String cameraName;
+    private NetworkTableInstance ntInstance;
+    private NetworkTable table;
+    private TrackingType trackType;
+
+
+    private int tv;
+    private double tx;
+    private double ty;
+
+
+    public LimelightAprilTagSimulator(String limelightName, TrackingType trackingType){
+        this.cameraName = limelightName;
+        ntInstance = NetworkTableInstance.getDefault();
+        table = ntInstance.getTable(limelightName);
+        this.trackType = trackingType;
+    }
+
+    public void simulationUpdate(){
+
+        
+
+
+        // Publish new vals
+        table.putValue("tv", NetworkTableValue.makeInteger(tv));
+        table.putValue("tx", NetworkTableValue.makeDouble(tx));
+        table.putValue("ty", NetworkTableValue.makeDouble(ty));
+    }
+
+    public enum TrackingType{
+        BOT_POSE,
+        STANDARD
+    }
+
+}