make arm hold while tuning + 0 joystick input

team4909 · Mar 28, 2024 · 017d723 · 017d723
1 parent 1cb4870
commit 017d723
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -210,7 +210,10 @@ public Command aimElbowForTuning(DoubleSupplier driveEffort) {
     return this.run(
             () -> {
               m_io.setWristRotations(Units.radiansToRotations(ArmSetpoints.kStowed.wristAngle));
-              m_io.setElbowVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 3.0);
+              final double driveVoltage =
+                  MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 3.0;
+              if (driveVoltage == 0.0) m_io.setElbowRotations(m_inputs.elbowPositionRot);
+              else m_io.setElbowVoltage(driveVoltage);
             })
         .finallyDo(() -> holdSetpoint().schedule());
   }
@@ -219,7 +222,10 @@ public Command aimWristForTuning(DoubleSupplier driveEffort) {
     return this.run(
             () -> {
               m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle));
-              m_io.setWristVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0);
+              final double driveVoltage =
+                  MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0;
+              if (driveVoltage == 0.0) m_io.setWristRotations(m_inputs.wristPositionRot);
+              else m_io.setWristVoltage(driveVoltage);
             })
         .finallyDo(() -> holdSetpoint().schedule());
   }

diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java
@@ -65,8 +65,6 @@ public ArmIOTalonFX() {
     elbowLeftMotorConfig.Slot0.kG = 0.37766;
     elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5;
     elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0;
-    elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kV = 16.0;
-    elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kA = 6.0;
     elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0;
     elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0;
     m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0);