From fcbea5c3ca5ff1ecb8e5b152a7b6a0993331d36a Mon Sep 17 00:00:00 2001
From: Simon <87281874+simonp17@users.noreply.github.com>
Date: Tue, 1 Aug 2023 18:55:50 -0700
Subject: [PATCH 1/2] Add simple controls for wrist positions

---
 src/main/java/frc/robot/Robot.java | 48 ++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c06ecc9..aaac108 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,14 +4,62 @@
 
 package frc.robot;
 
+
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.XboxController;
+
 public class Robot extends LoggedRobot {
+
+  private TalonFX armMotor = new TalonFX(16);
+  XboxController controller = new XboxController(0);
+  VoltageOut armVoltage = new VoltageOut(0);
+  StatusSignal<Double> armRotor = armMotor.getRotorPosition();
+  double xGoalRotations = Rotation2d.fromDegrees(10).getRotations();
+  double yGoalRotations = Rotation2d.fromDegrees(50).getRotations();
+
+
   public Robot() {
     Logger.getInstance().addDataReceiver(new NT4Publisher());
 
     Logger.getInstance().start();
   }
+
+  public void setWristAngle(double degrees, double currentRotation) {
+    if (Rotation2d.fromDegrees(degrees).getRotations()<currentRotation) {
+      armMotor.setControl(armVoltage.withOutput(0.2));
+    } else if (Rotation2d.fromDegrees(degrees).getRotations()>currentRotation) {
+      armMotor.setControl(armVoltage.withOutput(-0.2));
+    } else if (Rotation2d.fromDegrees(degrees).getRotations()== currentRotation) {
+      armMotor.setControl(armVoltage.withOutput(0));
+    }
+  }
+  @Override
+  public void teleopPeriodic() {
+    boolean aPressed = controller.getAButtonPressed();
+    boolean xPressed = controller.getXButtonPressed();
+    boolean yPressed = controller.getYButtonPressed();
+    boolean bPressed = controller.getBButtonPressed();
+    double motorRotations = armRotor.getValue();
+    double armRotations = motorRotations /50;
+    if(aPressed){
+      armMotor.disable();
+    }
+    if (bPressed) {
+      armMotor.setRotorPosition(0);
+    }
+    if (xPressed) {
+      setWristAngle(10, armRotations);
+    }
+    if(yPressed) {
+      setWristAngle(50, armRotations);
+    }
+  }
 }

From 063a73a00d3a8aa90c1d2acc16dc1160d952460a Mon Sep 17 00:00:00 2001
From: Simon <87281874+simonp17@users.noreply.github.com>
Date: Fri, 4 Aug 2023 10:46:44 -0700
Subject: [PATCH 2/2] Stop wrist when it reaches the right angle

---
 src/main/java/frc/robot/Robot.java | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index aaac108..80ca6f0 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -33,11 +33,19 @@ public Robot() {
   }
 
   public void setWristAngle(double degrees, double currentRotation) {
-    if (Rotation2d.fromDegrees(degrees).getRotations()<currentRotation) {
+    double setDegrees = Rotation2d.fromDegrees(degrees).getRotations();
+
+    if (setDegrees<currentRotation) {
       armMotor.setControl(armVoltage.withOutput(0.2));
-    } else if (Rotation2d.fromDegrees(degrees).getRotations()>currentRotation) {
+      if(setDegrees==currentRotation){
+        armMotor.setControl(armVoltage.withOutput(0));
+      }
+    } else if (setDegrees>currentRotation) {
       armMotor.setControl(armVoltage.withOutput(-0.2));
-    } else if (Rotation2d.fromDegrees(degrees).getRotations()== currentRotation) {
+      if(setDegrees==currentRotation){
+        armMotor.setControl(armVoltage.withOutput(0));
+      }
+    } else if (setDegrees== currentRotation) {
       armMotor.setControl(armVoltage.withOutput(0));
     }
   }