Nahum

.run/Build & Deploy Robot for Debugging.run.xml

@@ -65,4 +65,26 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" value="-PdebugMode=true" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="deploy" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Build & Deploy Robot.run.xml

@@ -65,4 +65,26 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="deploy" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Build & Run Simulate Java.run.xml

@@ -0,0 +1,24 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="Build &amp; Run Simulate Java" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="simulateJava" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
+</component>

.run/Build Robot.run.xml

@@ -65,4 +65,26 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="build" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Clean Build & Deploy Robot for Debugging.run.xml

@@ -68,4 +68,27 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Clean Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" value="-PdebugMode=true" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="clean" />
+          <option value="deploy" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Clean Build & Deploy Robot.run.xml

@@ -68,4 +68,27 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Clean Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="clean" />
+          <option value="deploy" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Clean Build & Run Simulate Java.run.xml

@@ -0,0 +1,25 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="Clean Build &amp; Run Simulate Java" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="clean" />
+          <option value="simulateJava" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
+</component>

.run/Clean Build Robot.run.xml

@@ -68,4 +68,27 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Clean Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="clean" />
+          <option value="build" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

.run/Clean.run.xml

@@ -65,4 +65,26 @@
     <ForceTestExec>false</ForceTestExec>
     <method v="2" />
   </configuration>
+  <configuration default="false" name="Clean" type="GradleRunConfiguration" factoryName="Gradle">
+    <ExternalSystemSettings>
+      <option name="executionName" />
+      <option name="externalProjectPath" value="$PROJECT_DIR$" />
+      <option name="externalSystemIdString" value="GRADLE" />
+      <option name="scriptParameters" />
+      <option name="taskDescriptions">
+        <list />
+      </option>
+      <option name="taskNames">
+        <list>
+          <option value="clean" />
+        </list>
+      </option>
+      <option name="vmOptions" />
+    </ExternalSystemSettings>
+    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
+    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
+    <DebugAllEnabled>false</DebugAllEnabled>
+    <ForceTestExec>false</ForceTestExec>
+    <method v="2" />
+  </configuration>
 </component>

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -21,4 +21,4 @@ public Command getAutonomousCommand() {
 
     private void configureBindings() {
     }
-}
+}

src/main/java/frc/trigon/robot/constants/RobotConstants.java

@@ -4,10 +4,10 @@
 
 public class RobotConstants {
     private static final String DEPLOY_PATH = Filesystem.getDeployDirectory().getPath() + "/";
+    public static final RobotType ROBOT_TYPE = RobotType.SIMULATION;
     public static final boolean IS_REPLAY = false;
     public static final double PERIODIC_TIME_SECONDS = 0.02;
-    public static final RobotType ROBOT_TYPE = RobotType.SIMULATION;
-
+
     public enum RobotType {
         KABLAMA("/media/sda1/logs/"),
         TRIHARD(DEPLOY_PATH + "logs/"),

src/main/java/frc/trigon/robot/subsystems/arm/Arm.java

@@ -0,0 +1,101 @@
+package frc.trigon.robot.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.trigon.robot.utilities.Conversions;
+import org.littletonrobotics.junction.Logger;
+
+public class Arm extends SubsystemBase {
+    private final static Arm INSTANCE = new Arm();
+    private final ArmIO armIO = ArmIO.generateIO();
+    private final ArmInputsAutoLogged armInputs = new ArmInputsAutoLogged();
+    private TrapezoidProfile
+            angleMotorProfile = null,
+            elevatorMotorProfile = null;
+    private double
+            lastAngleMotorProfileGenerationTime,
+            lastElevatorMotorProfileGenerationTime;
+
+    public static Arm getInstance() {
+        return INSTANCE;
+    }
+
+    private Arm() {
+    }
+
+    @Override
+    public void periodic() {
+        armIO.updateInputs(armInputs);
+        Logger.processInputs("Arm", armInputs);
+        updateMechanism();
+    }
+
+    boolean atAngle(Rotation2d targetAngle) {
+        return Math.abs(armInputs.anglePositionDegrees - targetAngle.getDegrees()) <= ArmConstants.ANGLE_TOLERANCE_DEGREES;
+    }
+
+    boolean atTargetElevatorPosition(double targetElevatorPositionMeters) {
+        return Math.abs(armInputs.elevatorPositionMeters - targetElevatorPositionMeters) <= ArmConstants.ELEVATOR_TOLERANCE_METERS;
+    }
+
+    boolean isElevatorOpening(double targetElevatorPositionMeters) {
+        return armInputs.elevatorPositionMeters < targetElevatorPositionMeters;
+    }
+
+    void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) {
+        angleMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, speedPercentage),
+                new TrapezoidProfile.State(targetAngle.getDegrees(), 0),
+                new TrapezoidProfile.State(armInputs.anglePositionDegrees, armInputs.angleVelocityDegreesPerSecond)
+        );
+
+        lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    void generateElevatorMotorProfile(double targetElevatorPositionMeters, double speedPercentage) {
+        elevatorMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, speedPercentage),
+                new TrapezoidProfile.State(targetElevatorPositionMeters, 0),
+                new TrapezoidProfile.State(armInputs.elevatorPositionMeters, armInputs.elevatorVelocityMetersPerSecond)
+        );
+
+        lastElevatorMotorProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    void setTargetAngleFromProfile() {
+        if (angleMotorProfile == null) {
+            armIO.stopAngleMotors();
+            return;
+        }
+
+        TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime());
+        ArmConstants.TARGET_POSITION_LIGAMENT.setAngle(targetState.position);
+        armIO.setTargetAngleState(targetState);
+    }
+
+    void setTargetElevatorPositionFromProfile() {
+        if (elevatorMotorProfile == null) {
+            armIO.stopElevatorMotors();
+            return;
+        }
+
+        TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime());
+        ArmConstants.TARGET_POSITION_LIGAMENT.setLength(targetState.position + ArmConstants.RETRACTED_ARM_LENGTH_METERS);
+        armIO.setTargetElevatorState(targetState);
+    }
+
+    private double getAngleMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime;
+    }
+
+    private double getElevatorMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastElevatorMotorProfileGenerationTime;
+    }
+    private void updateMechanism() {
+        ArmConstants.ARM_LIGAMENT.setLength(armInputs.elevatorPositionMeters + ArmConstants.RETRACTED_ARM_LENGTH_METERS);
+        ArmConstants.ARM_LIGAMENT.setAngle(armInputs.anglePositionDegrees);
+        Logger.recordOutput("ArmMechanism", ArmConstants.ARM_MECHANISM);
+    }
+}