From a110e392ef6ebfe6c6eb82606a4b1b99f4af00dc Mon Sep 17 00:00:00 2001
From: driverstation1 <5026craptop@gmail.com>
Date: Tue, 5 Dec 2023 16:09:18 -0800
Subject: [PATCH] Moves

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

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 247e298..bc8097a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -30,6 +30,7 @@ public static class Intake {
 
     public static final double UNLOADING_WAIT_TIME = 2f;
   }
+
   public final class Wrist {
     public static final int DRIVER_CONTROLLER_PORT = 0;
     public static final int WRIST_MOTOR_DEVICE_NUMBER = 16;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d81591a..9fa60b8 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,17 +4,14 @@
 
 package frc.robot;
 
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.commands.StopIntakeMotorCommand;
-import frc.robot.commands.IntakeCommand;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.SequentialCommands;
-import frc.robot.subsystems.WristSubsystem;
 import frc.robot.commands.AdvancedIntakeSequence;
+import frc.robot.commands.SequentialCommands;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.WristSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -34,7 +31,7 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
-    driverB.y().onTrue(new SequentialCommands(wristSubsystem));
+    
     configureButtonBindings();
 
     //   } else if (driverB.leftTrigger().getAsBoolean()) {
@@ -42,7 +39,7 @@ public RobotContainer() {
     // 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));
+    
   }
 
   /**
@@ -78,6 +75,8 @@ public void containerMatchStarting() {
    */
   private void configureButtonBindings() {
     // intake and outtake
+    driverB.y().onTrue(new SequentialCommands(wristSubsystem));
+    driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem));
     driverB.rightTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, true));
     driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true));
     driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false));
diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java
index e2ba11a..e643b08 100644
--- a/src/main/java/frc/robot/subsystems/WristSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java
@@ -32,9 +32,9 @@ public WristSubsystem() {
     this.wrist_motor = new TalonFX(Constants.Wrist.WRIST_MOTOR_DEVICE_NUMBER);
     this.wrist_motor.configFactoryDefault();
     this.wrist_motor.clearStickyFaults();
-    this.wrist_motor.configForwardSoftLimitThreshold(0);
-    this.wrist_motor.configReverseSoftLimitThreshold(degreesToTicks(90));
-    this.wrist_motor.configReverseSoftLimitEnable(true, 0);
+    //this.wrist_motor.configForwardSoftLimitThreshold(0);
+    //this.wrist_motor.configReverseSoftLimitThreshold(degreesToTicks(90));
+    //this.wrist_motor.configReverseSoftLimitEnable(true, 0);
     this.wrist_motor.configForwardSoftLimitEnable(true, 0);
     this.wrist_motor.setNeutralMode(NeutralMode.Coast);
     this.wrist_motor.setSelectedSensorPosition(0);
@@ -43,12 +43,13 @@ public WristSubsystem() {
     pidController = new PIDController(0.1, 0, 0);
     WristTab.add(pidController);
     WristTab.addNumber("Current Motor Position", wrist_motor::getSelectedSensorPosition);
+    WristTab.addNumber("Current motor angle", this::getCurrentAngle);
     WristTab.addBoolean("Is at target", this::nearTargetAngle);
     WristTab.addNumber("Target Angle", () -> this.targetAngle);
-}
+  }
 
   public static double degreesToTicks(double angle) {
-    return (angle*360d)/((Wrist.GEAR_RATIO * Wrist.TICKS));
+    return (angle * 360d) / ((Wrist.GEAR_RATIO * Wrist.TICKS));
   }
 
   public void setTargetAngle(double targetAngle) {
@@ -57,7 +58,7 @@ public void setTargetAngle(double targetAngle) {
   }
 
   public static double ticksToDegrees(double ticks) {
-    return ((ticks / Wrist.TICKS) * (Wrist.GEAR_RATIO/360));
+    return ((ticks / Wrist.TICKS) * (Wrist.GEAR_RATIO) / 360);
   }
 
   private double getCurrentAngle() {