new arm controls and fixed dumb voltage mistake

Team-4795 · Feb 6, 2025 · 129122a · 129122a
1 parent 3215530
commit 129122a
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 13 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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));
     }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -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) {
         }
@@ Expand Down @@