Auto align scoring

src/main/java/frc4388/robot/Constants.java

@@ -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_ELEVATOR = -7.5; // 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_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()

src/main/java/frc4388/robot/RobotContainer.java

@@ -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(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        new InstantCommand(() -> System.out.println("Soup")),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
-        new WaitCommand(1),
+        // new InstantCommand(() -> System.out.println("Soup")),
-        new LidarAlign(m_robotSwerveDrive, m_lidar)
+        // 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));
         
             

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -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)

src/main/java/frc4388/robot/commands/waitElevatorRefrence.java

@@ -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();
+  }
+}

src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java

@@ -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();
+  }
+}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -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) {
+    if (currentState == CoordinationState.Waiting) {
-    //   periodicWaiting();
+      periodicWaiting();
-    // } else if (currentState == CoordinationState.WatingBeamTriped) {
+    } else if (currentState == CoordinationState.WatingBeamTriped) {
-    //   periodicWaitingTripped();
+      periodicWaitingTripped();
-    // }
+    } else if (currentState == CoordinationState.Ready) {
+      periodicReady();
+    }
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();
     // }

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -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); 
     }