Add JankCoder

src/main/java/frc4388/robot/RobotMap.java

@@ -34,6 +34,7 @@ import frc4388.robot.subsystems.swerve.SwerveIO;
 import frc4388.robot.subsystems.swerve.SwerveReal;
 import frc4388.robot.subsystems.vision.VisionIO;
 import frc4388.robot.subsystems.vision.VisionReal;
+import frc4388.utility.compute.JankCoder;
 import frc4388.utility.status.FaultCANCoder;
 import frc4388.utility.status.FaultPhotonCamera;
 import frc4388.utility.status.FaultPidgeon2;
@@ -106,11 +107,10 @@ public class RobotMap {
                 // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
                 // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-
-                // shooterIO = new ShooterIO() {};
                 shooterIO = new ShooterReal(shooter1, shooter2, indexer);
+                JankCoder armEncoder = new JankCoder(0, IntakeConstants.ARM_ENCODER_OFFSET);
 
-                intakeIO = new IntakeReal(arm, roller);
+                intakeIO = new IntakeReal(arm, roller, armEncoder);
 
                 // Fault
                 FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -44,7 +44,7 @@ public class Intake extends SubsystemBase {
         switch (mode) {
             case Bouncing:
                 // When bounce is enabled: set the bounce timer
-                this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD;
+                this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
                 break;
             default:
                 break;
@@ -118,15 +118,15 @@ public class Intake extends SubsystemBase {
                     state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() &&
                     state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
                 ) {
-                    this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD;
+                    this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
                 }
 
                 // Get the time delta from the last bounce time update
                 double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
                 // Get the percentage through the bounce period (0 output means one half period has passed)
-                double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT;
+                double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
                 // Clamp the output of the motor to some value
-                percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT);
+                percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
 
                 io.armOutput(percentOutput);
                 break;

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -13,11 +13,13 @@ public class IntakeConstants {
 
     public static final double ARM_MOTOR_GEAR_RATIO = 125;
     public static final double ROLLER_MOTOR_GEAR_RATIO = 3;
+    public static final double ARM_ENCODER_OFFSET = 0.;
 
 
-    public static final double BOUNCE_HALF_PERIOD = 5.;
-    public static final double INTAKE_BOUNCE_OUTPUT = 0.2;
-    public static final double INTAKE_BOUNCE_MAX_OUTPUT = 0.5;
+
+    public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.);
+    public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.2);
+    public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.5);
     public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20);
     public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20);
 
@@ -27,7 +29,7 @@ public class IntakeConstants {
 
     public static final CanDevice ARM_ID   = new CanDevice("ARM", 20);
     public static final CanDevice ROLLER_ID   = new CanDevice("INTAKE_ROLLER", 21);
-    // public static final int ARM_LIMIT_SWITCH_CHANNEL = 9;
+    public static final int ARM_ENCODER_ID = 0;
         
     // Limits
 
@@ -40,7 +42,7 @@ public class IntakeConstants {
     // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
 
     public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1);
-    public static final ConfigurableDouble ARM_ssLIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33);
+    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33);
     public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4);
     public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4);
     public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70);

src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java

@@ -17,11 +17,15 @@ public interface IntakeIO {
     public class IntakeState {
         double currentBounceTime = 0;
 
+        boolean extendedLimit = false;
         boolean retractedLimit = false;
+        Angle intakeEncoder = Rotations.of(0);
+
         Angle armAngle = Rotations.of(0);
         Angle armTargetAngle = Rotations.of(0);
         Current armMotorCurrent = Amps.of(0);
 
+
         AngularVelocity armMotorVelocity = RotationsPerSecond.of(0);
         // AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0);
 

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -19,22 +19,24 @@ import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import frc4388.utility.compute.JankCoder;
 
 public class IntakeReal implements IntakeIO {
 
     SparkMax m_armMotor;
     SparkMax m_rollerMotor;
-    DutyCycleEncoder encoder;
+    JankCoder m_encoder;
 
     public IntakeReal(
-        // DigitalInput armLimitSwitch,
         SparkMax armMotor,
-        SparkMax rollerMotor
+        SparkMax rollerMotor,
+        JankCoder jankCoder
     ) {
         // m_angleMotor = angleMotor;
         // m_pitchMotor = pitchMotor;
         m_armMotor = armMotor;
         m_rollerMotor = rollerMotor;
+        m_encoder = jankCoder;
         // m_armLimitSwitch = armLimitSwitch;
     }
 
@@ -74,9 +76,24 @@ public class IntakeReal implements IntakeIO {
         // m_rollerMotor.set(0);
     }
 
+    private boolean retractedLimit() {
+        return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get();
+    }
+    private boolean extendedLimit() {
+        return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get();
+    }
+
     @Override
     public void armOutput(double percentOutput){
+
+        if(retractedLimit()) {
+            percentOutput = Math.max(percentOutput, 0);
+        } else if (extendedLimit()) {
+            percentOutput = Math.min(percentOutput, 0);
+        }
+
         m_armMotor.set(percentOutput);
+
     }
 
     @Override
@@ -89,6 +106,12 @@ public class IntakeReal implements IntakeIO {
         state.rollerOutput = m_rollerMotor.get();
         state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent());
 
+        state.retractedLimit = retractedLimit();
+        state.extendedLimit = extendedLimit();
+        state.armAngle = m_encoder.getRotations();
+
+        
+
 
         // if(state.retractedLimit) {
         //     // Set the arm motor to be zero if the limit switch is pressed

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

@@ -0,0 +1,105 @@
+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;
+    int rotations = 0;
+    double lastRotation = 0;
+    final double offset;
+
+    public JankCoder(int dio_channel) {
+        this.offset = 0;
+        m_encoder = new DutyCycleEncoder(dio_channel);
+        loadRotations();
+    }
+    public JankCoder(int dio_channel, double offset) {
+        this.offset = 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) {
+                rotations += 1;
+                saveRotations();
+            } else if (curRot - lastRotation > 0.5) {
+                rotations -= 1;
+                saveRotations();
+            }
+
+            lastRotation = curRot;
+        }
+    }
+
+    public double get() {
+        return (double) rotations + m_encoder.get() + offset;
+    }
+
+    public Angle getRotations() {
+        return Rotations.of(get());
+    }
+    
+    public int getRotationCount() {
+        return rotations;
+    }
+    
+    public void resetRotations() {
+        setRotations(0);
+    }
+
+    public void setRotations(int rotation) {
+        rotations = rotation;
+        saveRotations();
+    }
+
+    private void saveRotations() {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) {
+            stream.write(DataUtils.intToByteArray(rotations));
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!");
+            rotations = 0;
+        }
+    }
+
+    private boolean loadRotations() {
+        lastRotation = m_encoder.get();
+        this.loaded_rotations = true;
+
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) {
+            int fileValue = DataUtils.byteArrayToInt(stream.readNBytes(4));
+            
+            rotations = fileValue;
+            // 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/status/FaultSparkMax.java

@@ -63,7 +63,7 @@ public class FaultSparkMax implements Queryable {
             s.addReport(ReportLevel.ERROR, "Sensor fault");
         }
         if(faults.temperature) {
-            s.addReport(ReportLevel.ERROR, "Tempreture fault");
+            s.addReport(ReportLevel.ERROR, "Temperature fault");
         }
 
         Warnings warnings = motor.getWarnings();