add support for CANcoder to MaroonFramework (#21)

* start adding support for CANcoder in MaroonFramework * remove CANcoderEncoder.get() * add support for CANcoders * responding to PR comments
Team766 · Feb 1, 2024 · 5592608 · 5592608
1 parent 38bedf9
commit 5592608
Show file tree

Hide file tree

Showing 8 changed files with 118 additions and 8 deletions.
diff --git a/src/main/java/com/team766/hal/EncoderReader.java b/src/main/java/com/team766/hal/EncoderReader.java
@@ -2,6 +2,10 @@
 
 public interface EncoderReader extends ControlInputReader {
 
+    enum Type {
+        CANcoder
+    };
+
     /**
      * Reset the Encoder distance to zero. Resets the current count to zero on the encoder.
      */

diff --git a/src/main/java/com/team766/hal/RobotProvider.java b/src/main/java/com/team766/hal/RobotProvider.java
@@ -48,6 +48,8 @@ protected abstract MotorController getMotor(
 
     public abstract EncoderReader getEncoder(int index1, int index2);
 
+    public abstract EncoderReader getEncoder(int index, String configPrefix);
+
     public abstract DigitalInputReader getDigitalInput(int index);
 
     public abstract AnalogInputReader getAnalogInput(int index);
@@ -158,6 +160,13 @@ public MotorController getMotor(final String configName) {
 
     public EncoderReader getEncoder(final String configName) {
         try {
+            // check for a single port encoder
+            final ValueProvider<Integer> deviceId =
+                    ConfigFileReader.getInstance().getInt(configName + ".deviceId");
+            if (deviceId.hasValue()) {
+                return getEncoder(deviceId.get(), configName);
+            }
+            // or a dual-port encoder
             final ValueProvider<Integer[]> ports =
                     ConfigFileReader.getInstance().getInts(configName + ".ports");
             final ValueProvider<Double> distancePerPulseConfig =
@@ -171,7 +180,7 @@ public EncoderReader getEncoder(final String configName) {
                                 "Encoder %s has %d config values, but expected 2",
                                 configName,
                                 portsValue.length);
-                return new MockEncoder(0, 0);
+                return new MockEncoder();
             }
             checkDeviceName("encoder/digital input", digitalIoNames, portsValue[0], configName);
             checkDeviceName("encoder/digital input", digitalIoNames, portsValue[1], configName);
@@ -186,7 +195,7 @@ public EncoderReader getEncoder(final String configName) {
                             Severity.ERROR,
                             "Encoder %s not found in config file, using mock encoder instead",
                             configName);
-            return new MockEncoder(0, 0);
+            return new MockEncoder();
         }
     }
 

diff --git a/src/main/java/com/team766/hal/mock/MockEncoder.java b/src/main/java/com/team766/hal/mock/MockEncoder.java
@@ -8,7 +8,7 @@ public class MockEncoder implements EncoderReader {
     private double rate = 0;
     private double distancePerPulse = 1;
 
-    public MockEncoder(final int a, final int b) {}
+    public MockEncoder() {}
 
     @Override
     public void reset() {

diff --git a/src/main/java/com/team766/hal/mock/TestRobotProvider.java b/src/main/java/com/team766/hal/mock/TestRobotProvider.java
@@ -35,19 +35,24 @@ public MotorController getMotor(
                     new LocalMotorController(
                             configPrefix,
                             new MockMotorController(index),
-                            localSensor != null ? localSensor : new MockEncoder(-1, -1));
+                            localSensor != null ? localSensor : new MockEncoder());
         }
         return motors[index];
     }
 
     @Override
     public EncoderReader getEncoder(final int index1, final int index2) {
         if (encoders[index1] == null) {
-            encoders[index1] = new MockEncoder(index1, index2);
+            encoders[index1] = new MockEncoder();
         }
         return encoders[index1];
     }
 
+    @Override
+    public EncoderReader getEncoder(final int index1, String configPrefix) {
+        return new MockEncoder();
+    }
+
     @Override
     public SolenoidController getSolenoid(final int index) {
         if (solenoids[index] == null) {

diff --git a/src/main/java/com/team766/hal/simulator/Encoder.java b/src/main/java/com/team766/hal/simulator/Encoder.java
@@ -8,8 +8,8 @@ public class Encoder implements EncoderReader {
     private final int channel;
     private double distancePerPulse = 1.0;
 
-    public Encoder(final int a, final int b) {
-        this.channel = a;
+    public Encoder(final int channel) {
+        this.channel = channel;
     }
 
     @Override

diff --git a/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java b/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java
@@ -44,7 +44,15 @@ public MotorController getMotor(
     @Override
     public EncoderReader getEncoder(final int index1, final int index2) {
         if (encoders[index1] == null) {
-            encoders[index1] = new Encoder(index1, index2);
+            encoders[index1] = new Encoder(index1);
+        }
+        return encoders[index1];
+    }
+
+    @Override
+    public EncoderReader getEncoder(final int index1, String configPrefix) {
+        if (encoders[index1] == null) {
+            encoders[index1] = new Encoder(index1);
         }
         return encoders[index1];
     }

diff --git a/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java b/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java
@@ -0,0 +1,61 @@
+package com.team766.hal.wpilib;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.team766.hal.EncoderReader;
+import com.team766.logging.Category;
+import com.team766.logging.Logger;
+import com.team766.logging.Severity;
+
+public class CANcoderEncoder implements EncoderReader {
+
+    private final CANcoder cancoder;
+    private double distancePerPulse = 1;
+
+    public CANcoderEncoder(int deviceId) {
+        cancoder = new CANcoder(deviceId);
+    }
+
+    public CANcoderEncoder(int deviceId, String canBus) {
+        cancoder = new CANcoder(deviceId, canBus);
+    }
+
+    @Override
+    public double getDistance() {
+        StatusSignal<Double> position = cancoder.getPosition();
+        if (!position.getStatus().isOK()) {
+            Logger.get(Category.HAL)
+                    .logData(
+                            Severity.ERROR,
+                            "Unable to get position: %s",
+                            position.getStatus().toString());
+            return 0;
+        }
+        return distancePerPulse * position.getValueAsDouble();
+    }
+
+    @Override
+    public double getRate() {
+        StatusSignal<Double> velocity = cancoder.getVelocity();
+        if (!velocity.getStatus().isOK()) {
+            Logger.get(Category.HAL)
+                    .logData(
+                            Severity.ERROR,
+                            "Unable to get rate: %s",
+                            velocity.getStatus().toString());
+            return 0;
+        }
+        return distancePerPulse * velocity.getValueAsDouble();
+    }
+
+    @Override
+    public void reset() {
+        cancoder.setPosition(0);
+    }
+
+    @Override
+    public void setDistancePerPulse(double distancePerPulse) {
+        // each "pulse" is a rotation
+        this.distancePerPulse = distancePerPulse;
+    }
+}
diff --git a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java
@@ -18,6 +18,7 @@
 import com.team766.hal.RobotProvider;
 import com.team766.hal.SolenoidController;
 import com.team766.hal.mock.MockBeaconSensor;
+import com.team766.hal.mock.MockEncoder;
 import com.team766.hal.mock.MockGyro;
 import com.team766.hal.mock.MockMotorController;
 import com.team766.hal.mock.MockPositionSensor;
@@ -177,6 +178,28 @@ public EncoderReader getEncoder(int index1, int index2) {
         return encoders[index1];
     }
 
+    @Override
+    public EncoderReader getEncoder(int index1, String configPrefix) {
+        final ValueProvider<EncoderReader.Type> type =
+                ConfigFileReader.getInstance()
+                        .getEnum(EncoderReader.Type.class, configPrefix + ".type");
+
+        if (type.hasValue()) {
+            if (type.get() == EncoderReader.Type.CANcoder) {
+                ValueProvider<String> canBus =
+                        ConfigFileReader.getInstance().getString(configPrefix + ".CANBus");
+                return new CANcoderEncoder(index1, canBus.get());
+            } else {
+                Logger.get(Category.CONFIGURATION)
+                        .logData(
+                                Severity.WARNING,
+                                "Unknown encoder type: %s",
+                                type.get().toString());
+            }
+        }
+        return new MockEncoder();
+    }
+
     @Override
     public SolenoidController getSolenoid(int index) {
         if (solenoids[index] == null) {