From 21416574b1e553851977a302e2756f352f644d76 Mon Sep 17 00:00:00 2001
From: driverstation1 <5026craptop@gmail.com>
Date: Mon, 27 Nov 2023 16:55:34 -0800
Subject: [PATCH] Make the code stop bugging as much

---
 src/main/java/frc/robot/RobotContainer.java        | 14 ++++++--------
 .../java/frc/robot/subsystems/WristSubsystem.java  | 12 +++++++-----
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b32e060..d508e60 100644
--- 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
index 74b66f3..bd2bfc1 100644
--- 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)));