Lots of fixes

Author: Blue-Flag-666

InertialNavigation.java

@@ -3,12 +3,13 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.libzodiac.util.Vec2D;
 
-public class InertialNavigation extends Zubsystem {
+public class InertialNavigation extends Zubsystem implements ZmartDash {
     private final Gyro gyro;
 
     private final Timer timer = new Timer();
 
     private double zero = 0;
+    private Vec2D gravity = new Vec2D(0,0);
     private Vec2D pos = new Vec2D(0, 0);
     private Vec2D speed = new Vec2D(0, 0);
 
@@ -26,6 +27,7 @@ public InertialNavigation init() {
 
     public InertialNavigation setZero() {
         this.zero = this.gyro.getYaw();
+//        this.gravity = this.gyro.getAcceleration();
         return this;
     }
 
@@ -35,10 +37,13 @@ public InertialNavigation setZero(double zero) {
     }
 
     private Vec2D getAcceleration() {
-        return this.gyro.getAcceleration().rot(-this.zero);
+        return this.gyro.getAcceleration().sub(gravity).mul(10);//.rot(-this.zero);
     }
 
     public InertialNavigation update() {
+        this.debug("posinav", "" + this.getPosition());
+        this.debug("yawinav", this.getYaw());
+        this.debug("acc", "" + this.getAcceleration());
         final var loopTime = timer.get();
         timer.reset();
         this.speed = this.speed.add(this.getAcceleration().mul(loopTime));
@@ -59,6 +64,11 @@ public double getYaw() {
         return this.gyro.getYaw() - this.zero;
     }
 
+    @Override
+    public String key() {
+        return "inav";
+    }
+
     public interface Gyro {
         double getYaw();
 

Zwerve.java

@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.libzodiac.ui.Axis;
 import frc.libzodiac.util.Vec2D;
-import frc.robot.subsystems.Chassis;
 
 import java.util.ArrayDeque;
 
@@ -12,6 +11,7 @@
  */
 public abstract class Zwerve extends Zubsystem implements ZmartDash {
     private static final double POS_FIX_KP = 1;
+    private static final double ROTATION_OUTPUT = 1;
     public final Vec2D shape;
     /**
      * Gyro.
@@ -128,7 +128,6 @@ public Zwerve init() {
      * @param rot rotate velocity, CCW positive
      */
     public Zwerve go(Vec2D vel, double rot) {
-        this.debug("pos", ""+Chassis.inav.getPosition());
         // direction adjustment
         final var curr_yaw = this.yaw.get();
         this.debug("desired", this.desired_yaw);
@@ -160,7 +159,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).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));

hardware/Pigeon.java

@@ -63,6 +63,6 @@ public double getYaw() {
 
     @Override
     public Vec2D getAcceleration() {
-        return new Vec2D(this.pigeon.getAccelerationX().refresh().getValue(), this.pigeon.getAccelerationY().refresh().getValue());
+        return new Vec2D(this.pigeon.getAccelerationX().refresh().getValue()-this.pigeon.getGravityVectorX().refresh().getValue(), this.pigeon.getAccelerationY().refresh().getValue()-this.pigeon.getGravityVectorY().refresh().getValue());
     }
 }