Make the code stop bugging as much

Iron-Panthers · Nov 28, 2023 · 2141657 · 2141657
1 parent e275001
commit 2141657
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.WristCommand;
 import frc.robot.subsystems.WristSubsystem;
-import frc.util.Layer;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -21,7 +20,7 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final WristSubsystem wristSubsystem = new WristSubsystem();
-  
+
   // private final WristCommand wristCommand = new WristCommand();
   private final CommandXboxController driverB =
       new CommandXboxController(Constants.Wrist.DRIVER_CONTROLLER_PORT);
@@ -32,13 +31,12 @@ public RobotContainer() {
 
     configureButtonBindings();
 
+    //   } else if (driverB.leftTrigger().getAsBoolean()) {
 
-  //   } else if (driverB.leftTrigger().getAsBoolean()) {
-
-      driverB.y().onTrue(new WristCommand(wristSubsystem, 0));
-      driverB.a().onTrue(new WristCommand(wristSubsystem, 20));
-      driverB.b().onTrue(new WristCommand(wristSubsystem, 40));
-      driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem));
+    driverB.y().onTrue(new WristCommand(wristSubsystem, 0));
+    driverB.a().onTrue(new WristCommand(wristSubsystem, 20));
+    driverB.b().onTrue(new WristCommand(wristSubsystem, 40));
+    driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java
@@ -22,9 +22,8 @@ public class WristSubsystem extends SubsystemBase {
   private PIDController pidController;
   private double motorPower;
 
-  public WristSubsystem() {}
+  public WristSubsystem() {
 
-  public void setTargetAngle(double targetAngle) {
     this.targetAngle = targetAngle;
     this.wrist_motor = new TalonFX(Constants.Wrist.WRIST_MOTOR_DEVICE_NUMBER);
     this.wrist_motor.configFactoryDefault();
@@ -35,12 +34,15 @@ public void setTargetAngle(double targetAngle) {
     this.wrist_motor.configOpenloopRamp(.5); // can't go from 0 to 1 instantly
 
     pidController = new PIDController(0.1, 0, 0);
-  }
+}
 
   public static double degreesToTicks(double angle) {
     return angle * ((Wrist.GEAR_RATIO * Wrist.TICKS));
   }
-
+  public void setTargetAngle(double targetAngle){
+    this.targetAngle = targetAngle;
+    pidController.setSetpoint(targetAngle);
+  }
   public static double ticksToDegrees(double ticks) {
     return (ticks / (Wrist.TICKS * Wrist.GEAR_RATIO));
   }
@@ -52,7 +54,7 @@ private double getCurrentAngle() {
 
   @Override
   public void periodic() {
-    //calculates motor power
+    // calculates motor power
     motorPower = pidController.calculate(getCurrentAngle());
     // This method will be called once per scheduler run
     wrist_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower, -0.5, 0.5)));