Add JankCoder

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