diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a1f1955..7a00433 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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_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() diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dfa4bc6..ecb7157 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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(() -> 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)); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 60f87ac..45e8bff 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/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) diff --git a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java new file mode 100644 index 0000000..d6d803d --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java new file mode 100644 index 0000000..4a1a83e --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 43a8d8c..7747bfe 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/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) { - // 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(); // } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 11dbf03..ab1eb34 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/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); }