Merge branch 'master' into swerve-drive

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

@@ -121,7 +121,13 @@ public final class Constants {
   public static final class GyroConstants {
     public static final int ID = 14; // TODO: find the actual ID
   }
-    
+  
+  public static final class ArmConstants {
+      public static final int MIN_ARM_LEN = 0;
+      public static final int MAX_ARM_LEN = 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

@@ -0,0 +1,39 @@
+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 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
+        m_armmotor.set(ControlMode.Position, len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) +
+            ArmConstants.MIN_ARM_LEN);
+    }
+
+    public double getArmLength() {
+        return (m_armmotor.getSelectedSensorPosition() - ArmConstants.MIN_ARM_LEN) /
+            (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN);
+    }
+
+    @Override
+    public void periodic() {
+        if (m_debug)
+            SmartDashboard.putNumber("Arm Motor", m_armmotor.getSelectedSensorPosition());
+    }
+}

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

@@ -0,0 +1,23 @@
+package frc4388.robot.subsystems;
+import edu.wpi.first.wpilibj.PWM;
+
+public class Claw {
+	private PWM m_clawMotor;
+	private boolean m_open = false;
+
+    // Opens claw
+	public Claw(PWM m_clawMotor) {
+		 this.m_clawMotor = m_clawMotor;
+		 setClaw(true);
+	}
+
+    public void setClaw(boolean open) {
+        // Open claw
+		m_open = open;
+		m_clawMotor.setRaw(open ? 0 : 2000);
+    }
+
+	public boolean isClawOpen() {
+		return m_open;
+	}
+}