From 37ca977331615fbe7c531b5a768411ae2c53469c Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 8 Feb 2024 17:46:56 -0800
Subject: [PATCH] Arm Movement remake

---
 src/main/java/frc/robot/Constants.java        |  4 +++-
 .../robot/commands/ArmJoystickControl.java    | 21 ++++++++++++-------
 src/main/java/frc/robot/subsystems/Arm.java   | 17 +++++++++++----
 3 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1a3f6aa..6b659bc 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -94,7 +94,7 @@ public static class SwerveDrivetrainConstants {
 
     public static class ArmConstants {
 
-        public static final double MAXIMUM_ARM_DEGREES = 3;
+        public static final double MAXIMUM_ARM_DEGREES = 1;
         public static final double MINIMUM_ARM_DEGREES = 0;
 
         public static final double ARM_AMP_SHOOTING_DEGREES = 0;
@@ -105,6 +105,8 @@ public static class ArmConstants {
         //public static final int LEFT_ENCODER_ID = 0;
         public static final int RIGHT_MOTOR_ID = 0;
         public static final int RIGHT_ENCODER_ID = 0;
+
+        public static final double DEGREES_PER_SECOND = 2.0;
     }
 }
 
diff --git a/src/main/java/frc/robot/commands/ArmJoystickControl.java b/src/main/java/frc/robot/commands/ArmJoystickControl.java
index 8bccc2c..1933559 100644
--- a/src/main/java/frc/robot/commands/ArmJoystickControl.java
+++ b/src/main/java/frc/robot/commands/ArmJoystickControl.java
@@ -1,11 +1,13 @@
 package frc.robot.commands;
 
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 
 import frc.robot.utils.OptionButton;
 import frc.robot.utils.OptionButton.ActivationMode;
-
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 
 public class ArmJoystickControl extends Command {
@@ -15,6 +17,8 @@ public class ArmJoystickControl extends Command {
     private final OptionButton raiseArmButton;
     private final OptionButton lowerArmButton;
 
+
+
     private double armPos;
 
     public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
@@ -22,8 +26,12 @@ public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
         this.joystick = joystick;
 
 
-        lowerArmButton = new OptionButton(joystick, 101, ActivationMode.HOLD);
-        raiseArmButton = new OptionButton(joystick, 202, ActivationMode.HOLD);
+        lowerArmButton = new OptionButton(joystick, 11, ActivationMode.HOLD);
+        raiseArmButton = new OptionButton(joystick, 12, ActivationMode.HOLD);
+
+        joystick.povUp().onTrue(Commands.run(arm::setArmToSpeakerPosition));
+        joystick.povRight().onTrue(Commands.run(arm::setArmToAmpPosition));
+        joystick.povDown().onTrue(Commands.run(arm::setArmToIntakePosition));
 
         addRequirements(arm);
     }
@@ -35,10 +43,9 @@ public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
     @Override
     public void execute() {
 
-        armPos += raiseArmButton.getStateAsInt();
-        armPos -= lowerArmButton.getStateAsInt();
-        
 
-        arm.setArmAngleDegrees(armPos);
+        //two buttons determining the raising and lowering of the arm
+        arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2bece67..ebc06f0 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -58,13 +58,20 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
 
     }
 
-    public void setArmAngleDegrees(double desiredDegree) {
-        //maximum should ALWAYS be a greater value then minimum
-        if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
-            armRotation2d = Rotation2d.fromDegrees(desiredDegree);
+    // public void setArmAngleDegrees(double desiredDegree) {
+    //     //maximum should ALWAYS be a greater value then minimum
+    //     if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
+    //         armRotation2d = Rotation2d.fromDegrees(desiredDegree);
+    //     }
+    // }
+
+     public void changeArmAngleDegreesBy(double desiredDegrees) {
+        if (armRotation2d.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > armRotation2d.getDegrees()) {
+            armRotation2d = Rotation2d.fromDegrees(armRotation2d.getDegrees() + desiredDegrees);
         }
     }
 
+
     public void setArmToAmpPosition() {
         armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
     }
@@ -84,6 +91,8 @@ public void setArmToIntakePosition() {
     public void periodic() {
         // This method will be called once per scheduler run
 
+        
+
         final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
         leftArmMotor.set(armSpeed);
         rightArmMotor.set(armSpeed);