beans

src/main/java/frc4388/robot/Constants.java

@@ -23,7 +23,11 @@ public final class Constants {
 
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
-    
+    public static final class ArmConstants {
+        public static final int minArmLen = 0;
+        public static final int maxArmLen = 1;
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+    }
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;
 

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -1,7 +1,39 @@
-public class Arm {
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import frc4388.robot.Constants.ArmConstants;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Arm extends SubsystemBase {
+    private WPI_TalonFX m_armmotor;
+    private boolean m_debug;
     // Moves arm to distence [dist] then returns new ang
-    public int funtion moveArm(int dist) {
+    public Arm(WPI_TalonFX armmotor, boolean debug) {
+        armmotor.configFactoryDefault();
+        m_armmotor = armmotor;
+    }
+
+    public Arm(WPI_TalonFX armmoter) {
+        this(armmoter, false);
+    }
+
+    public void armSetLength(double len) {
+        if (len > 1 || len < 0) return;
         // Move arm code
-        return newDist;
+        m_armmotor.set(ControlMode.Position, len * (ArmConstants.maxArmLen - ArmConstants.minArmLen) +
+            ArmConstants.minArmLen);
+    }
+
+    public double getArmLength() {
+        return (m_armmotor.getSelectedSensorPosition() - ArmConstants.minArmLen) /
+            (ArmConstants.maxArmLen - ArmConstants.minArmLen);
+    }
+
+    @Override
+    public void periodic() {
+        if (m_debug)
+            SmartDashboard.putNumber("Arm Motor", m_armmotor.getSelectedSensorPosition());
     }
 }