Intake

src/main/java/frc4388/utility/compute/JankCoder copy.java.old

@@ -0,0 +1,100 @@
+package frc4388.utility.compute;
+
+import static edu.wpi.first.units.Units.Rotations;
+
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+public class JankCoder {
+    DutyCycleEncoder m_encoder;
+
+    boolean loaded_rotations = false;
+    double lastRotation = 0;
+    double offset = 0;
+
+    public JankCoder(int dio_channel) {
+        m_encoder = new DutyCycleEncoder(dio_channel);
+        loadRotations();
+    }
+    public JankCoder(int dio_channel, double _offset) {
+        m_encoder = new DutyCycleEncoder(dio_channel);
+        loadRotations();
+    }
+
+    public void update() {
+        if(!m_encoder.isConnected()) {
+            return;
+        }
+            
+        if(!loaded_rotations) {
+            loadRotations();
+        } else {
+            double curRot = m_encoder.get();
+            if(lastRotation - curRot > 0.5) {
+                offset += 1;
+                saveRotations();
+            } else if (curRot - lastRotation > 0.5) {
+                offset -= 1;
+                saveRotations();
+            }
+
+            lastRotation = curRot;
+        }
+    }
+
+    public double get() {
+        return (double) offset + m_encoder.get();
+    }
+
+    public Angle getRotations() {
+        return Rotations.of(get());
+    }
+    
+    public int getRotationCount() {
+        return (int) offset;
+    }
+    
+    public void resetRotation() {
+        setRotation(0);
+    }
+
+    public void setRotation(double rotation) {
+        offset = rotation - m_encoder.get();
+        saveRotations();
+    }
+
+    private void saveRotations() {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) {
+            stream.write(DataUtils.doubleToByteArray(offset));
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!");
+        }
+    }
+
+    private boolean loadRotations() {
+        lastRotation = m_encoder.get();
+        this.loaded_rotations = true;
+
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) {
+            offset = DataUtils.byteArrayToDouble(stream.readNBytes(4));
+            
+            // clampModify();
+            // modified = false;
+
+            // if (fileValue != currentValue) {
+            //     // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
+            //     // modified = true;
+            // }
+            return true;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("ENCODER: Unable to read encoder `" + m_encoder.getSourceChannel() + "`, using current value...");
+            return false;
+        }
+        
+    }
+}

src/main/java/frc4388/utility/compute/JankCoder.java

@@ -7,6 +7,7 @@ import java.io.FileOutputStream;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 public class JankCoder {
     DutyCycleEncoder m_encoder;
@@ -14,23 +15,18 @@ public class JankCoder {
     boolean loaded_rotations = false;
     int rotations = 0;
     double lastRotation = 0;
-    final double offset;
+    final ConfigurableDouble offset;
 
-    public JankCoder(int dio_channel) {
-        this.offset = 0;
-        m_encoder = new DutyCycleEncoder(dio_channel);
-        loadRotations();
-    }
-    public JankCoder(int dio_channel, double offset) {
+    public JankCoder(int dio_channel, ConfigurableDouble offset) {
         this.offset = offset;
         m_encoder = new DutyCycleEncoder(dio_channel);
         loadRotations();
     }
 
     public void update() {
-        if(!m_encoder.isConnected()) {
-            return;
-        }
+        // if(!m_encoder.isConnected()) {
+        //     return;
+        // }
             
         if(!loaded_rotations) {
             loadRotations();
@@ -49,7 +45,7 @@ public class JankCoder {
     }
 
     public double get() {
-        return (double) rotations + m_encoder.get() + offset;
+        return (double) rotations + m_encoder.get() + offset.get();
     }
 
     public Angle getRotations() {
@@ -61,6 +57,7 @@ public class JankCoder {
     }
     
     public void resetRotations() {
+        offset.set(-m_encoder.get());
         setRotations(0);
     }
 
@@ -69,7 +66,11 @@ public class JankCoder {
         saveRotations();
     }
 
-    private void saveRotations() {
+    public boolean isConnected() {
+        return m_encoder.isConnected();
+    }
+
+    public void saveRotations() {
         try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) {
             stream.write(DataUtils.intToByteArray(rotations));
         } catch (Exception e) {
@@ -78,7 +79,7 @@ public class JankCoder {
         }
     }
 
-    private boolean loadRotations() {
+    public boolean loadRotations() {
         lastRotation = m_encoder.get();
         this.loaded_rotations = true;
 

src/main/java/frc4388/utility/configurable/ConfigurableDouble.java

@@ -20,4 +20,8 @@ public class ConfigurableDouble {
     public double get() {
         return SmartDashboard.getNumber(name, defualtValue);
     }
+    
+    public void set(double value) {
+        SmartDashboard.putNumber(name, value);
+    }
 }