Auto align scoring

2026-06-09 00:38:02 -06:00 · 2025-02-18 19:39:01 -07:00
parent 47646dc12b
commit 2641abe8a4
7 changed files with 180 additions and 27 deletions
@@ -358,11 +358,16 @@ public final class Constants {
    public static final class FieldConstants {
        public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
        // public static final double HORISONTAL_SCORING_POSITION_OFFSET =  Units.inchesToMeters(9.5);
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET =  Units.inchesToMeters(6.5);
+
+        public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
+
+
        ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
        // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right

        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
+        public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);



@@ -402,17 +407,22 @@ public final class Constants {
        public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
        
        public static final double GEAR_RATIO_ELEVATOR = -9.0;
-
+        //Max for elevator = 50%
+        
        public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = (2 / 3.d) * GEAR_RATIO_ELEVATOR; // TODO: find 2-4 in off the pipe
-        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find on the pipe
+        public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
+        public static final double SCORING_THREE_ELEVATOR = -9.25;
        public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
-
+        
        public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
+        public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
+        //Max for endefector = 60%

        public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
        public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
+        public static final double SCORING_THREE_ENDEFECTOR = 0.375 * GEAR_RATIO_ENDEFECTOR;
+        public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
        public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;

        public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
+import frc4388.robot.Constants.FieldConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -40,6 +41,8 @@ import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.waitElevatorRefrence;
+import frc4388.robot.commands.waitEndefectorRefrence;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;

@@ -109,20 +112,29 @@ public class RobotContainer {
    // () -> autoplaybackName.get(), // lastAutoName
    //        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
    //        true, false);
-    private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
+    // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
    private Command AprilLidarAlign = new SequentialCommandGroup(
-        new GotoLastApril(m_robotSwerveDrive, m_vision),
-        new InstantCommand(() -> System.out.println("Soup")),
-        new WaitCommand(1),
-        new LidarAlign(m_robotSwerveDrive, m_lidar)
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+        // new InstantCommand(() -> System.out.println("Soup")),
+        // new WaitCommand(1),
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+        new waitEndefectorRefrence(m_robotElevator),
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+        new Translation2d(0,1), new Translation2d(), 500, true),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
    );
    private Command AprilLidarLeft = new SequentialCommandGroup(
-        AutoGotoPosition.asProxy(),
+        AprilLidarAlign.asProxy(),
        new LidarAlign(m_robotSwerveDrive, m_lidar)
    );

    private Command AprilLidarRight = new SequentialCommandGroup(
-        AutoGotoPosition.asProxy(),
+        AprilLidarAlign.asProxy(),
        new InstantCommand(() -> System.out.println("Soup")),
        new WaitCommand(1),
        new LidarAlign(m_robotSwerveDrive, m_lidar)//,
@@ -153,7 +165,7 @@ public class RobotContainer {
     */
    public RobotContainer() {
        
-        NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
+        NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlign);
        NamedCommands.registerCommand("april-allign", AprilLidarAlign);
        NamedCommands.registerCommand("place-coral", placeCoral);
        NamedCommands.registerCommand("grab-coral", grabCoral);
@@ -251,6 +263,13 @@ public class RobotContainer {

        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
+            
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(AprilLidarAlign);
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
          
        // ?  /* Operator Buttons */

@@ -272,6 +291,9 @@ public class RobotContainer {
            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));

        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
+
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
        
            
@@ -28,15 +28,19 @@ public class GotoLastApril extends Command {

    SwerveDrive swerveDrive;
    Vision vision;
+    double distance;
+    Side side;

    /**
     * Command to drive robot to position.
     * @param SwerveDrive m_robotSwerveDrive
     */

-    public GotoLastApril(SwerveDrive swerveDrive, Vision vision) {
+    public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
        this.swerveDrive = swerveDrive;
        this.vision = vision;
+        this.distance = distance;
+        this.side = side;
        // addRequirements(swerveDrive);
    }

@@ -50,7 +54,7 @@ public class GotoLastApril extends Command {
    public void initialize() {
        xPID.initialize();
        yPID.initialize();
-        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
+        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
    }
    
    double xerr;
@@ -70,6 +74,10 @@ public class GotoLastApril extends Command {
        double youtput = yPID.update(yerr);
        double rotoutput = rotPID.update(roterr);

+        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
+
        Translation2d leftStick = new Translation2d(
            Math.max(Math.min(-youtput, 1), -1),
            Math.max(Math.min(-xoutput, 1), -1)
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitElevatorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitElevatorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.elevatorAtRefrence();
+  }
+}
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitEndefectorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitEndefectorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.endefectorAtRefrence();
+  }
+}
@@ -4,6 +4,8 @@

 package frc4388.robot.subsystems;

+import java.time.Instant;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -23,6 +25,9 @@ public class Elevator extends SubsystemBase {
  private TalonFX elevatorMotor;
  private TalonFX endefectorMotor;

+  private long wait = 0;
+  private long maxWait = 1000;
+
  private DigitalInput basinBeamBreak;
  private DigitalInput endefectorLimitSwitch;

@@ -30,6 +35,7 @@ public class Elevator extends SubsystemBase {
    Waiting, // for coral into the though
    WatingBeamTriped, //once the beam break trips
    Ready, // Has coral in endefector
+    Hovering, // Has coral in endefector
    PrimedThree, // Arm and elevator Waiting to score in the level 3 position
    ScoringThree, // Arm and elevator in the level three position
    PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -76,6 +82,7 @@ public class Elevator extends SubsystemBase {
    currentState = state;
    switch (currentState) {
      case Waiting: {
+        wait = System.currentTimeMillis() + maxWait;
        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
        break;
@@ -84,7 +91,6 @@ public class Elevator extends SubsystemBase {
      case WatingBeamTriped: {
        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
-        armShuffle();
        break;
      }

@@ -94,7 +100,19 @@ public class Elevator extends SubsystemBase {
        break;
      }

+      case Hovering: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        break;
+      }
+
      case ScoringThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_THREE_ENDEFECTOR);
+        break;
+      }
+
+      case PrimedFour: {
        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
        break;
@@ -102,13 +120,26 @@ public class Elevator extends SubsystemBase {

      case ScoringFour: {
        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
        break;
      }
    }

  }

+  public boolean elevatorAtRefrence() {
+    double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
+    double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
+
+    return Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
+  }
+
+  public boolean endefectorAtRefrence() {
+    double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
+    double elevatorPosition = endefectorMotor.getPosition().getValueAsDouble();
+
+    return Math.abs(elevatorRefrence - elevatorPosition) <= 0.2;
+  }
  // public void driveElevatorStick(Translation2d stick) {
  //   if (stick.getNorm() > 0.05) {
  //     elevatorMotor.set(stick.getY());
@@ -116,28 +147,38 @@ public class Elevator extends SubsystemBase {
  // }

  private void periodicWaiting() {
-    if (basinBeamBreak.get()) transitionState(CoordinationState.WatingBeamTriped);
+    if (!basinBeamBreak.get()) 
+      transitionState(CoordinationState.WatingBeamTriped);
  }

  private void periodicWaitingTripped() {
-    if (basinBeamBreak.get()) transitionState(CoordinationState.Ready);
+    if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
+      transitionState(CoordinationState.Ready);
  }
  
+  private void periodicReady() {
+    if (elevatorAtRefrence())
+      transitionState(CoordinationState.Hovering);
+  }
+
  private void periodicScoring() {
    if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
  }

  @Override
  public void periodic() {
+
    // This method will be called once per scheduler run
    SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
    SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);

-    // if (currentState == CoordinationState.Waiting) {
-    //   periodicWaiting();
-    // } else if (currentState == CoordinationState.WatingBeamTriped) {
-    //   periodicWaitingTripped();
-    // }
+    if (currentState == CoordinationState.Waiting) {
+      periodicWaiting();
+    } else if (currentState == CoordinationState.WatingBeamTriped) {
+      periodicWaitingTripped();
+    } else if (currentState == CoordinationState.Ready) {
+      periodicReady();
+    }
    // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
    //   periodicScoring();
    // }
@@ -72,10 +72,10 @@ public class ReefPositionHelper {
        return new Pose2d();
    }

-    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) {
+    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
        return offset(getNearestTag(position), 
        (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
-        FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); 
+        ydistance); 
    }