diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ab5a991..f648e60 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -234,7 +234,10 @@ public final class Constants { public static final Gains XY_GAINS = new Gains(3,0.01,0.0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); - public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0); public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole public static final int LIDAR_DIO_CHANNEL = 7; @@ -361,8 +364,6 @@ public final class Constants { 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(17); @@ -418,12 +419,15 @@ public final class Constants { 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 DEALGAE_L2_ELEVATOR = -23.5; + public static final double DEALGAE_L3_ELEVATOR = -33.75; 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 L2_SCORE_ENDEFECTOR = -19; + public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e25878f..4180d77 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -314,11 +314,20 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); + + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) @@ -360,6 +369,23 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(AprilLidarAlignL3); + + //Trims + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + + + // Lower coral removal DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new SequentialCommandGroup( new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), @@ -372,6 +398,19 @@ public class RobotContainer { new Translation2d(0,1), new Translation2d(), 500, true) )); + // Upper coral removal + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + .onTrue(new SequentialCommandGroup( + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), 500, true) + )); + // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b3c6507..7bd2861 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -3,6 +3,7 @@ package frc4388.robot.commands; 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.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; @@ -54,7 +55,9 @@ public class GotoLastApril extends Command { public void initialize() { xPID.initialize(); yPID.initialize(); - this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance); + this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, + Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), + distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())); } double xerr; diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 579de20..72137f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ElevatorConstants; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ @@ -28,6 +29,9 @@ public class Elevator extends SubsystemBase { private long wait = 0; private long maxWait = 1000; + private double elevatorRefrence = 0; + private double endefectorRefrence = 0; + private DigitalInput basinBeamBreak; private DigitalInput endefectorLimitSwitch; @@ -36,6 +40,7 @@ public class Elevator extends SubsystemBase { WatingBeamTriped, //once the beam break trips Ready, // Has coral in endefector Hovering, // Has coral in endefector + L2Score, 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 @@ -66,6 +71,9 @@ public class Elevator extends SubsystemBase { //PID methods private void PIDPosition(TalonFX motor, double position) { + if (motor == elevatorMotor) elevatorRefrence = position; + else endefectorRefrence = position; + var request = new PositionDutyCycle(position); motor.setControl(request); } @@ -106,39 +114,57 @@ public class Elevator extends SubsystemBase { break; } + case L2Score: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + case PrimedFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); break; } case ScoringFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR); + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case PrimedThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR); break; } case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR); + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + + case BallRemoverL3Primed: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); + break; + } + + case BallRemoverL3Go: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } @@ -150,17 +176,22 @@ public class Elevator extends SubsystemBase { } public boolean elevatorAtRefrence() { - double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); + // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.1; + if (atPos) { + SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence); + SmartDashboard.putNumber("Elevator Pos", elevatorPosition); + } - return Math.abs(elevatorRefrence - elevatorPosition) <= 0.1; + return atPos; } public boolean endefectorAtRefrence() { - double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); - double elevatorPosition = endefectorMotor.getPosition().getValueAsDouble(); + // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); + double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); - return Math.abs(elevatorRefrence - elevatorPosition) <= 0.2; + return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2; } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -173,14 +204,14 @@ public class Elevator extends SubsystemBase { transitionState(CoordinationState.Ready); } - private void periodicWaitingTripped() { - if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) - transitionState(CoordinationState.Ready); - } + // private void periodicWaitingTripped() { + // if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) + // transitionState(CoordinationState.Ready); + // } private void periodicReady() { - // if (elevatorAtRefrence()) - // transitionState(CoordinationState.Hovering); + if (elevatorAtRefrence()) + transitionState(CoordinationState.Hovering); } private void periodicScoring() { @@ -193,13 +224,14 @@ public class Elevator extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putString("State", currentState.toString()); if (currentState == CoordinationState.Waiting) { - // periodicWaiting(); + periodicWaiting(); } else if (currentState == CoordinationState.WatingBeamTriped) { // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { - // periodicReady(); + periodicReady(); } // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring();