need to fix elevator pid

Team334 · Mar 21, 2024 · 1f5f3c8 · 1f5f3c8
1 parent 3e53d91
commit 1f5f3c8
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -162,6 +162,7 @@ private void configureBindings() {
     }, _swerveSubsystem));
 
     _driveController.L2().whileTrue(
+      Commands.runOnce(() -> SmartDashboard.putBoolean("REACHED", false)).andThen(
       new AutoAim2( // TODO: test
         _swerveSubsystem,
         _shooterSubsystem, 
@@ -172,7 +173,7 @@ private void configureBindings() {
         () -> (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180,
         () -> Presets.CLOSE_SHOOTER_ANGLE, 
         () -> Presets.CLOSE_ELEVATOR_HEIGHT
-      )
+      ).andThen(Commands.runOnce(() -> SmartDashboard.putBoolean("REACHED", true))))
     );
 
     _driveController.R2().whileTrue(

diff --git a/src/main/java/frc/robot/commands/auto/AutoAim2.java b/src/main/java/frc/robot/commands/auto/AutoAim2.java
@@ -51,7 +51,7 @@ private AutoAim2(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
+      // new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
       new SetShooter(shooter, shooterAngle, runOnce),
       new SetElevator(elevator, elevatorHeight, runOnce)
       // led command here

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(0, 0, 0);
+  private final ElevatorFeedforward _elevatorFeed = new ElevatorFeedforward(0.001, 0, 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);
@@ -42,14 +42,14 @@ public ElevatorSubsystem() {
 
     _leftMotor.getConfigurator().apply(softLimits);
 
-    _heightController.setTolerance(0.02);
+    _heightController.setTolerance(0.01);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
     // harry chen code maybe fix
-
+    SmartDashboard.putNumber("ELEVATOR SETPOINT", _heightController.getSetpoint());
     SmartDashboard.putNumber("ELEVATOR HEIGHT METERS", getHeight());
     SmartDashboard.putNumber("ELEVATOR PERCENT OUTPUT", _leftMotor.get());
     SmartDashboard.putNumber("ELEVATOR VELOCITY", getVelocity());
@@ -109,10 +109,10 @@ public void driveElevator(double speed) {
 
     if (_usingClimberFeed)
       // ff = _climbFeed.calculate(speed);
-      ff = _elevatorFeed.calculate(0);
+      ff = _climbFeed.calculate(0);
     else {
       // ff = _elevatorFeed.calculate(speed);
-      ff = _elevatorFeed.calculate(0);
+      ff = _elevatorFeed.calculate(speed);
     }
 
     ff = UtilFuncs.FromVolts(ff);

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -83,6 +83,7 @@ public void periodic() {
     _holdNote = beamBroken ? true : _holdNote;
 
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("SHOOTER SETPOINT", _angleController.getSetpoint());
     SmartDashboard.putNumber("SHOOTER ANGLE", getAngle());
     SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
     SmartDashboard.putNumber("SHOOTER ANGULAR VELOCITY", getAngularVelocity());