From ce9b76a5d8716a055e1ec22a7d2426152cca490a Mon Sep 17 00:00:00 2001
From: NeonCoal <52942449+NeonCoal@users.noreply.github.com>
Date: Tue, 9 Jan 2024 20:12:22 -0500
Subject: [PATCH] =?UTF-8?q?pseudocode=20written=20=F0=9F=92=AA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

based off TelescopingClimber.kt of 2022 robot
---
 .../TelescopingArm/TelescopingArm.kt          | 45 ++++++++++++++++++-
 1 file changed, 44 insertions(+), 1 deletion(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt
index 6e59d178..8c6e4066 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt
@@ -20,4 +20,47 @@ import edu.wpi.first.wpilibj2.boardmand.SubsystemBase
 import org.littletonrobotics.junction.Logger
 class TelescopingArm(val io: TelescopingClimberIO) : SubsystemBase() {
 
-}
\ No newline at end of file
+}
+
+Pseudocode:
+Class TelescopingArm
+    Define io as TelescopingClimberIO
+    Define inputs as TelescopingClimberIOInputs
+    Define loadedFeedForward as ElevatorFeedforward with load constants
+    Define noLoadFeedForward as ElevatorFeedForward with no-load constants
+    Define activelyHold as boolean, set initial value to false
+    Define PID constants (kP, kI, kD) with tunable numbers
+    Define desiredState as DesiredTelescopeStates
+    Define constraints as TrapezoidProfile.Constraints with max velocity and acceleration
+    Define leftSetpoint and rightSetpoint as TrapezoidProfile.State based on current positions and velocities
+
+    Method periodic()
+        Update inputs from IO
+        Log various inputs and states
+        If PID constants have changed, configure IO with new PID values
+
+    Define limit switch properties for left and right forward and reverse limits
+
+    Method setOpenLoop(leftPower, rightPower, useSoftLimits)
+        If using soft limits and limits are reached, set power to zero, else set provided power
+
+    Define currentPosition property
+        Return the greater of left or right position
+
+    Define currentState property
+        Determine current state based on the currentPosition within various ranges
+
+    Method setPosition(leftSetpoint, rightSetpoint, isUnderLoad)
+        Calculate acceleration for left and right
+        Update setpoints
+        Set position with appropriate feedforward calculation based on load
+
+    Method holdPosition(loaded)
+        Set position for left and right with feedforward calculation for zero velocity
+
+    Method zeroLeftEncoder()
+        Zero the left encoder via IO
+
+    Method zeroRightEncoder()
+        Zero the right encoder via IO
+End Class
\ No newline at end of file