diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e06d69c..3508a8d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -110,6 +110,8 @@ public final class Constants { public static final double MAX_ARM_LEN = 2; public static final double ARM_HEIGHT = 1; public static final double CURVE_POWER = 2; + + public static final double TELE_TICKS_PER_SECOND = (-5); public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 3b5165e..513be03 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -46,6 +46,14 @@ public class Arm extends SubsystemBase { this(pivot, tele, encoder, false); } + public void setRotVel(double vel) { + m_pivot.set(ControlMode.Velocity, vel); + } + + public void setTeleVel(double vel) { + m_tele.set(ControlMode.Velocity, vel); + } + public void armSetRotation(double rot) { if (rot > 1 || rot < 0) return; // Move arm code @@ -71,6 +79,11 @@ public class Arm extends SubsystemBase { (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); } + public double getArmRotation() { + return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) / + (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + } + public void runPivotTele(double pivot, double tele) { var rot = 0;