working armelevator

awtybots · Feb 20, 2024 · 70dbca8 · 70dbca8
1 parent 8e6f6b3
commit 70dbca8
Show file tree

Hide file tree

Showing 7 changed files with 29 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -131,10 +131,10 @@ public static final class ArmElevatorConstants {
     public static double minExtension = 0;
     public static double maxExtension = 16; // Inches
 
-    // public static final double armElevatorConversion = 1; 
+    // public static final double armElevatorConversion = 1;
 
     // ArmElevator PID constants
-    public static final double kP = 0.1; // TODO TUNE
+    public static final double kP = 1; // TODO TUNE
     public static final double kI = 0.0; // TODO TUNE
     public static final double kD = 0.0; // TODO TUNE
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -222,7 +222,10 @@ private void configureButtonBindings() {
         WristCommands.joystickDrive(sWrist, () -> operatorController.getLeftY()));
 
     sArmElevator.setDefaultCommand(
-        ArmElevatorCommands.triggerDrive(sArmElevator, () -> operatorController.getLeftTriggerAxis(), () -> operatorController.getRightTriggerAxis()));
+        ArmElevatorCommands.triggerDrive(
+            sArmElevator,
+            () -> operatorController.getLeftTriggerAxis(),
+            () -> operatorController.getRightTriggerAxis()));
 
     operatorController
         .a()

diff --git a/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java b/src/main/java/frc/robot/commands/ControlCommands/ArmElevatorCommands.java
@@ -27,14 +27,15 @@ private ArmElevatorCommands() {}
 
   /** Wrist command using one axis of a joystick (controlling wrist velocity). */
   public static Command triggerDrive(
-    ArmElevator armElevator, 
-    DoubleSupplier leftTriggerSupplier,
-    DoubleSupplier rightTriggerSupplier) {
+      ArmElevator armElevator,
+      DoubleSupplier leftTriggerSupplier,
+      DoubleSupplier rightTriggerSupplier) {
     return Commands.run(
         () -> {
           // Apply deadband (i.e. min DEADBAND max 1.0)
-          double stickMagnitude = MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND) 
-              + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND);
+          double stickMagnitude =
+              MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND)
+                  + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND);
 
           // Calcaulate new rotational velocity
           double rotationalVelocity = stickMagnitude * MAXINCHESPERSECOND;

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
@@ -22,7 +22,6 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.ArmConstants;
-import frc.robot.commands.ControlCommands.ArmCommands;
 
 /**
  * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with

diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java
@@ -31,6 +31,7 @@ public ArmElevator(ArmElevatorIO io) {
 
     // Switch constants based on mode (the physics simulator is treated as a
     // separate robot with different tuning)
+
     switch (EnvironmentalConstants.currentMode) {
       case REAL:
         io.configurePID(ArmElevatorConstants.kP, ArmElevatorConstants.kI, ArmElevatorConstants.kD);
@@ -69,9 +70,10 @@ public void runVolts(double volts) {
   // }
 
   public void runTargetVelocity(double velocityInchesPerSecond) {
-    io.setTargetDistance(inputs.targetDistance 
-      + 0.02 // TODO cycle time needed
-      * velocityInchesPerSecond);
+    io.setTargetDistance(
+        inputs.targetDistance
+            + 0.02 // TODO cycle time needed
+                * velocityInchesPerSecond);
   }
 
   /** Stops the ArmElevator. */

diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java
@@ -18,7 +18,6 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkPIDController.ArbFFUnits;
-
 import edu.wpi.first.math.MathUtil;
 import frc.robot.Constants.ArmElevatorConstants;
 
@@ -28,7 +27,7 @@
  */
 public class ArmElevatorIOSparkMax implements ArmElevatorIO {
   private static final double GEAR_RATIO = 40.0; // gear ratio
-  private static final double PULLEY_DIAMETER = 2; // pulley diameter in meters
+  private static final double PULLEY_DIAMETER = 2.0; // pulley diameter in meters
   private static final double PULLEY_CIRCUMFERENCE =
       Math.PI * PULLEY_DIAMETER; // pulley circumference
 
@@ -50,7 +49,7 @@ public ArmElevatorIOSparkMax() {
 
     motor.setSmartCurrentLimit(ArmElevatorConstants.kCurrentLimit);
 
-    pid.setOutputRange(0, ArmElevatorConstants.maxExtension);
+    // pid.setOutputRange(0, ArmElevatorConstants.maxExtension);
 
     // motor.burnFlash();
   }
@@ -61,7 +60,6 @@ public void updateInputs(ArmElevatorIOInputs inputs) {
     inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
     inputs.targetDistance = targetDistance;
     inputs.currentAmps = new double[] {motor.getOutputCurrent()};
-
   }
 
   @Override
@@ -71,12 +69,12 @@ public void setVoltage(double volts) {
 
   // @Override
   // public void setVelocity(double velocityRadPerSec, double ffVolts) {
-    // pid.setReference(
-    //     Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
-    //     ControlType.kVelocity,
-    //     0,
-    //     ffVolts,
-    //     ArbFFUnits.kVoltage);
+  // pid.setReference(
+  //     Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
+  //     ControlType.kVelocity,
+  //     0,
+  //     ffVolts,
+  //     ArbFFUnits.kVoltage);
   // }
 
   @Override
@@ -94,7 +92,9 @@ public void configurePID(double kP, double kI, double kD) {
 
   @Override
   public void setTargetDistance(double distanceInches) {
-    targetDistance = MathUtil.clamp(distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension);
+    targetDistance =
+        MathUtil.clamp(
+            distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension);
     pid.setReference(
         targetDistance / PULLEY_CIRCUMFERENCE * GEAR_RATIO,
         ControlType.kPosition,

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java
@@ -41,7 +41,7 @@ public WristIOSparkMax() {
 
     motor.setCANTimeout(250);
 
-    encoder.setPosition(WristConstants.initialAngle);
+    // encoder.setPosition(WristConstants.initialAngle);
 
     // motor.enableVoltageCompensation(12.0);