arm elevator

awtybots · Feb 19, 2024 · 69f4a79 · 69f4a79
1 parent 73b30b5
commit 69f4a79
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 115 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -128,7 +128,7 @@ public static final class ArmElevatorConstants {
     public static final int kCurrentLimit = 30;
 
     public static final int initialExtension = 0; // TODO TUNE
-    public static final int maxExtension = 1; // TODO TUNE
+    public static final int maxExtension = 1000; // TODO TUNE
 
     public static final double armElevatorConversion = 1; // TODO TUNE
 
@@ -138,8 +138,8 @@ public static final class ArmElevatorConstants {
     public static final double kD = 0.0; // TODO TUNE
 
     // Arm Elevator Feedforward characterization constants
-    public static final double ks = 0.10; // TODO TUNE
-    public static final double kv = 0.05; // TODO TUNE
+    public static final double ks = 0; // TODO TUNE
+    public static final double kv = 0; // TODO TUNE
 
     // public static final double ArmThreshold = 0.1; // TODO TUNE
 

diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java
@@ -73,14 +73,18 @@ public void stop() {
     io.stop();
   }
 
-  /** Returns the current velocity in RPM. */
-  @AutoLogOutput
-  public double getVelocityRPM() {
-    return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
+  @AutoLogOutput(key = "ArmElevator/TargetDistance")
+  public double getTargetDistance() {
+    return inputs.targetDistance;
   }
 
-  /** Returns the current velocity in radians per second. */
-  public double getCharacterizationVelocity() {
-    return inputs.velocityRadPerSec;
+  // get the current position of the elevator
+  @AutoLogOutput(key = "ArmElevator/PositionMeters")
+  public double getPositionMeters() {
+    return inputs.positionMeters;
+  }
+
+  public void setTargetDistance(double distanceMeters) {
+    io.setTargetDistance(distanceMeters);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java
@@ -18,10 +18,9 @@
 public interface ArmElevatorIO {
   @AutoLog
   public static class ArmElevatorIOInputs {
-    public double position = 0.0;
-    public double targetPosition = 0.0;
-    public double appliedVolts = 0.0;
-    public double velocityRadPerSec = 0.0;
+    public double positionMeters = 0.0;
+    public double velocityMetersPerSec = 0.0;
+    public double targetDistance = 0.0;
     public double[] currentAmps = new double[] {};
   }
 
@@ -39,4 +38,8 @@ public default void stop() {}
 
   /** Set velocity PID constants. */
   public default void configurePID(double kP, double kI, double kD) {}
+
+  public default void setTargetDistance(double distance) {}
+
+
 }
diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java
@@ -40,8 +40,8 @@ public void updateInputs(ArmElevatorIOInputs inputs) {
 
     sim.update(0.02);
 
-    inputs.position = 0.0;
-    inputs.appliedVolts = appliedVolts;
+    inputs.targetDistance = 0.0;
+    inputs.positionMeters = sim.getPositionMeters();
     inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()};
   }
 

diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java
@@ -18,17 +18,19 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkPIDController.ArbFFUnits;
-import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.ArmElevatorConstants;
 
 /**
  * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
  * "CANSparkFlex".
  */
 public class ArmElevatorIOSparkMax implements ArmElevatorIO {
-  private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal
+  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_CIRCUMFERENCE =
+      Math.PI * PULLEY_DIAMETER; // pulley circumference
 
-  public double targetPosition = 0.0;
+  private double targetDistance = 0.0;
 
   private final CANSparkMax motor =
       new CANSparkMax(ArmElevatorConstants.kArmElevatorMotorId, MotorType.kBrushless);
@@ -53,10 +55,12 @@ public ArmElevatorIOSparkMax() {
 
   @Override
   public void updateInputs(ArmElevatorIOInputs inputs) {
-    inputs.position = encoder.getPosition() / GEAR_RATIO;
-    inputs.targetPosition = targetPosition;
-    inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage();
+    inputs.positionMeters = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
+    inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
+    inputs.targetDistance = targetDistance;
     inputs.currentAmps = new double[] {motor.getOutputCurrent()};
+
+    System.out.println(encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE);
   }
 
   @Override
@@ -66,12 +70,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
@@ -86,4 +90,18 @@ public void configurePID(double kP, double kI, double kD) {
     pid.setD(kD, 0);
     pid.setFF(0, 0);
   }
+
+  @Override
+  public void setTargetDistance(double distanceInches) {
+    targetDistance = distanceMeters;
+    pid.setReference(
+        distanceMeters / PULLEY_CIRCUMFERENCE * GEAR_RATIO,
+        ControlType.kPosition,
+        0,
+        0,
+        ArbFFUnits.kVoltage);
+
+    // targetDistance = MathUtil.clamp(targetDistance, ArmConstants.minimumDistance,
+    // ArmConstants.maximumDistance);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java
@@ -59,7 +59,6 @@ public ClimberIOSparkMax() {
     leftMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit);
     rightMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit);
 
-
     // leftMotor.burnFlash();
     // rightMotor.burnFlash();
   }

diff --git a/src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/BottomFlywheelIOSparkMax.java