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