End of Tippecanoe Comp

RAR1741 · Mar 26, 2023 · 49a30c4 · 49a30c4
1 parent 44b5bb6
commit 49a30c4
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -121,7 +121,7 @@ public static class Wrist {
       public static final double k_minAngle = Units.degreesToRadians(-360.0);
       public static final double k_maxAngle = Units.degreesToRadians(360.0);
 
-      public static final double k_offset = 0.311455;
+      public static final double k_offset = 0.144028;
     }
 
     public static enum Preset {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -195,10 +195,16 @@ public void teleopPeriodic() {
           m_operatorController.getArmVerticalChange(0.5));
     }
 
-    if (m_operatorController.getRawButtonPressed(6)) {
+    if (m_operatorController.getWantsWristRotate()) {
       m_arm.rotateWrist();
     }
 
+    if (m_operatorController.getWantsWristCorrectionClockwise()) {
+      m_arm.rotateWrist(1);
+    } else if (m_operatorController.getWantsWristCorrectionCounterClockwise()) {
+      m_arm.rotateWrist(-1);
+    }
+
     if (m_operatorController.getWantsDefaultState()) {
       m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
     }

diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java
@@ -65,4 +65,16 @@ public boolean getWantsMidCubeScore() {
   public boolean getWantsElbowChange() {
     return this.getRawButton(7);
   }
+
+  public boolean getWantsWristRotate() {
+    return this.getRawButtonPressed(6);
+  }
+
+  public boolean getWantsWristCorrectionClockwise() {
+    return this.getFilteredAxis(3) > 0.5;
+  }
+
+  public boolean getWantsWristCorrectionCounterClockwise() {
+    return this.getFilteredAxis(2) > 0.5;
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -291,6 +291,10 @@ public void rotateWrist() {
     m_periodicIO.wristAngle -= 90;
   }
 
+  public void rotateWrist(double change) {
+    m_periodicIO.wristAngle += change;
+  }
+
   public void setWristAngle(double wristAngle) {
     m_periodicIO.wristAngle = wristAngle;
   }