auto amp is more consistent

Team334 · Apr 3, 2024 · c54fd1f · c54fd1f
1 parent 013ed6e
commit c54fd1f
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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);
   }