add absolute encoder for falcon

src/main/java/frc/libzodiac/Constant.java

@@ -0,0 +1,14 @@
+package frc.libzodiac;
+
+public final class Constant {
+    // 2023.6.24: going to measure
+    /**
+     * How many radians a raw <code>TalonSRX</code>'s unit is equal to.
+     */
+    public static double TALONSRX_ENCODER_UNIT = 2 * Math.PI / 4096;
+
+    /**
+     * Falcon uses rotation as its position unit.
+     */
+    public static double FALCON_POSITION_UNIT = 2 * Math.PI;
+}

src/main/java/frc/libzodiac/Util.java

@@ -21,7 +21,7 @@ public static double deg(double rad) {
     /**
      * Extends the modulo operation to R.
      * <p>
-     * THe definition here:
+     * The definition here:
      * <p>
      * a and b are congruent modulo c, if and only if (a-b)/c is integer.
      *

src/main/java/frc/libzodiac/ZEncoder.java

@@ -11,9 +11,9 @@ public abstract class ZEncoder {
     public final int can_id;
 
     /**
-     * Zero position of this encoder in raw units.
+     * Zero position of this encoder in radians. The adjustion is applied to output automatically by <code>ZEncoder</code>.
      */
-    public double zero_pos;
+    public double zero;
 
     public ZEncoder(int can_id) {
         this.can_id = can_id;
@@ -28,7 +28,7 @@ public ZEncoder(int can_id) {
      * Configures the zero position of the encoder.
      */
     public ZEncoder set_zero(double pos) {
-        this.zero_pos = pos;
+        this.zero = pos;
         return this;
     }
 
@@ -37,8 +37,15 @@ public ZEncoder set_zero(double pos) {
      */
     public abstract double get_raw();
 
+    /**
+     * Get the current position in radians, regardless of zero position.
+     */
+    public abstract double get_rad();
+
     /**
      * Get the current position.
      */
-    public abstract double get();
+    public double get() {
+        return this.get_rad() - this.zero;
+    }
 }

src/main/java/frc/libzodiac/ZGyro.java

@@ -1,7 +1,7 @@
 package frc.libzodiac;
 
 public abstract class ZGyro {
-    protected final int can_id;
+    public final int can_id;
 
     /**
      * Zero position in raw sensor units.

src/main/java/frc/libzodiac/ZMotor.java

@@ -1,9 +1,10 @@
 package frc.libzodiac;
 
-import java.util.HashMap;
-
 import frc.libzodiac.util.PIDProfile;
 
+import java.util.HashMap;
+import java.util.Objects;
+
 /**
  * Defines a large collection of APIs to operate various motors so that motors
  * can be controled under a unified generic way.
@@ -20,18 +21,6 @@ public abstract class ZMotor {
      */
     public HashMap<String, Double> profile;
 
-    /**
-     * Gets the value of the specified motion profile.
-     */
-    protected double profile(String name) {
-        var opt = this.profile.get(name);
-        if (opt == null) {
-            return 0;
-        } else {
-            return opt;
-        }
-    }
-
     /**
      * Initialize this motor, e.g. binds to the CAN bus.
      */
@@ -45,10 +34,12 @@ protected double profile(String name) {
     /**
      * Set PID parameters.
      */
-    public ZMotor set_pid(PIDProfile pid) {
+    public final ZMotor set_pid(PIDProfile pid) {
         this.pid = pid;
         return this.apply_pid();
-    };
+    }
+
+    ;
 
     /**
      * Set PID parameters.
@@ -75,7 +66,7 @@ public ZMotor set_pid(double k_p, double k_i, double k_d) {
 
     /**
      * Set the output.
-     * 
+     *
      * @param raw_unit Usually speed output percentage in [-1,1] for generic motors
      *                 and desired angle positions in rads for servos.
      */

src/main/java/frc/libzodiac/Zoystick.java

@@ -69,14 +69,14 @@ public double axis(int axis) {
     }
 
     /**
-     * Gets the x axis of the joystick.
+     * Gets the x-axis of the joystick.
      */
     public double x() {
         return this.axis(0);
     }
 
     /**
-     * Gets the y axis of the joystick.
+     * Gets the y-axis of the joystick.
      */
     public double y() {
         return this.axis(1);
@@ -85,7 +85,7 @@ public double y() {
     /**
      * Set the input filter function.
      */
-    public Zoystick with_filter(Function<Double, Double> f) {
+    public Zoystick set_filter(Function<Double, Double> f) {
         this.filter_fn = f;
         return this;
     }

src/main/java/frc/libzodiac/Zwerve.java

@@ -12,27 +12,13 @@
  */
 public abstract class Zwerve extends SubsystemBase {
 
-    /**
-     * The length of the bot.
-     * <p>
-     * Units are not that important since the program only uses the ratio of length
-     * and width.
-     */
-    private final double length;
-
-    /**
-     * The width of the bot.
-     * <p>
-     * Units are not that important since the program only uses the ratio of length
-     * and width.
-     */
-    private final double width;
+    public final Vec2D shape;
 
     /**
      * Method to calculate the radius of the rectangular robot.
      */
     private double radius() {
-        return Math.hypot(this.length, this.width) / 2;
+        return this.shape.div(2).r();
     }
 
     public boolean headless = false;
@@ -115,13 +101,14 @@ public static interface Module {
     /**
      * Creates a new Zwerve.
      *
-     * @param modules See <code>Zwerve.module</code>
+     * @param modules See <code>Zwerve.module</code>.
+     * @param gyro    The gyro.
+     * @param shape   Shape of the robot, <code>x</code> for length and <code>y</code> for width.
      */
-    public Zwerve(Module[] modules, ZGyro gyro, double length, double width) {
+    public Zwerve(Module[] modules, ZGyro gyro, Vec2D shape) {
         this.module = modules;
         this.gyro = gyro;
-        this.length = length;
-        this.width = width;
+        this.shape = shape;
     }
 
     /**
@@ -142,11 +129,12 @@ public Zwerve init() {
     /**
      * Kinematics part from 6941.
      */
+    @Deprecated
     public Zwerve go_previous(Vec2D velocity, double omega) {
         var x = velocity.x;
         var y = velocity.y;
-        var l = this.length;
-        var w = this.width;
+        var l = this.shape.x;
+        var w = this.shape.y;
         var r = this.radius();
         var a = y - omega * l / r;
         var b = y + omega * l / r;
@@ -174,8 +162,8 @@ public Zwerve go_previous(Vec2D velocity, double omega) {
      * Kinematics part rewritten using vector calculations.
      */
     public Zwerve go(Vec2D velocity, double omega) {
-        var l = this.length / 2;
-        var w = this.width / 2;
+        var l = this.shape.x / 2;
+        var w = this.shape.y / 2;
         var pi2 = Math.PI / 2;
         var vt = omega * this.radius();
         Vec2D[] v = {
@@ -184,10 +172,8 @@ public Zwerve go(Vec2D velocity, double omega) {
                 new Vec2D(-l, -w).rot(pi2).with_r(vt).add(velocity),
                 new Vec2D(l, -w).rot(pi2).with_r(vt).add(velocity),
         };
-        var max = v[1].max(v[2]).max(v[3]).max(v[4]).r();
-        if (max > 1)
-            for (var i : v)
-                i = i.div(max);
+        var max = v[0].max(v[1]).max(v[2]).max(v[3]).r();
+        if (max > 1) for (int i = 0; i < 4; i++) v[i] = v[i].div(max);
         for (int i = 0; i < 4; i++)
             this.module[i].go(v[i].mul(output).rot(this.dir_fix()));
         return this;
@@ -264,12 +250,6 @@ protected Drive exec() {
                 v_y = -v_y;
             if (this.inv_rot)
                 omega = -omega;
-            if (v_x < 1e-3)
-                v_x = 0;
-            if (v_y < 1e-3)
-                v_y = 0;
-            if (omega < 1e-3)
-                omega = 0;
             this.chassis.go(new Vec2D(v_x, v_y), omega);
             return this;
         }
@@ -302,4 +282,5 @@ public Drive inv(boolean v_x, boolean v_y, boolean rot) {
     public Drive control(Zoystick vel, Zoystick rot) {
         return new Drive(this, vel, rot);
     }
+
 }

src/main/java/frc/libzodiac/hardware/Falcon.java

@@ -3,9 +3,10 @@
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.controls.StaticBrake;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
-
+import frc.libzodiac.Constant;
 import frc.libzodiac.ZMotor;
 import frc.libzodiac.Zervo;
 
@@ -48,13 +49,13 @@ public Falcon shutdown() {
 
     @Override
     public Falcon stop() {
-        this.motor.stopMotor();
+        this.motor.setControl(new StaticBrake());
         return this;
     }
 
     @Override
     public Falcon go(String profile) {
-        this.motor.setControl(new VelocityDutyCycle(this.profile(profile)));
+        this.motor.setControl(new VelocityDutyCycle(this.profile.get(profile)));
         return this;
     }
 
@@ -66,6 +67,9 @@ public Falcon go(double raw_unit) {
 
     public static class Servo extends Falcon implements Zervo {
 
+        /**
+         * Zero position applied to adjust output value of Falcon's builtin encoder.
+         */
         protected double zero = 0;
 
         public Servo(int can_id) {
@@ -74,13 +78,13 @@ public Servo(int can_id) {
 
         @Override
         public Servo go(String profile) {
-            this.motor.setControl(new PositionDutyCycle(this.profile(profile)));
+            this.motor.setControl(new PositionDutyCycle(this.profile.get(profile)));
             return this;
         }
 
         @Override
-        public Servo go(double rad) {
-            this.motor.setControl(new PositionDutyCycle(rad / 2 / Math.PI));
+        public Servo go(double angle) {
+            this.motor.setControl(new PositionDutyCycle((angle - this.zero) / Constant.FALCON_POSITION_UNIT));
             return this;
         }
 
@@ -97,7 +101,7 @@ public double get_zero() {
 
         @Override
         public double get_pos() {
-            return this.motor.getPosition().getValue() * 2 * Math.PI - this.zero;
+            return this.motor.getPosition().refresh().getValue() * Constant.FALCON_POSITION_UNIT - this.zero;
         }
 
         @Override

src/main/java/frc/libzodiac/hardware/Pigeon.java

@@ -21,17 +21,17 @@ public Pigeon init() {
 
     @Override
     protected double get_yaw() {
-        return Util.rad(this.gyro.getYaw().getValue());
+        return Util.rad(this.gyro.getYaw().refresh().getValue());
     }
 
     @Override
     protected double get_pitch() {
-        return Util.rad(this.gyro.getPitch().getValue());
+        return Util.rad(this.gyro.getPitch().refresh().getValue());
     }
 
     @Override
     protected double get_roll() {
-        return Util.rad(this.gyro.getRoll().getValue());
+        return Util.rad(this.gyro.getRoll().refresh().getValue());
     }
 
 }

src/main/java/frc/libzodiac/hardware/TalonSRXEncoder.java

@@ -1,10 +1,11 @@
 package frc.libzodiac.hardware;
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-
+import frc.libzodiac.Constant;
 import frc.libzodiac.ZEncoder;
 
 public class TalonSRXEncoder extends ZEncoder {
+
     public TalonSRXEncoder(int can_id) {
         super(can_id);
     }
@@ -23,9 +24,8 @@ public double get_raw() {
     }
 
     @Override
-    public double get() {
-        // TODO
-        return this.get_raw();
+    public double get_rad() {
+        return this.get_raw() / Constant.TALONSRX_ENCODER_UNIT;
     }
 
 }

src/main/java/frc/libzodiac/hardware/TalonSRXMotor.java

@@ -44,7 +44,7 @@ public ZMotor stop() {
 
     @Override
     public ZMotor go(String profile) {
-        this.motor.set(ControlMode.Velocity, this.profile(profile));
+        this.motor.set(ControlMode.Velocity, this.profile.get(profile));
         return this;
     }
 
@@ -80,7 +80,7 @@ public double get_pos() {
 
         @Override
         public Servo go(String profile) {
-            this.motor.set(ControlMode.Position, this.profile(profile));
+            this.motor.set(ControlMode.Position, this.profile.get(profile));
             return this;
         }