Finish testing end effector and start tesing arm

RAR1741 · Feb 12, 2025 · 046947c · 046947c
1 parent 37546b8
commit 046947c
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -170,7 +170,7 @@ public void teleopPeriodic() {
     }
 
     if (m_operatorController.getWantsArmScore()) {
-      m_arm.setArmState(ArmState.SCORE);
+      m_arm.setArmState(ArmState.EXTEND);
     } else if (m_operatorController.getWantsArmStow()) {
       m_arm.setArmState(ArmState.STOW);
     }

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -154,21 +154,21 @@ public class TurnConstants {
   public static class ArmConstants {
     public final int k_motorId = 30;
 
-    public final double k_P = 0.01;
+    public final double k_P = 0.03;
     public final double k_I = 0.0;
     public final double k_D = 0.0;
     public final double k_IZone = 0.0;
-    public final double k_FF = 0.0;
+    public final double k_FF = 0.25;
 
     public final int k_maxCurrent = 5;
 
     public final double k_stowAngle = 0.0;
-    public final double k_L4Angle = 30.0;
+    public final double k_L4Angle = 120.0;
 
     public final double k_maxAcceleration = 0.0;
     public final double k_maxVelocity = 0.0;
 
-    public final double k_armOffset = 253.9 / 360.0;
+    public final double k_armOffset = (360.0 - 101.8) / 360.0;
   }
 
   public static class ElevatorConstants {
@@ -210,8 +210,8 @@ public static class EndEffectorConstants {
     public final double[] k_stopSpeeds = new double[] { 0.0, 0.0 };
     public final double[] k_indexSpeeds = new double[] { 0.3, 0.3 };
     public final double[] k_reverseSpeeds = new double[] { -0.3, -0.3 };
-    public final double[] k_branchSpeeds = new double[] { 0.1, 0.1 };
-    public final double[] k_troughSpeeds = new double[] { 0.5, 0.5 };
+    public final double[] k_branchSpeeds = new double[] { 0.5, 0.5 };
+    public final double[] k_troughSpeeds = new double[] { 0.1, 0.1 };
   }
 
   // TODO: add Gamepiece class for Coral- and Algae-related constants

diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java
@@ -42,9 +42,9 @@ public boolean getWantsArmStow() {
   }
 
   public int getWantsScore() {
-    if (this.getRawAxis(Axis.RIGHT_TRIGGER) > this.getRawAxis(Axis.LEFT_TRIGGER)) {
+    if (this.getFilteredAxis(Axis.RIGHT_TRIGGER) > this.getFilteredAxis(Axis.LEFT_TRIGGER)) {
       return 1;
-    } else if (this.getRawAxis(Axis.RIGHT_TRIGGER) < this.getRawAxis(Axis.LEFT_TRIGGER)) {
+    } else if (this.getFilteredAxis(Axis.RIGHT_TRIGGER) < this.getFilteredAxis(Axis.LEFT_TRIGGER)) {
       return -1;
     } else {
       return 0;

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -12,6 +12,7 @@
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Timer;
@@ -47,7 +48,8 @@ private Arm() {
         .pid(RobotConstants.robotConstants.Arm.k_P,
             RobotConstants.robotConstants.Arm.k_I,
             RobotConstants.robotConstants.Arm.k_D)
-        .iZone(RobotConstants.robotConstants.Arm.k_IZone);
+        .iZone(RobotConstants.robotConstants.Arm.k_IZone)
+        .feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
 
     armConfig.absoluteEncoder
         .positionConversionFactor(360.0) // [0, 1) to [0, 360)
@@ -112,7 +114,7 @@ public void writePeriodicOutputs() {
   }
 
   public enum ArmState {
-    STOW, SCORE
+    STOW, EXTEND
   }
 
   @Override
@@ -136,7 +138,7 @@ public double getArmTarget() {
       case STOW -> {
         return RobotConstants.robotConstants.Arm.k_stowAngle;
       }
-      case SCORE -> {
+      case EXTEND -> {
         return RobotConstants.robotConstants.Arm.k_L4Angle;
       }
       default -> {
@@ -145,6 +147,11 @@ public double getArmTarget() {
     }
   }
 
+  @AutoLogOutput(key = "Arm/State")
+  public ArmState getArmState() {
+    return m_periodicIO.arm_state;
+  }
+
   @AutoLogOutput(key = "Arm/Position/Setpoint")
   private double getElevatorPosSetpoint() {
     return m_currentState.position;

diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -2,7 +2,6 @@
 
 import org.littletonrobotics.junction.AutoLogOutput;
 
-import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -13,6 +12,7 @@
 
 import frc.robot.LaserCanHandler;
 import frc.robot.constants.RobotConstants;
+import frc.robot.subsystems.Arm.ArmState;
 
 public class EndEffector extends Subsystem {
   private static EndEffector m_instance;
@@ -103,8 +103,14 @@ public void periodic() {
   @Override
   public void writePeriodicOutputs() {
     double[] speeds = getIntakeSpeeds();
-    m_leftMotor.getClosedLoopController().setReference(speeds[0], ControlType.kVelocity);
-    m_rightMotor.getClosedLoopController().setReference(speeds[1], ControlType.kVelocity);
+
+    if (Arm.getInstance().getArmState() == ArmState.EXTEND) {
+      speeds[0] *= -1;
+      speeds[1] *= -1;
+    }
+
+    m_leftMotor.set(speeds[0]);
+    m_rightMotor.set(speeds[1]);
   }
 
   @AutoLogOutput(key = "EndEffector/State")