Skip to content

Commit

Permalink
Added constants and removed depreciated library
Browse files Browse the repository at this point in the history
  • Loading branch information
@NeonCoal
NeonCoal committed Jan 13, 2024
1 parent 2d160fc commit eea5884
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 20 deletions.
53 changes: 48 additions & 5 deletions src/main/kotlin/com/team4099/robot2023/config/constants/TelescopingArmConstants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,58 @@ package com.team4099.robot2023.config.constants

import org.team4099.lib.units.base.Length
import org.team4099.lib.units.base.inches
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.perSecond

object TelescopingArmConstants {
val L_ARM_ID = 0
val R_ARM_ID = 0
const val SENSOR_CPR = 0
const val GEAR_RATIO = 0

val SENSOR_CPR = 0.0
val ARM_GEAR_RATIO = 0.0.inches
val LEFT_SPOOL_RADIUS = 0.volts
const val KP = 0.0
const val KI = 0.0
const val KD = 0.0
const val KFF = 0.0

val NO_LOAD_KS = 0.volts
val NO_LOAD_KG = 0.volts
val NO_LOAD_KV = 0.volts / 0.meters.perSecond
val NO_LOAD_KA = 0.volts / 0.meters.perSecond.perSecond

val LOAD_KS = 0.0.volts
val LOAD_KG = 0.volts
val LOAD_KV = 0.volts / 0.meters.perSecond
val LOAD_KA = 0.volts / 0.meters.perSecond.perSecond

val SPOOL_RADIUS = 0.inches
val LEFT_SPOOL_RADIUS = 0.inches
val RIGHT_SPOOL_RADIUS = 0.inches
val MAX_VELOCITY = 0.inches.perSecond
val MAX_ACCELERATION = 0.inches.perSecond.perSecond

const val BOTTOM_SAFETY_THRESHOLD = 0
const val TOP_SAFETY_THRESHOLD = 0

enum class DesiredTelescopeStates(val position: Length) {
START(0.0.inches),
MAX_RETRACT(0.inches),
MAX_EXTENSION(0.inches),
DUMMY(-Double.NEGATIVE_INFINITY.inches)
}

enum class ActualTelescopeStates(val correspondingDesiredState: DesiredTelescopeStates) {
START(DesiredTelescopeStates.START),
BETWEEN_START_AND_MAX_RETRACT(DesiredTelescopeStates.DUMMY),
MAX_RETRACT(DesiredTelescopeStates.MAX_RETRACT),
BETWEEN_MAX_RETRACT_AND_MAX_EXTENSION(DesiredTelescopeStates.DUMMY),
MAX_EXTENSION(DesiredTelescopeStates.MAX_EXTENSION),
}

val telescopingTolerance = 0.inches

val FORWARD_SOFT_LIMIT = 0.inches // old one was 24 inches
val SLOW_TELESCOPING_THRESHOLD = 0.inches
val REVERSE_SOFT_LIMIT = 0.inches

const val TAB = "Telescoping Climber"
}
30 changes: 15 additions & 15 deletions src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,32 +45,32 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() {
override fun periodic() {
io.updateInputs(inputs)

Logger.getInstance().processInputs("TelescopingArm", inputs)
Logger.getInstance().recordOutput("TelescopingArm/desiredState", desiredState.name)
Logger.getInstance().recordOutput("TelescopingArm/currentState", currentState.name)
Logger.getInstance()
Logger.get().processInputs("TelescopingArm", inputs)
Logger.get().recordOutput("TelescopingArm/desiredState", desiredState.name)
Logger.get().recordOutput("TelescopingArm/currentState", currentState.name)
Logger.get()
.recordOutput(
"TelescopingArm/leftPositionSetpointInches", leftSetpoint.position.meters.inInches
)
Logger.getInstance()
Logger.get()
.recordOutput("TelescopingArm/leftVelocitySetpointMetersPerSec", leftSetpoint.velocity)
Logger.getInstance()
Logger.get()
.recordOutput(
"TelescopingArm/rightPositionSetpointInches",
rightSetpoint.position.meters.inInches
)
Logger.getInstance()
Logger.get()
.recordOutput(
"TelescopingArm/rightVelocitySetpointMetersPerSec", rightSetpoint.velocity
)

Logger.getInstance()
Logger.get()
.recordOutput("TelescopingArm/leftForwardLimitReached", leftForwardLimitReached)
Logger.getInstance()
Logger.get()
.recordOutput("TelescopingArm/leftReverseLimitReached", leftReverseLimitReached)
Logger.getInstance()
Logger.get()
.recordOutput("TelescopingArm/rightForwardLimitReached", rightForwardLimitReached)
Logger.getInstance()
Logger.get()
.recordOutput("TelescopingArm/rightReverseLimitReached", rightReverseLimitReached)

if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
Expand Down Expand Up @@ -213,7 +213,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() {
.volts
)

Logger.getInstance()
Logger.get()
.recordOutput(
"TelescopingArm/leftFeedForwardVolts",
noLoadFeedForward.calculate(
Expand All @@ -225,7 +225,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() {
noLoadFeedForward.calculate(rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond)
.volts
)
Logger.getInstance()
Logger.get()
.recordOutput(
"TelescopingArm/rightFeedForwardVolts",
noLoadFeedForward.calculate(
Expand All @@ -238,7 +238,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() {
loadedFeedForward.calculate(leftSetpoint.velocity, leftAccel.inMetersPerSecondPerSecond)
.volts
)
Logger.getInstance()
Logger.get()
.recordOutput(
"TelescopingArm/leftFeedForwardVolts",
loadedFeedForward.calculate(
Expand All @@ -250,7 +250,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() {
loadedFeedForward.calculate(rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond)
.volts
)
Logger.getInstance().recordOutput(
Logger.get().recordOutput(
"TelescopingArm/rightFeedForwardVolts",
loadedFeedForward.calculate(
rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond
Expand Down

0 comments on commit eea5884

Please sign in to comment.