diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8c2577b..e17123b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,7 +65,7 @@ public static class Speeds { public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3; public static final double ELEVATOR_MAX_SPEED = 0.75; - public static final double INTAKE_FEED_SPEED = 0.5; // TODO: Get this + public static final double INTAKE_FEED_SPEED = 0.6; // TODO: Get this public static final double OUTTAKE_FEED_SPEED = -0.4; public static final double INTAKE_ACTUATE_MAX_SPEED = 0.3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4662218..a6817c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,10 +143,10 @@ private void configureBindings() { _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE)); _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE)); - // test for the presets - _driveController.square().onTrue( - new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem) - ); + _operatorController.R2().whileTrue( + Commands.runOnce(_intakeSubsystem::disableReverseSoftLimit, _intakeSubsystem).andThen( + Commands.run(() -> _intakeSubsystem.actuate(-0.3), _intakeSubsystem).handleInterrupt(_intakeSubsystem::resetReverseSoftLimit) + )); // driver bindings _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem)); diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 63e1d22..ea96a4b 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -160,10 +160,6 @@ public void execute() { Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED ); - _reachedSwerveHeading = _headingController.atSetpoint(); - _reachedShooterAngle = _shooter.atDesiredAngle(); - _reachedElevatorHeight = _elevator.atDesiredHeight(); - if (_reachedSwerveHeading) rotationVelocity = 0; SmartDashboard.putNumber("Y", _desiredSwerveHeading); @@ -184,6 +180,10 @@ public void execute() { _shooter.setAngle(_desiredShooterAngle); _elevator.setHeight(_desiredElevatorHeight); + + _reachedSwerveHeading = _headingController.atSetpoint(); + _reachedShooterAngle = _shooter.atDesiredAngle(); + _reachedElevatorHeight = _elevator.atDesiredHeight(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 5a27813..38dc611 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.PID; +import frc.robot.Constants.Speeds; import frc.robot.utils.configs.NeoConfig; /** @@ -93,6 +94,19 @@ public boolean isFeedMode(FeedMode feedMode) { public boolean isActuatorState(ActuatorState actuatorState) { return _actuatorState == actuatorState; + /** + * Disables the reverse soft limit of the actuator. + */ + public void disableReverseSoftLimit() { + _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + } + + /** + * Resets the reverse soft limit (and encoder) of the actuator. + */ + public void resetReverseSoftLimit() { + _actuatorEncoder.setPosition(0); + _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); } /** @@ -111,45 +125,42 @@ public double getActuator() { return _actuatorEncoder.getPosition(); } + /** + * Actuate the intake at a given speed. Speed gets clamped. + */ + public void actuate(double speed) { + _actuatorMotor.set(MathUtil.clamp( + speed, + -Speeds.INTAKE_ACTUATE_MAX_SPEED, + Speeds.INTAKE_ACTUATE_MAX_SPEED + )); + } + /** * Set the actuator's state. MUST BE CALLED REPEATEDLY. * * @param actuatorState * The state to set the actuator to. */ - public void actuate(ActuatorState actuatorState) { - double out = 0; - + public void actuate(ActuatorState actuatorState) { _actuatorState = actuatorState; switch (actuatorState) { case STOWED : - out = MathUtil.clamp( - _actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_STOWED), - -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED, - Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED - ); + actuate(_actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_STOWED)); break; case OUT : - out = MathUtil.clamp( - _actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_OUT), - -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED, - Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED - ); + actuate(_actuatorController.calculate(getActuator(), Constants.Encoders.INTAKE_OUT)); break; case NONE: - out = 0; + actuate(0); break; default: break; } - - // System.out.println(out); - SmartDashboard.putNumber("OUT", out); - _actuatorMotor.set(out); } // public void setAngle(double angle){ @@ -195,6 +206,7 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("ACTUATOR ENCODER", _actuatorEncoder.getPosition()); SmartDashboard.putData("ACTUATOR PID", _actuatorController); + SmartDashboard.putNumber("ACTUATOR OUT", _actuatorMotor.get()); // SmartDashboard.putBoolean("NOTE SAFETY", noteSafety()); diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 426e202..73b4fd9 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -86,9 +86,9 @@ public class SwerveDriveSubsystem extends SubsystemBase { Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(), new SwerveModulePosition[]{_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(), _backLeft.getPosition()}, - new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 1.2)); + new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 1.8)); // note: LL heading std was 0.9 - // OTHER POSSIBLE STD VALUES: + // OTHER POSSIBLE STD DEV VALUES: // VecBuilder.fill(0.006, 0.006, 0.007), VecBuilder.fill(0.52, 0.52, 1.35) // VecBuilder.fill(0.006, 0.006, 0.007), VecBuilder.fill(0.5, 0.5, 1.3) @@ -173,6 +173,7 @@ public void periodic() { if (_visionSubsystem.isValid()) { // TODO: make sure this works Optional visionBotpose = _visionSubsystem.getBotpose(); if (visionBotpose.isPresent()) { + // Pose2d updateBotpose = new Pose2d(visionBotpose.get().getTranslation(), getHeading()); // use gyro heading only? _estimator.addVisionMeasurement(visionBotpose.get(), _visionSubsystem.getLatency()); } }