Last change of 2024 Offseason

Author: Blue-Flag-666

InertialNavigation.java

@@ -11,7 +11,9 @@
  */
 public abstract class Zwerve extends Zubsystem implements ZmartDash {
     private static final double POS_FIX_KP = 0.2;
-    private static final double ROTATION_OUTPUT = 2;
+    private static final double ROTATION_OUTPUT = 1;
+    private static final double ROTATION_OUTPUT_SLOW = 0.5;
+    private static final double ROTATION_OUTPUT_FAST = 2;
     public final Vec2D shape;
     /**
      * Gyro.
@@ -136,7 +138,7 @@ public Zwerve go(Vec2D vel, double rot) {
             this.desired_yaw = curr_yaw;
         }
 
-        if (this.last_rot.get() < 0.5) {
+        if (this.last_rot.get() < 0.2) {
             this.desired_yaw = curr_yaw;
         }
 
@@ -145,7 +147,7 @@ public Zwerve go(Vec2D vel, double rot) {
             rot += error * POS_FIX_KP;
         }
 
-        this.prev.add(vel);
+        this.prev.add(vel.rot(Math.PI));
         var sum = new Vec2D(0, 0);
         for (final var i : this.prev) {
             sum = sum.add(i);
@@ -159,7 +161,7 @@ public Zwerve go(Vec2D vel, double rot) {
         final var w = this.shape.y / 2;
         Vec2D[] v = { new Vec2D(l, w), new Vec2D(-l, w), new Vec2D(-l, -w), new Vec2D(l, -w) };
         for (var i = 0; i < 4; i++) {
-            v[i] = v[i].rot(Math.PI / 2).with_r(rot*ROTATION_OUTPUT).add(vt);
+            v[i] = v[i].rot(Math.PI / 2).with_r(rot * ROTATION_OUTPUT).add(vt);
         }
         for (int i = 0; i < 4; i++) {
             this.module[i].go(v[i].mul(this.output));
@@ -196,10 +198,10 @@ public Zwerve toggle_headless() {
         return this;
     }
 
-    public ZCommand drive(Axis x, Axis y, Axis rot, boolean useLimelight, double limelightForwardSpeed, double limelightRotateSpeed) {
+    public ZCommand drive(Axis x, Axis y, Axis rot) {
         return new Zambda(this, () -> {
-            final var vel = new Vec2D(useLimelight ? limelightForwardSpeed : x.get(), y.get());
-            this.go(vel, useLimelight ? limelightRotateSpeed : rot.get());
+            final var vel = new Vec2D(x.get(), y.get());
+            this.go(vel, rot.get());
         });
     }
 

Zwerve.java

@@ -36,7 +36,7 @@ public String key() {
         return "Limelight";
     }
 
-    // Copied from https://github.com/LimelightVision/limelight-examples/blob/main/java-wpilib/swerve-aim-and-range/src/main/java/frc/robot/LimelightHelpers.java
+    // Copied from https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java
     private static class LimelightHelpers {
         private static final Map<String, DoubleArrayEntry> doubleArrayEntries = new ConcurrentHashMap<>();
         /**

hardware/Limelight.java

@@ -1,16 +1,14 @@
 package frc.libzodiac.hardware;
 
 import com.ctre.phoenix6.hardware.Pigeon2;
-import frc.libzodiac.InertialNavigation.Gyro;
 import frc.libzodiac.Util;
 import frc.libzodiac.Zensor;
 import frc.libzodiac.ZmartDash;
 import frc.libzodiac.ui.Axis;
-import frc.libzodiac.util.Vec2D;
 
 import java.security.InvalidParameterException;
 
-public class Pigeon implements Gyro, Zensor, ZmartDash {
+public class Pigeon implements Zensor, ZmartDash {
     public final int can_id;
     protected Pigeon2 pigeon = null;
 
@@ -55,14 +53,4 @@ public double get(String value) {
     public String key() {
         return "Pigeon(" + this.can_id + ")";
     }
-
-    @Override
-    public double getYaw() {
-        return this.get();
-    }
-
-    @Override
-    public Vec2D getAcceleration() {
-        return new Vec2D(this.pigeon.getAccelerationX().refresh().getValue()-this.pigeon.getGravityVectorX().refresh().getValue(), this.pigeon.getAccelerationY().refresh().getValue()-this.pigeon.getGravityVectorY().refresh().getValue());
-    }
 }