From 24aada6c698d3e04505c8ed3db57d7832b623c94 Mon Sep 17 00:00:00 2001
From: Guinea Wheek <guineawheek@gmail.com>
Date: Mon, 15 Jan 2024 18:51:32 -0800
Subject: [PATCH] Canandcoder tweaks

* Make getAbsolutePosition() return between 0 and 360 (as documented)
* Make getVelocity() actually return degrees/sec (versus rotations/sec)
* Add support for setting the absolute encoder offset (added in v2024). One can also use `Canandcoder.setAbsPosition()` as well.
* Invert direction now happens on-device (the vendordep will tell the driver station if settings sets fail)
---
 .../swervelib/encoders/CanAndCoderSwerve.java | 26 +++++++------------
 1 file changed, 10 insertions(+), 16 deletions(-)

diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java
index 4b214984..9ab00987 100644
--- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java
+++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java
@@ -1,7 +1,6 @@
 package swervelib.encoders;
 
 import com.reduxrobotics.sensors.canandcoder.Canandcoder;
-import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus.
@@ -12,11 +11,7 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder
   /**
    * The {@link Canandcoder} representing the CANandCoder on the CAN bus.
    */
-  public  Canandcoder encoder;
-  /**
-   * Inversion state of the encoder.
-   */
-  private boolean     inverted = false;
+  public Canandcoder encoder;
 
   /**
    * Create the {@link Canandcoder}
@@ -30,6 +25,8 @@ public CanAndCoderSwerve(int canid)
 
   /**
    * Reset the encoder to factory defaults.
+   * 
+   * This will not clear the stored zero offset.
    */
   @Override
   public void factoryDefault()
@@ -47,14 +44,14 @@ public void clearStickyFaults()
   }
 
   /**
-   * Configure the CANandCoder to read from [0, 360) per second.
+   * Configure the Canandcoder to read from [0, 360) per second.
    *
    * @param inverted Whether the encoder is inverted.
    */
   @Override
   public void configure(boolean inverted)
   {
-    this.inverted = inverted;
+    encoder.setSettings(new Canandcoder.Settings().setInvertDirection(inverted));
   }
 
   /**
@@ -65,7 +62,7 @@ public void configure(boolean inverted)
   @Override
   public double getAbsolutePosition()
   {
-    return (inverted ? -1.0 : 1.0) * encoder.getPosition() * 360;
+    return encoder.getAbsPosition() * 360;
   }
 
   /**
@@ -80,18 +77,15 @@ public Object getAbsoluteEncoder()
   }
 
   /**
-   * Cannot set the offset of the CanAndCoder.
+   * Cannot set the offset of the Canandcoder.
    *
    * @param offset the offset the Absolute Encoder uses as the zero point.
-   * @return always false due to CanAndCoder not supporting offset changing.
+   * @return true if setting the zero point succeeded, false otherwise
    */
   @Override
   public boolean setAbsoluteEncoderOffset(double offset)
   {
-    //CanAndCoder does not support Absolute Offset Changing
-    DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(),
-                                false);
-    return false;
+    return encoder.setSettings(new Canandcoder.Settings().setZeroOffset(offset));
   }
 
   /**
@@ -102,6 +96,6 @@ public boolean setAbsoluteEncoderOffset(double offset)
   @Override
   public double getVelocity()
   {
-    return encoder.getVelocity();
+    return encoder.getVelocity() * 360;
   }
 }