From c54fd1f7387195efe48848903953bbcdf3c4e7a3 Mon Sep 17 00:00:00 2001 From: Jerry Date: Wed, 3 Apr 2024 16:44:43 -0400 Subject: [PATCH] auto amp is more consistent --- src/main/java/frc/robot/Constants.java | 9 +++++---- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/auto/AutoAmp.java | 9 +++------ .../java/frc/robot/subsystems/ElevatorSubsystem.java | 6 +++--- 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 242ea62..8433929 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,7 @@ public static class Speeds { public static final double SHOOTER_FAST_SPIN_SPEED = 1; public static final double SHOOTER_SLOW_SPIN_SPEED = 0.8; public static final double SHOOTER_AMP_SPEED = 1; - public static final double SHOOTER_AMP_SLOW_SPEED = 0.3; + public static final double SHOOTER_AMP_SLOW_SPEED = 0.5; public static final double SHOOTER_INTAKE_SPEED = -0.15; public static final double SHOOTER_IDLE_SPEED = 0.3; @@ -107,8 +107,8 @@ public static class Encoders { } public static class FeedForward { - public static final double ELEVATOR_KG = 0.0; - public static final double ELEVATOR_KS = 0.3; + public static final double ELEVATOR_KG = 0.25; + public static final double ELEVATOR_KS = 0.0; public static final double MODULE_DRIVE_KS = 0.3; public static final double MODULE_DRIVE_KV = 2.6; @@ -212,8 +212,9 @@ public static class FieldConstants { public static class LEDColors { public static final int[] NOTHING = {0, 0, 0}; public static final int[] GREEN = {0, 255, 0}; - public static final int[] ORANGE = {255, 165, 0}; + public static final int[] ORANGE = {255, 70, 0}; public static final int[] YELLOW = {255, 255, 0}; + public static final int[] BLUE = {0, 0, 255}; public static final int[] RED = {255, 0, 0}; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4be83ce..0af5a85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -236,7 +236,7 @@ private void configureBindings() { if (aimed) { _ledSubsystem.setColor(LEDColors.GREEN); } else { - _ledSubsystem.blink(LEDColors.RED, LEDColors.NOTHING, 0.1); + _ledSubsystem.blink(LEDColors.YELLOW, LEDColors.NOTHING, 0.1); } }, _ledSubsystem)); } @@ -247,7 +247,7 @@ private void configureBindings() { public void teleopInit() { _swerveSubsystem.fieldOriented = true; _swerveSubsystem.isClosedLoop = false; - // _shooterSubsystem.setShooterState(ShooterState.IDLE) + _shooterSubsystem.setShooterState(ShooterState.NONE); } /** @return The Command to schedule for auton. */ diff --git a/src/main/java/frc/robot/commands/auto/AutoAmp.java b/src/main/java/frc/robot/commands/auto/AutoAmp.java index 7b3e98a..ec624d9 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAmp.java +++ b/src/main/java/frc/robot/commands/auto/AutoAmp.java @@ -1,3 +1,4 @@ + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -27,12 +28,8 @@ public AutoAmp( // addCommands(new FooCommand(), new BarCommand()); addCommands( new SpinShooter(shooter, ShooterState.AMP, false).withTimeout(0.2), - new PrintCommand("REVVED"), - new SpinShooter(shooter, ShooterState.NONE, true), - new PrintCommand("CAN AMP"), - new FeedActuate(intake, ActuatorState.STOWED, FeedMode.OUTTAKE).withTimeout(0.5), - new SpinShooter(shooter, ShooterState.SLOW, false).withTimeout(0.3) - + new FeedActuate(intake, ActuatorState.STOWED, FeedMode.OUTTAKE).withTimeout(0.5) + // new SpinShooter(shooter, ShooterState.SLOW, false).withTimeout(0.1) ); } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 2e28bc1..2ba5e9c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -20,7 +20,7 @@ public class ElevatorSubsystem extends SubsystemBase { private final TalonFX _leftMotor = new TalonFX(Constants.CAN.ELEVATOR_LEFT); private final TalonFX _rightMotor = new TalonFX(Constants.CAN.ELEVATOR_RIGHT); - private final ElevatorFeedforward _elevatorFeed = new ElevatorFeedforward(FeedForward.ELEVATOR_KS, 0, 0); + private final ElevatorFeedforward _elevatorFeed = new ElevatorFeedforward(FeedForward.ELEVATOR_KS, FeedForward.ELEVATOR_KG, 0); private final ElevatorFeedforward _climbFeed = new ElevatorFeedforward(0, 0, 0); // TODO: Get this value private final PIDController _heightController = new PIDController(Constants.PID.ELEVATOR_KP, 0, 0); @@ -44,7 +44,7 @@ public ElevatorSubsystem() { _leftMotor.getConfigurator().apply(softLimits); - _heightController.setTolerance(0.02); + _heightController.setTolerance(0.005); SmartDashboard.putNumber("ELEVATOR TRIM", _elevatorTrim); } @@ -120,8 +120,8 @@ public void driveElevator(double speed) { } ff = UtilFuncs.FromVolts(ff); - speed = MathUtil.applyDeadband(speed, 0.08); + SmartDashboard.putNumber("FF PERCENT OUTPUT", ff); _leftMotor.set(ff + speed); }