From 6071b0e0be11590a186acdd291bb424a6358ace6 Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 1 Mar 2023 16:45:57 -0700 Subject: [PATCH] added velocity control --- src/main/java/frc4388/robot/Constants.java | 2 ++ src/main/java/frc4388/robot/subsystems/Arm.java | 13 +++++++++++++ 2 files changed, 15 insertions(+) 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;