From 129122a7d7e376776c29cdbd93e27d7526c059db Mon Sep 17 00:00:00 2001
From: ozziexyz <ozzieuh@gmail.com>
Date: Thu, 6 Feb 2025 13:03:27 -0500
Subject: [PATCH] new arm controls and fixed dumb voltage mistake

---
 src/main/java/frc/robot/RobotContainer.java   | 36 +++++++++++++------
 .../java/frc/robot/subsystems/arm/Arm.java    |  1 +
 .../java/frc/robot/subsystems/arm/ArmIO.java  |  2 ++
 .../frc/robot/subsystems/arm/ArmIOReal.java   |  9 +++--
 4 files changed, 35 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2631126..16821fa 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -166,20 +166,34 @@ private void configureBindings() {
       );
 
 
-      OIConstants.driverController.leftTrigger().whileTrue(
-        Commands.startEnd(
-        () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), 
-        () -> Arm.getInstance().manualVoltage(0), 
-        Arm.getInstance()
-      ));
+      // OIConstants.driverController.leftTrigger().whileTrue(
+      //   Commands.startEnd(
+      //   () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), 
+      //   () -> Arm.getInstance().manualVoltage(0), 
+      //   Arm.getInstance()
+      // ));
+
 
+      // OIConstants.driverController.rightTrigger().whileTrue(
+      //   Commands.startEnd(
+      //   () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), 
+      //   () -> Arm.getInstance().manualVoltage(0), 
+      //   Arm.getInstance()
+      // ));
 
+      OIConstants.driverController.leftTrigger().whileTrue(
+        Commands.run(
+          () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * 6), 
+          Arm.getInstance()
+        )
+      );
+      
       OIConstants.driverController.rightTrigger().whileTrue(
-        Commands.startEnd(
-        () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getRightTriggerAxis() * -6), 
-        () -> Arm.getInstance().manualVoltage(0), 
-        Arm.getInstance()
-      ));
+        Commands.run(
+          () -> Arm.getInstance().manualVoltage(OIConstants.operatorController.getLeftTriggerAxis() * -6), 
+          Arm.getInstance()
+        )
+      );
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
index a8c6119..b831bae 100644
--- a/src/main/java/frc/robot/subsystems/arm/Arm.java
+++ b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -22,6 +22,7 @@ public class Arm extends SubsystemBase {
 
     private Arm(ArmIO io) {
         this.io = io;
+        setDefaultCommand(Commands.run(() -> io.hold()));
     }
 
     public double getAngle() {
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
index 0e25798..ba81db1 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
@@ -29,6 +29,8 @@ public default void setVoltage(double voltage) {
     public default void setGoal(double angle) {
     }
 
+    public default void hold() {}
+
     public default void setFFValues(double kS, double kG, double kV, double kA) {
 
     }
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java
index 73d6a92..a27baa3 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java
@@ -69,6 +69,12 @@ public void setFFValues(double kS, double kG, double kV, double kA) {
         ffmodel = new ArmFeedforward(kS, kG, kV);
     }
 
+    @Override
+    public void hold() {
+        double ffvolts = ffmodel.calculate(armEncoder.getPosition(), 0);
+        onboardController.setReference(ffvolts, ControlType.kVoltage);
+    }
+
     @Override
     public void resetAbsoluteEncoder() {
         armMotor.getEncoder().setPosition(0);
@@ -91,8 +97,7 @@ public void updateInputs(ArmIOInputs inputs) {
     }
 
     public void setVoltage(double voltage) {
-        double ffvolts = ffmodel.calculate(armEncoder.getPosition()  , 0);
-        onboardController.setReference(ffvolts, ControlType.kVoltage);
+        onboardController.setReference(voltage, ControlType.kVoltage);
         // armMotor.setVoltage(MathUtil.clamp(-voltage, -12, 12));
     }
 }