diff --git a/src/main/java/com/team1816/lib/BaseRobotState.java b/src/main/java/com/team1816/lib/BaseRobotState.java index 75e28745..9d20bab8 100644 --- a/src/main/java/com/team1816/lib/BaseRobotState.java +++ b/src/main/java/com/team1816/lib/BaseRobotState.java @@ -29,7 +29,7 @@ public class BaseRobotState { * its pose. Other cases might be if we leave the ground while climbing, or if we had to use a * default auto without the correct starting pose. */ - public static volatile boolean hasAccuratePoseEstimate = true; + public static boolean hasAccuratePoseEstimate = true; /** * If you are trying to get the pose of the robot on the field, you probably want to use the @@ -42,5 +42,5 @@ public class BaseRobotState { * as the source of truth during simulation, while the raw odometry position is just supposed * to be the pre-vision position estimate of the real robot. */ - public static volatile Pose2d simActualOrRawOdometryPose = Pose2d.kZero; + public static Pose2d simActualOrRawOdometryPose = Pose2d.kZero; } diff --git a/src/main/java/com/team1816/lib/subsystems/BaseSuperstructure.java b/src/main/java/com/team1816/lib/subsystems/BaseSuperstructure.java index 03157429..08e5f8e9 100644 --- a/src/main/java/com/team1816/lib/subsystems/BaseSuperstructure.java +++ b/src/main/java/com/team1816/lib/subsystems/BaseSuperstructure.java @@ -51,7 +51,7 @@ public void periodic() { * and both methods access {@code goodVisionEstimatesSincePoseLoss} and * {@link BaseRobotState#hasAccuratePoseEstimate}. */ - private synchronized void handlePoseLossFromTilting() { + private void handlePoseLossFromTilting() { // The threshold of how far the robot can be tilted before we decide we don't have an // accurate pose estimate. Mainly to account for gyro noise. final double tiltPoseLossThresholdRadians = Units.degreesToRadians(5.0); @@ -75,7 +75,7 @@ private synchronized void handlePoseLossFromTilting() { * ({@code goodVisionEstimatesSincePoseLoss}, {@code visionEstimateSincePoseLossVarianceSum}, * and {@link BaseRobotState#hasAccuratePoseEstimate}). */ - public synchronized void addVisionMeasurementsToDrivetrain() { + public void addVisionMeasurementsToDrivetrain() { List>> visionMeasurements = vision .getVisionEstimatedPosesWithStdDevs(); diff --git a/src/main/java/com/team1816/season/subsystems/Gatekeeper.java b/src/main/java/com/team1816/season/subsystems/Gatekeeper.java index 20f9559d..82ed77e9 100644 --- a/src/main/java/com/team1816/season/subsystems/Gatekeeper.java +++ b/src/main/java/com/team1816/season/subsystems/Gatekeeper.java @@ -16,7 +16,6 @@ public class Gatekeeper extends SubsystemBase implements ITestableSubsystem { //MOTORS private final IMotor topMotor = (IMotor)factory.getDevice(NAME, "topMotor"); - private final IMotor bottomMotor = (IMotor)factory.getDevice(NAME, "bottomMotor"); private final VelocityVoltage voltageReq = new VelocityVoltage(0); @@ -33,17 +32,12 @@ public void periodic() { private void applyState() { setTopVelocity(wantedState.getTopMotorValue()); - setBottomVelocity(wantedState.getBottomMotorValue()); } private void setTopVelocity(double velocity) { topMotor.setControl(voltageReq.withVelocity(velocity)); } - private void setBottomVelocity(double velocity) { - bottomMotor.setControl(voltageReq.withVelocity(velocity)); - } - public void setWantedState(GatekeeperState state) { wantedState = state; } @@ -58,9 +52,9 @@ public GatekeeperState getState() { } public enum GatekeeperState { - OPEN(factory.getConstant(NAME, "topOpenVelocity", 0), factory.getConstant(NAME, "bottomOpenVelocity", 0)), - CLOSED(factory.getConstant(NAME, "topClosedVelocity", 0), factory.getConstant(NAME, "bottomClosedVelocity", 0)), - REVERSING(factory.getConstant(NAME, "topReversingVelocity", 0), factory.getConstant(NAME, "bottomReversingVelocity", 0)); + OPEN(factory.getConstant(NAME, "topOpenVelocity", 0), 0), + CLOSED(factory.getConstant(NAME, "topClosedVelocity", 0), 0), + REVERSING(factory.getConstant(NAME, "topReversingVelocity", 0), 0); private double topMotorValue, bottomMotorValue; @@ -72,9 +66,5 @@ public enum GatekeeperState { public double getTopMotorValue() { return topMotorValue; } - - public double getBottomMotorValue() { - return bottomMotorValue; - } } } diff --git a/src/main/java/com/team1816/season/subsystems/Intake.java b/src/main/java/com/team1816/season/subsystems/Intake.java index d8d12cc3..84ba7d43 100644 --- a/src/main/java/com/team1816/season/subsystems/Intake.java +++ b/src/main/java/com/team1816/season/subsystems/Intake.java @@ -35,8 +35,6 @@ public class Intake extends SubsystemBase implements ITestableSubsystem { * lower current to just hold it out instead of pushing it out. */ private final double FLIPPER_MOTOR_OUT_POSITION; - private final double FLIPPER_MOTOR_PULL_IN_ONE_POSITION; - private final double FLIPPER_MOTOR_PULL_IN_TWO_POSITION; private final double FLIPPER_MOTOR_IN_POSITION; private final double FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES; private final double FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES; @@ -55,8 +53,6 @@ public Intake () { super(); SmartDashboard.putData("Intake", intakeMech); FLIPPER_MOTOR_OUT_POSITION = factory.getConstant(NAME, "flipperMotorOutPosition", 0); - FLIPPER_MOTOR_PULL_IN_ONE_POSITION = factory.getConstant(NAME, "flipperMotorPullInOnePosition", 0); - FLIPPER_MOTOR_PULL_IN_TWO_POSITION = factory.getConstant(NAME, "flipperMotorPullInTwoPosition", 0); FLIPPER_MOTOR_IN_POSITION = factory.getConstant(NAME, "flipperMotorInPosition", 0); FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorRetractCurrentAmperes", 0); FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorExtendCurrentAmperes", 0); @@ -119,30 +115,6 @@ private void setFlipperPosition(FlipperPosition position) { ); } } - case PULL_IN_ONE -> { - if (flipperMotor.getMotorPosition() > FLIPPER_MOTOR_PULL_IN_ONE_POSITION) { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES) - ); - } - else { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(0) - ); - } - } - case PULL_IN_TWO -> { - if (flipperMotor.getMotorPosition() > FLIPPER_MOTOR_PULL_IN_TWO_POSITION) { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES) - ); - } - else { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(0) - ); - } - } } } @@ -156,14 +128,12 @@ public void setWantedState(IntakeState state) { */ public enum FlipperPosition { IN, - OUT, - PULL_IN_ONE, - PULL_IN_TWO + OUT } public enum IntakeState { STOW( - factory.getConstant(NAME, "intakeOffSpeed", 0, true), + factory.getConstant(NAME, "intakeSlowSpeed", .1, true), FlipperPosition.IN ), INTAKE( @@ -177,14 +147,6 @@ public enum IntakeState { STOP_OUT( 0, FlipperPosition.OUT - ), - PULL_IN_ONE( - factory.getConstant(NAME, "intakeOnSpeed", 0), - FlipperPosition.PULL_IN_ONE - ), - PULL_IN_TWO( - factory.getConstant(NAME, "intakeOnSpeed", 0), - FlipperPosition.PULL_IN_TWO ); private double intakeMotorValue; diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 3b47e4fb..6e88318b 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -636,11 +636,11 @@ private boolean areLaunchMotorsAimed() { private void setInclineAngle(double wantedAngleDegrees) { wantedInclineAngleDegrees = wantedAngleDegrees + inclineAngleAdjustmentDegrees; double rotations = Units.degreesToRotations(wantedInclineAngleDegrees); -// if (isInclineDucking) { -// // If we are trying to duck under the trench, restrict the angle of the incline to be -// // below the limit. -// rotations = Math.min(rotations, INCLINE_DUCKING_LIMIT_ROTATIONS); -// } + if (isInclineDucking) { + // If we are trying to duck under the trench, restrict the angle of the incline to be + // below the limit. + rotations = Math.min(rotations, INCLINE_DUCKING_LIMIT_ROTATIONS); + } inclineMotor.setControl(inclineMotorPositionRequest.withPosition(rotations)); } diff --git a/src/main/java/com/team1816/season/subsystems/Superstructure.java b/src/main/java/com/team1816/season/subsystems/Superstructure.java index 2d6a5f01..677d3fdc 100644 --- a/src/main/java/com/team1816/season/subsystems/Superstructure.java +++ b/src/main/java/com/team1816/season/subsystems/Superstructure.java @@ -92,14 +92,6 @@ public void setSuperstructureWantedIntakeState(WantedIntakeState intakeState) { this.wantedIntakeState = intakeState; } - public void incrementPullInSuperstructureIntakeState() { - wantedIntakeState = switch (wantedIntakeState) { - case INTAKE, OUTTAKE, STOP_OUT -> WantedIntakeState.PULL_IN_ONE; - case PULL_IN_ONE -> WantedIntakeState.PULL_IN_TWO; - case PULL_IN_TWO, STOW -> WantedIntakeState.STOW; - }; - } - /** * Sets if the incline of the shooter should duck down to fit under the trench. * @@ -252,8 +244,6 @@ private void defaulting() { case OUTTAKE -> Intake.IntakeState.OUTTAKE; case STOP_OUT -> Intake.IntakeState.STOP_OUT; case STOW -> Intake.IntakeState.STOW; - case PULL_IN_ONE -> Intake.IntakeState.PULL_IN_ONE; - case PULL_IN_TWO -> Intake.IntakeState.PULL_IN_TWO; } ); feeder.setWantedState( @@ -310,7 +300,5 @@ public enum WantedIntakeState { STOW, OUTTAKE, STOP_OUT, - PULL_IN_ONE, - PULL_IN_TWO } } diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 0a5ae401..6e488b13 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -169,7 +169,7 @@ subsystems: neutralMode: Coast motorRotation: CounterClockwise_Positive constants: - feedingDutyCycle: 0.8 + feedingDutyCycle: 0.7 stoppedDutyCycle: 0 reversingDutyCycle: -0.4 intake: @@ -183,6 +183,8 @@ subsystems: flipperMotor: id: 3 deviceType: TalonFX + supplyCurrentLimit: 35 + statorCurrentLimit: 60 neutralMode: Brake motorRotation: Clockwise_Positive remoteSensor: RemoteCANcoder # ExternalFeedbackSensorSourceValue @@ -194,7 +196,7 @@ subsystems: deviceType: CANcoder constants: intakeOnSpeed: .4 - intakeOffSpeed: 0 + intakeSlowSpeed: .1 outtakeSpeed: -0.6 flipperMotorOutPosition: 0.166 flipperMotorPullInOnePosition: 0.1 @@ -216,22 +218,10 @@ subsystems: kP: 0.5 kS: 0.31 kV: 0.081 - bottomMotor: - deviceType: TalonFX - id: 24 - motorRotation: Clockwise_Positive - pidConfig: - slot0: - kP: 0.5 - kS: 0.31 - kV: 0.08 constants: topClosedVelocity: 0 topOpenVelocity: 40 topReversingVelocity: -10 - bottomClosedVelocity: 0 - bottomOpenVelocity: 40 - bottomReversingVelocity: -10 shooter: canBusName: shootercanivore devices: @@ -295,11 +285,11 @@ subsystems: leftTurretSensorChannel: 0 rightTurretSensorChannel: 1 motorRotationsPerTurretRotation: 15.625 # 250/16 gear ratio - shooterOffsetXMeters: -0.11303 + shooterOffsetXMeters: -0.111303 shooterOffsetYMeters: -0.112268 shooterOffsetZMeters: 0.508 - inclineDuckingLimitRotations: 0.058 - distanceOneInclineAngleRotations: 0.058 + inclineDuckingLimitRotations: 0.064 + distanceOneInclineAngleRotations: 0.064 distanceOneLaunchVelocityRPS: 35 distanceTwoInclineAngleRotations: 0.08 distanceTwoLaunchVelocityRPS: 47