erm actually elevator

BitBucketsFRC4183 · Mar 7, 2025 · b30a4d9 · b30a4d9
1 parent ab1b4df
commit b30a4d9
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -14,7 +14,7 @@ public class ArmConstants {
     public static double kASim = 0;
 
     public static double kS = 0;
-    public static double kV = 1.5;
+    public static double kV = 2;
     public static double kG = 0;
     public static double kA = 0;
     public static double kP = 0;

diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -48,8 +48,8 @@ public class ElevatorConstants {
     public static double L3 = 1.04775;
     public static double L4 = 1.7018;
     public static double carriageMass = 20.0;
-    public static double maxHeight = Units.inchesToMeters(50);
-    public static double minHeight = 0;
+    public static double maxHeight = 0.571;
+    public static double minHeight = -0.340;
     public static double maxVelocity = 1;
     public static double maxAcceleration = 1;
 

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem/ArmSubsystem.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Unit;
+import edu.wpi.first.units.VelocityUnit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -15,7 +16,7 @@
 import frc.robot.constants.ArmConstants;
 import org.littletonrobotics.junction.Logger;
 
-import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.*;
 
 
 public class ArmSubsystem extends SubsystemBase {
@@ -48,10 +49,10 @@ public ArmSubsystem(ArmIO armIO, ArmEncoderIO armIOEncoder) {
         sysId =
                 new SysIdRoutine(
                         new SysIdRoutine.Config(
+                                Volts.of(0.1).per(Second),
+                                Volts.of(1.5),
                                 null,
-                                null,
-                                null,
-                                (state) -> Logger.recordOutput("ElevatorSubystem/SysIdState", state.toString())),
+                                (state) -> Logger.recordOutput("ArmSubsystem/SysIdState", state.toString())),
                         new SysIdRoutine.Mechanism(
                                 (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
 
@@ -75,10 +76,17 @@ public void setArmVoltage(double volts){
         double outputVoltage = volts;
         if (OperatorInput.mechanismLimitOverride.getAsBoolean()) {
             outputVoltage = volts;
-        } else if ((getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS) || (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS)) {
-            outputVoltage = outputVoltage * 0.1;
-        } else if ((getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(5)) || (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS - Units.degreesToRadians(5))) {
-            outputVoltage = outputVoltage * 0.333;
+
+
+        } else if ((getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS)) {
+            // if mechanism exceeds limit basically set it to zero
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.07;
+        } else if (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.07;
+        } else if (getCurrentAngle() <= ArmConstants.MIN_ANGLE_RADS + Units.degreesToRadians(15)) {
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.25;
+        } else if (getCurrentAngle() >= ArmConstants.MAX_ANGLE_RADS - Units.degreesToRadians(15)) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.25;
         }
 
         Logger.recordOutput("ArmSubsystem/outputVoltageAdjusted", outputVoltage);

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorSubsystem.java
@@ -95,10 +95,15 @@ public void setElevatorVoltage(double volts) {
         double outputVoltage = volts;
         if (OperatorInput.mechanismLimitOverride.getAsBoolean()) {
             outputVoltage = volts;
-        } else if ((getLoadHeight() <= ElevatorConstants.minHeight) || (getLoadHeight() >= ElevatorConstants.maxHeight)) {
-            outputVoltage = outputVoltage * 0.1;
-        } else if ((getLoadHeight() <= ElevatorConstants.minHeight + 0.1) || (getLoadHeight() >= ElevatorConstants.maxHeight - 0.1)) {
-            outputVoltage = outputVoltage * 0.333;
+        } else if ((getLoadHeight() <= ElevatorConstants.minHeight)) {
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : 0;
+        } else if (getLoadHeight() >= ElevatorConstants.maxHeight) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : 0;
+        } else if ((getLoadHeight() <= ElevatorConstants.minHeight + 0.05)) {
+            outputVoltage = Math.signum(outputVoltage) == 1 ? outputVoltage : outputVoltage * 0.333;
+        } else if (getLoadHeight() >= ElevatorConstants.maxHeight - 0.05) {
+            outputVoltage = Math.signum(outputVoltage) == -1 ? outputVoltage : outputVoltage * 0.333;
+
         }
         elevatorIO.setElevatorMotorVoltage(outputVoltage);
     }