add auto trapping

src/main/java/frc/robot/FieldConstants.java

@@ -1,13 +1,17 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.GeometryUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 
 public class FieldConstants {
   public static final double kFieldLength = Constants.fieldLayout.getFieldLength();
   public static final double kFieldWidth = Constants.fieldLayout.getFieldWidth();
+  public static final Pose2d trapPose =
+      GeometryUtil.flipFieldPose(new Pose2d(11.896, 4.658, new Rotation2d(-2.093)));
 
   public static class NotePositions {
     private static final double kCenterlineX = kFieldLength / 2.0;

src/main/java/frc/robot/Robot.java

@@ -17,6 +17,7 @@
 import frc.robot.arm.Arm;
 import frc.robot.arm.Arm.ArmSetpoints;
 import frc.robot.climber.Climber;
+import frc.robot.drivetrain.DriveToPose;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.drivetrain.WheelRadiusCharacterization;
 import frc.robot.drivetrain.WheelRadiusCharacterization.Direction;
@@ -231,10 +232,12 @@ public Robot() {
     m_driverController.a().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb));
     m_driverController.b().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap));
     m_driverController.b().whileTrue(Commands.parallel(m_climber.windWinch()));
-
-    // m_driverController.x().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap));
-    m_driverController.y().whileTrue(Commands.parallel(m_climber.windWinch()));
-
+    m_driverController
+        .x()
+        .whileTrue(Superstructure.trapRoutine(m_arm, m_climber, m_shooter, m_feeder));
+    m_driverController
+        .y()
+        .whileTrue(new DriveToPose(FieldConstants.trapPose, m_drivetrain, m_lights));
     m_driverController.povRight().onTrue(m_shooter.trapShot());
 
     m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake));

src/main/java/frc/robot/arm/Arm.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -207,26 +208,24 @@ public Command aimWrist(double wristAngleRad) {
   }
 
   public Command aimElbowForTuning(DoubleSupplier driveEffort) {
-    return this.run(
-            () -> {
-              m_io.setWristRotations(Units.radiansToRotations(ArmSetpoints.kStowed.wristAngle));
-              final double driveVoltage =
-                  MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 3.0;
-              if (driveVoltage == 0.0) m_io.setElbowRotations(m_inputs.elbowPositionRot);
-              else m_io.setElbowVoltage(driveVoltage);
-            })
+    var state =
+        new Object() {
+          double currentPos = ArmSetpoints.kStowed.elbowAngle;
+        };
+    return Commands.run(
+            () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05)
+        .alongWith(aim(() -> 0, () -> state.currentPos))
         .finallyDo(() -> holdSetpoint().schedule());
   }
 
   public Command aimWristForTuning(DoubleSupplier driveEffort) {
-    return this.run(
-            () -> {
-              m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle));
-              final double driveVoltage =
-                  MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0;
-              if (driveVoltage == 0.0) m_io.setWristRotations(m_inputs.wristPositionRot);
-              else m_io.setWristVoltage(driveVoltage);
-            })
+    var state =
+        new Object() {
+          double currentPos = ArmSetpoints.kStowed.wristAngle;
+        };
+    return Commands.run(
+            () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05)
+        .alongWith(aim(() -> 1, () -> state.currentPos))
         .finallyDo(() -> holdSetpoint().schedule());
   }
 

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -6,6 +6,7 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -24,7 +25,8 @@ public class ArmIOTalonFX implements ArmIO {
       m_elbowRightFollowerMotor,
       m_wristLeftMotor,
       m_wristRightFollowerMotor;
-  private final MotionMagicVoltage m_elbowControl, m_wristControl;
+  private final PositionVoltage m_elbowControl;
+  private final MotionMagicVoltage m_wristControl;
   private final StatusSignal<Double> m_elbowPositionSignal,
       m_elbowPositionSetpointSignal,
       m_elbowVelocitySignal,
@@ -55,7 +57,7 @@ public ArmIOTalonFX() {
     final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration();
     elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction;
     elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
     elbowLeftMotorConfig.Slot0.kP = 62.122;
@@ -66,6 +68,7 @@ public ArmIOTalonFX() {
     elbowLeftMotorConfig.Slot0.kG = 0.37766;
     elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5;
     elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
+    elbowLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01;
     elbowLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0;
     elbowLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0;
     elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0;
@@ -75,7 +78,7 @@ public ArmIOTalonFX() {
     final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration();
     wristLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true;
     wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction;
     wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
@@ -89,6 +92,7 @@ public ArmIOTalonFX() {
     wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
     wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0;
     wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0;
+    wristLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01;
     wristLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0;
     wristLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0;
 
@@ -141,10 +145,8 @@ public ArmIOTalonFX() {
     ParentDevice.optimizeBusUtilizationForAll(
         m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor);
 
-    m_elbowControl =
-        new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0);
-    m_wristControl =
-        new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0);
+    m_elbowControl = new PositionVoltage(0.0, 0.0, true, 0.0, 0, false, false, false);
+    m_wristControl = new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false);
   }
 
   @Override

src/main/java/frc/robot/climber/Climber.java

@@ -48,9 +48,16 @@ public Command unwindWinch() {
 
   public Command goToTrapLimit() {
     return Commands.parallel(
-        this.run(() -> m_io.setLeftVoltage(12.0))
-            .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit),
-        this.run(() -> m_io.setRightVoltage(12.0))
-            .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit));
+            this.run(
+                () -> {
+                  if (m_inputs.leftWinchPositionRot <= kTrapLimit) m_io.setLeftVoltage(12.0);
+                  else m_io.setLeftVoltage(0.0);
+                  if (m_inputs.leftWinchPositionRot <= kTrapLimit) m_io.setRightVoltage(12.0);
+                  else m_io.setRightVoltage(0.0);
+                }))
+        .until(
+            () ->
+                m_inputs.leftWinchPositionRot > kTrapLimit
+                    && m_inputs.leftWinchPositionRot > kTrapLimit);
   }
 }

src/main/java/frc/robot/drivetrain/DriveToPose.java

@@ -11,32 +11,37 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.PoseEstimation;
+import frc.robot.lights.Lights;
 import org.littletonrobotics.junction.Logger;
 
 public class DriveToPose extends Command {
 
   private final Drivetrain m_drivetrain;
+  private final Lights m_lights;
   private final ProfiledPIDController m_translationController, m_thetaController;
   private Translation2d m_lastSetpointTranslation;
   private Pose2d m_pose;
   private Pose2d m_goalPose;
 
-  public DriveToPose(Pose2d pose, Drivetrain drivetrain) {
+  public DriveToPose(Pose2d pose, Drivetrain drivetrain, Lights lights) {
     m_pose = pose;
     m_translationController =
         new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(3.5, 2.2));
     m_thetaController =
         new ProfiledPIDController(
             5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, 4 * Math.PI));
     this.m_drivetrain = drivetrain;
-    addRequirements(drivetrain);
+    this.m_lights = lights;
+    addRequirements(drivetrain, lights);
   }
 
   @Override
   public void initialize() {
+    m_lights.setBlink(Color.kWhite);
     m_goalPose =
         Constants.onRedAllianceSupplier.getAsBoolean()
             ? GeometryUtil.flipFieldPose(m_pose)