diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index f0d0c95..b9b9e6b 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -22,12 +22,6 @@ "name": "place-coral-left-l4" } }, - { - "type": "named", - "data": { - "name": "stop" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 4d15bb2..0a88f46 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.000189393939394, - "y": 1.6364267676767676 + "x": 5.959532828282828, + "y": 1.5754419191919191 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.47165404040404, - "y": 2.1141414141414145 + "x": 5.349684343434343, + "y": 2.2869318181818175 }, "prevControl": { - "x": 5.86940340909091, - "y": 1.7593308080808083 + "x": 5.747433712121213, + "y": 1.9321212121212112 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,18 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.8, + "y": 3.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index bd485e9..6c4d421 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 9df45e7..fb33877 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.1010732323232322, - "y": 1.00625 + "x": 1.286335227272727, + "y": 1.2828125 }, "prevControl": { - "x": 1.786268939393939, - "y": 2.2439551767676766 + "x": 1.9715309343434337, + "y": 2.5205176767676765 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f562627..cd388bb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -427,10 +427,11 @@ public final class Constants { public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; //Max for endefector = 60% public static final double L2_SCORE_ELEVATOR = -5; + public static final double L2_LEAVE_ELEVATOR = -11; public static final double L2_SCORE_ENDEFFECTOR = -19; - public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001; + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5d3c31b..58768c5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -274,6 +274,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), @@ -293,6 +295,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator), + new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), @@ -326,7 +330,7 @@ public class RobotContainer { new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true), - // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -354,7 +358,7 @@ public class RobotContainer { // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true), - // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4b0ccdd..4526bcd 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -48,6 +48,9 @@ public class Elevator extends Subsystem { private boolean disableAutoIntake = false; + private boolean seededZeroEndefector = false; + private boolean seededZeroElevator = false; + private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; private DigitalInput intakeIR; @@ -58,6 +61,7 @@ public class Elevator extends Subsystem { Ready, // Has coral in endefector Hovering, // Has coral in endefector L2Score, + L2ScoreLeave, 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 @@ -116,7 +120,7 @@ public class Elevator extends Subsystem { case Waiting: { wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); led.setMode(LEDConstants.WAITING_PATTERN); break; } @@ -129,7 +133,7 @@ public class Elevator extends Subsystem { } case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; @@ -148,6 +152,13 @@ public class Elevator extends Subsystem { led.setMode(LEDConstants.SCORING_PATTERN); break; } + + case L2ScoreLeave: { + PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); @@ -312,6 +323,16 @@ public class Elevator extends Subsystem { SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); + + if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { + endeffectorMotor.setPosition(0); + seededZeroEndefector = !seededZeroEndefector; + } + + if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { + elevatorMotor.setPosition(0); + seededZeroElevator = !seededZeroElevator; + } if (disableAutoIntake) return; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 4998f03..47ca4f8 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import java.time.Instant; import java.util.List; import java.util.Optional; @@ -124,6 +125,8 @@ public class Vision extends Subsystem { rotations -= 180; } + private Instant lastVisionTime = null; + private void update() { isTagProcessed = false; @@ -176,20 +179,27 @@ public class Vision extends Subsystem { lastLatency = latency / cams; if(isTagProcessed){ + Instant now = Instant.now(); - // double lastAngle = lastVisionPose.getRotation().getDegrees(); double curAngle = Yaw/cams; - // if(lastAngle - curAngle >= 90){ - // subtractRotation(); - // }else if(lastAngle - curAngle <= -90){ - // addRotation(); + // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){ + // double lastAngle = lastVisionPose.getRotation().getDegrees(); + + // if(lastAngle - curAngle >= 80){ + // addRotation(); + // }else if(lastAngle - curAngle <= -80){ + // subtractRotation(); + // } // } + + // SmartDashboard.putNumber("curAngle", curAngle); // SmartDashboard.putNumber("Rotations", rotations); lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + lastVisionTime = now; } }