add atStartAngle() in arm, a log, a tweak in abs encoder init, and ne…

…w Constants
firebots-software · Feb 22, 2024 · 0477985 · 0477985
1 parent bf6b177
commit 0477985
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -101,6 +101,9 @@ public static final class Arm {
     public static final double ABSOLUTE_ENCODER_HORIZONTAL = 0.629;
     public static final double ABSOLUTE_HORIZONTAL_OFFSET = 0.05;
 
+    public static final double ABSOLUTE_ENCODER_START = 0.4366;
+    public static final double ABSOLUTE_ENCODER_START_TOLERANCE = 0.05;
+
     public static final InterpolatingDoubleTreeMap INTERMAP = new InterpolatingDoubleTreeMap();
     static{
       INTERMAP.put(1.25, 6d); //measurements of distance are from front of robot bumper to wall

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -115,7 +115,7 @@ public ArmSubsystem() {
             () -> {
               try {
                 do {
-                  Thread.sleep(250);
+                  Thread.sleep(50);
                 } while (!revEncoder.isConnected());
                 master.setPosition(
                     (getAbsolutePosition()) * Constants.Arm.INTEGRATED_ABSOLUTE_CONVERSION_FACTOR);
@@ -224,14 +224,22 @@ public boolean atTarget(double tolerance) {
     return Math.abs(targetDegrees - getCorrectedDegrees()) < tolerance;
   }
 
+  public boolean atStartAngle(double tolerance) {
+    return Math.abs(Constants.Arm.ABSOLUTE_ENCODER_START - getAbsolutePosition()) < tolerance;
+  }
+
+  public boolean atStartAngle() {
+    return Math.abs(Constants.Arm.ABSOLUTE_ENCODER_START - getAbsolutePosition()) < Constants.Arm.ABSOLUTE_ENCODER_START_TOLERANCE;
+  }
+
   @Override
   public void periodic() {
     setPosition(targetDegrees);
 
     SmartDashboard.putString(
         "ARM Command:",
         this.getCurrentCommand() == null ? "none" : this.getCurrentCommand().getName());
-    SmartDashboard.putNumber("ARM Abs Enc Raw: ", revEncoder.getAbsolutePosition());
+    //SmartDashboard.putNumber("ARM Abs Enc Raw: ", revEncoder.getAbsolutePosition());
     SmartDashboard.putNumber("ARM Abs Enc Func: ", getAbsolutePosition());
     SmartDashboard.putNumber("ARM Integrated Rotations: ", getMotorPosRotations());
     SmartDashboard.putNumber("ARM Integrated Current: ", master.getSupplyCurrent().getValue());
@@ -245,5 +253,7 @@ public void periodic() {
     SmartDashboard.putNumber(
         "ARM FeedForward Calculations: ",
         armff.calculate((2 * Math.PI * getRawDegrees()) / 360d, 0));
+
+    SmartDashboard.putBoolean("ARM At Start Angle: ", atStartAngle());
   }
 }