diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path index 25f3281..0762347 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.39747159090909, - "y": 1.3027556818181814 + "x": 2.4125846006173957, + "y": 1.7771562346874141 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.3048863636363635, - "y": 2.5292613636363637 + "x": 3.5997443181818185, + "y": 2.6888068181818174 }, "prevControl": { - "x": 5.893210227272728, - "y": 1.9808238636363626 + "x": 2.6837095087024796, + "y": 1.9835311238550895 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 114.22774531795413 + "rotation": 56.633633998940475 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4807698..e9cce41 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -69,7 +69,7 @@ public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 3.5; + public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -329,7 +329,7 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.01,0.0); + public static final Gains XY_GAINS = new Gains(3,0.3,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.5, 0); @@ -339,6 +339,9 @@ public final class Constants { public static final double XY_TOLERANCE = 0.07; // Meters public static final double ROT_TOLERANCE = 5; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; // X is tangent to reef side // Y is normal to reef side @@ -346,7 +349,7 @@ public final class Constants { public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); - public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); @@ -366,15 +369,15 @@ public final class Constants { public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547+(1*-1)), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters + public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate // X, Y, Theta // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(3, 3, 4); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } @@ -424,6 +427,8 @@ public final class Constants { public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); + // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND @@ -432,6 +437,7 @@ public final class Constants { public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe + public static final double HOVERING_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 DEALGAE_L2_ELEVATOR = -23.5; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c31278f..7ad5266 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -148,8 +148,12 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -177,11 +181,17 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -295,8 +305,10 @@ public class RobotContainer { ); private Command thrustIntake = new SequentialCommandGroup( - new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true), - new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive) + new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true), + new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive) ); private Boolean operatorManualMode = false; @@ -310,7 +322,7 @@ public class RobotContainer { NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), - new Translation2d(), 200, true + new Translation2d(), 400, true ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); @@ -428,6 +440,10 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod())); + // While Left Trigger NOT Pressed: Fine Alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)) .whileTrue(new RunCommand( @@ -436,7 +452,7 @@ public class RobotContainer { 1, Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) ), - getDeadbandedDriverController().getRight(), 0.15 + getDeadbandedDriverController().getRight(), 0.30 ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); @@ -511,10 +527,10 @@ public class RobotContainer { .onTrue (new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator)) .onFalse(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;})); - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); // Auto Scoring diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 5f7f285..47b0430 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -1,5 +1,7 @@ package frc4388.robot.commands; +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; @@ -74,6 +76,10 @@ public class GotoLastApril extends Command { double yerr; double roterr; + double xoutput; + double youtput; + double rotoutput; + @Override public void execute() { xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); @@ -100,9 +106,9 @@ public class GotoLastApril extends Command { // SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID Y Error", yerr); - double xoutput = xPID.update(xerr); - double youtput = yPID.update(yerr); - double rotoutput = rotPID.update(roterr); + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + rotoutput = rotPID.update(roterr); xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; @@ -133,7 +139,10 @@ public class GotoLastApril extends Command { @Override public final boolean isFinished() { - boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT)); // System.out.println(finished); if(finished) diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 7e775d2..06162a4 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -49,6 +49,7 @@ public class Elevator extends Subsystem { private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; + // private DigitalInput intakeIR; public enum CoordinationState { Waiting, // for coral into the though @@ -69,12 +70,14 @@ public class Elevator extends Subsystem { private CoordinationState currentState; public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; + // this.intakeIR = intakeDigitalInput; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -129,14 +132,14 @@ public class Elevator extends Subsystem { } case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; @@ -240,8 +243,8 @@ public class Elevator extends Subsystem { private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); - if(!endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Hovering); + // if(!endeffectorLimitSwitch.get()) + // transitionState(CoordinationState.Hovering); } // private void periodicWaitingTripped() { @@ -253,7 +256,7 @@ public class Elevator extends Subsystem { if (elevatorAtReference() && !endeffectorLimitSwitch.get()) transitionState(CoordinationState.Hovering); if(elevatorAtReference() && endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Waiting); + transitionState(CoordinationState.Hovering); } private void periodicScoring() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 484480a..f8770a0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -326,7 +326,7 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); } // if(e.isPresent()) @@ -392,6 +392,11 @@ public class SwerveDrive extends Subsystem { setToSlow(); } + public void startTurboPeriod() { + tmp_gear_index = gear_index; + setToTurbo(); + } + public void endSlowPeriod() { setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index ed5ebe9..347d741 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -48,7 +48,7 @@ public class Vision extends Subsystem { private boolean isTagDetected = false; private boolean isTagProcessed = false; - private Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; @@ -265,7 +265,8 @@ public class Vision extends Subsystem { public Pose2d getPose2d() { if(isTagDetected && isTagProcessed) - return lastVisionPose; + // return lastVisionPose; + return lastPhysOdomPose; else return lastPhysOdomPose; }