From c27cb1071885c5bde4302673af2fb2264b7a38af Mon Sep 17 00:00:00 2001 From: cherriae Date: Wed, 6 Mar 2024 18:48:45 -0500 Subject: [PATCH] tested everything --- src/main/java/frc/robot/Constants.java | 36 ++++++++++--------- src/main/java/frc/robot/RobotContainer.java | 31 ++++++++-------- .../robot/subsystems/ElevatorSubsystem.java | 7 ++-- .../frc/robot/subsystems/IntakeSubsystem.java | 6 ++-- .../robot/subsystems/ShooterSubsystem.java | 7 ++-- .../frc/robot/utils/configs/NeoConfig.java | 2 +- .../robot/utils/configs/TalonFXConfig.java | 3 +- 7 files changed, 49 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 73f8745..3d76419 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,31 +22,31 @@ public final class Constants { public static final Alliance SAFE_ALLIANCE = Alliance.Red; public static class CAN { - public static final int DRIVE_FRONT_LEFT = 5; - public static final int ROT_FRONT_LEFT = 6; + public static final int DRIVE_FRONT_LEFT = 1; + public static final int ROT_FRONT_LEFT = 2; - public static final int DRIVE_FRONT_RIGHT = 7; - public static final int ROT_FRONT_RIGHT = 8; + public static final int DRIVE_FRONT_RIGHT = 3; + public static final int ROT_FRONT_RIGHT = 4; - public static final int DRIVE_BACK_RIGHT = 1; - public static final int ROT_BACK_RIGHT = 2; + public static final int DRIVE_BACK_RIGHT = 5; + public static final int ROT_BACK_RIGHT = 6; - public static final int DRIVE_BACK_LEFT = 3; - public static final int ROT_BACK_LEFT = 4; + public static final int DRIVE_BACK_LEFT = 7; + public static final int ROT_BACK_LEFT = 8; - public static final int ENC_FRONT_LEFT = 11; - public static final int ENC_FRONT_RIGHT = 12; - public static final int ENC_BACK_LEFT = 10; - public static final int ENC_BACK_RIGHT = 9; + public static final int ENC_FRONT_LEFT = 9; + public static final int ENC_FRONT_RIGHT = 10; + public static final int ENC_BACK_LEFT = 11; + public static final int ENC_BACK_RIGHT = 12; public static final int SHOOTER_LEFT = 13; public static final int SHOOTER_RIGHT = 14; - public static final int SHOOTER_ANGLE = 15; + public static final int SHOOTER_ANGLE = 15; // make sure settings are right. something got messed up when trying to fix intake sparkmax public static final int ELEVATOR_LEFT = 16; // TODO: Get right can ID's public static final int ELEVATOR_RIGHT = 17; - public static final int INTAKE_ACTUATOR = 18; + public static final int INTAKE_ACTUATOR = 18; // double check this because we are switching out for a new sparkmax public static final int INTAKE_FEED = 19; public static final int CAN_TIMEOUT = 10; @@ -64,10 +64,10 @@ public static class Speeds { public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3; public static final double ELEVATOR_MAX_SPEED = 0.2; - public static final double INTAKE_FEED_SPEED = 0.25; // TODO: Get this + public static final double INTAKE_FEED_SPEED = 0.4; // TODO: Get this public static final double OUTTAKE_FEED_SPEED = -0.4; - public static final double INTAKE_ACTUATE_MAX_SPEED = 0.5; + public static final double INTAKE_ACTUATE_MAX_SPEED = 0.3; } public static class Physical { @@ -92,6 +92,8 @@ public static class Encoders { public static final int INTAKE_OUT = 15; public static final int SHOOTER_SHOOT_VEL = 10; // TODO: get this + + public static final int ELEVATOR_MAX_HEIGHT = 155; } public static class FeedForward { @@ -106,7 +108,7 @@ public static class FeedForward { public static class PID { // TODO: Tune everything - public static final double MODULE_DRIVE_KP = 0.05; + public static final double MODULE_DRIVE_KP = 0.015; public static final double MODULE_ROTATION_KP = 0.009; public static final double SHOOTER_ANGLE_KP = 0.08; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 362a67d..ad11926 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final VisionSubsystem _visionSubsystem = new VisionSubsystem(); - // private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem); + private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem); private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem(); private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem(); private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem(); @@ -95,10 +95,10 @@ public RobotContainer() { // Drive/Operate default commands - // _swerveSubsystem.setDefaultCommand(new TeleopDrive(_swerveSubsystem, - // () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1), - // () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1), - // () -> MathUtil.applyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.1))); + _swerveSubsystem.setDefaultCommand(new TeleopDrive(_swerveSubsystem, + () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05), + () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05), + () -> MathUtil.applyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.05))); _shooterSubsystem.setDefaultCommand(new OperateShooter( _shooterSubsystem, @@ -135,10 +135,10 @@ public RobotContainer() { // to configure button bindings private void configureBindings() { - // Command safeActuate = new SetShooter( - // _shooterSubsystem, - // () -> 0 - // ).andThen(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE)); + Command safeActuate = new SetShooter( + _shooterSubsystem, + () -> 0 + ).andThen(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)); // safeActuate = new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE); @@ -169,10 +169,10 @@ private void configureBindings() { // Commands.run(() -> _elevatorSubsystem.driveElevator(0.5), _elevatorSubsystem).handleInterrupt(() -> _elevatorSubsystem.stopElevator()) // ); - // Runnable stop = () -> _shooterSubsystem.stopShooter(); + Runnable stop = () -> _shooterSubsystem.stopShooter(); - // _operatorController.L1().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT).handleInterrupt(stop)); - // _operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stop)); + _operatorController.L1().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT).handleInterrupt(stop)); + _operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stop)); // _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE)); // _operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)); @@ -182,9 +182,10 @@ private void configureBindings() { // _operatorController.square().whileTrue( // Commands.run(() -> _intakeSubsystem.feed(FeedMode.INTAKE), _intakeSubsystem).handleInterrupt(() -> _intakeSubsystem.feed(FeedMode.NONE)) // ); - _operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE)); - // _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE)); - _operatorController.triangle().whileTrue(Commands.run(_intakeSubsystem::work, _intakeSubsystem)); + _operatorController.square().whileTrue(safeActuate); + _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE)); + _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.NONE, FeedMode.INTAKE)); + // _operatorController.triangle().whileTrue(Commands.run(_intakeSubsystem::work, _intakeSubsystem)); // _operatorController.R1().whileTrue(new SetShooter(_shooterSubsystem, () -> 38)); // _operatorController.R2().whileTrue(new SetElevator(_elevatorSubsystem, () -> 45)); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index afcb741..8bd5b25 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Constants.Encoders; import frc.robot.utils.UtilFuncs; import frc.robot.utils.configs.TalonFXConfig; @@ -31,11 +32,11 @@ public ElevatorSubsystem() { SoftwareLimitSwitchConfigs softLimits = new SoftwareLimitSwitchConfigs(); - softLimits.ForwardSoftLimitThreshold = 85; + softLimits.ForwardSoftLimitThreshold = Encoders.ELEVATOR_MAX_HEIGHT; softLimits.ReverseSoftLimitThreshold = 0; - softLimits.ForwardSoftLimitEnable = true; - softLimits.ReverseSoftLimitEnable = true; + softLimits.ForwardSoftLimitEnable = false; + softLimits.ReverseSoftLimitEnable = false; _leftMotor.getConfigurator().apply(softLimits); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a4a2bc8..4dd0254 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -62,8 +62,8 @@ public IntakeSubsystem() { _actuatorMotor.setSoftLimit(SoftLimitDirection.kForward, Constants.Encoders.INTAKE_OUT); _actuatorMotor.setSoftLimit(SoftLimitDirection.kReverse, Constants.Encoders.INTAKE_STOWED); - _actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + _actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); } /** @@ -85,7 +85,7 @@ public boolean noteSafety() { } public void work() { - _actuatorMotor.set(0.6); + _actuatorMotor.set(0.1); } /** diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ff6a1ea..f0cd5b7 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -34,7 +34,7 @@ public class ShooterSubsystem extends SubsystemBase { private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder(); - private final ArmFeedforward _angleFeed = new ArmFeedforward(FeedForward.SHOOTER_ANGLE_KG, 0, 0); + private final ArmFeedforward _angleFeed = new ArmFeedforward(0, 0, 0); private final PIDController _angleController = new PIDController(PID.SHOOTER_ANGLE_KP, 0, 0); /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */ @@ -54,8 +54,8 @@ public ShooterSubsystem() { // soft limits SoftwareLimitSwitchConfigs softLimits = new SoftwareLimitSwitchConfigs(); - softLimits.ForwardSoftLimitThreshold = 54 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360; - softLimits.ReverseSoftLimitThreshold = 0 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360; + softLimits.ForwardSoftLimitThreshold = 20 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360; + softLimits.ReverseSoftLimitThreshold = -7 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360; softLimits.ForwardSoftLimitEnable = false; softLimits.ReverseSoftLimitEnable = false; @@ -104,6 +104,7 @@ public double getVelocity() { */ public void driveAngle(double speed) { _angleMotor.set(UtilFuncs.FromVolts(_angleFeed.calculate(Math.toRadians(getAngle()), 0)) + speed); + System.out.println(speed); } /** Stops the shooter's angular movement. */ diff --git a/src/main/java/frc/robot/utils/configs/NeoConfig.java b/src/main/java/frc/robot/utils/configs/NeoConfig.java index 4315193..fe3b175 100644 --- a/src/main/java/frc/robot/utils/configs/NeoConfig.java +++ b/src/main/java/frc/robot/utils/configs/NeoConfig.java @@ -25,7 +25,7 @@ public static void configureNeo(CANSparkMax neo, boolean invert) { // neo.setCANTimeout(10); - neo.setIdleMode(IdleMode.kBrake); + neo.setIdleMode(IdleMode.kCoast); neo.enableSoftLimit(SoftLimitDirection.kForward, false); neo.enableSoftLimit(SoftLimitDirection.kReverse, false); diff --git a/src/main/java/frc/robot/utils/configs/TalonFXConfig.java b/src/main/java/frc/robot/utils/configs/TalonFXConfig.java index 40ed8c0..580163d 100644 --- a/src/main/java/frc/robot/utils/configs/TalonFXConfig.java +++ b/src/main/java/frc/robot/utils/configs/TalonFXConfig.java @@ -43,9 +43,10 @@ public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean inver config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - config.CurrentLimits.StatorCurrentLimit = 50; // TODO: Find + // config.CurrentLimits.StatorCurrentLimit = 50; // TODO: Find falcon.getConfigurator().apply(config); + falcon.setPosition(0); return config; }