From 859a77dfaccb47f15a6483042107c04979e48aec Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Sat, 13 Jan 2024 19:05:24 -0500 Subject: [PATCH] more constants --- .../robot2023/config/constants/Constants.kt | 6 +++ .../TelescopingArm/TelescopingArm.kt | 41 +++++++------------ .../TelescopingArm/TelescopingArmIOReal.kt | 9 ++-- 3 files changed, 26 insertions(+), 30 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4f1f2add..12a18762 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -134,4 +134,10 @@ object Constants { const val LED_CANDLE_ID = 61 const val LED_BLINKEN_ID = 1 } + + object TelescopingClimber { + const val SOLENOID_ID = 0 + const val R_ARM_ID = 0 + const val L_ARM_ID = 0 + } } 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 1e23a617..7bed2459 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -48,33 +48,25 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { override fun periodic() { io.updateInputs(inputs) - Logger.get().processInputs("TelescopingArm", inputs) - Logger.get().recordOutput("TelescopingArm/desiredState", desiredState.name) - Logger.get().recordOutput("TelescopingArm/currentState", currentState.name) - Logger.get() - .recordOutput( + Logger.processInputs("TelescopingArm", inputs) + Logger.recordOutput("TelescopingArm/desiredState", desiredState.name) + Logger.recordOutput("TelescopingArm/currentState", currentState.name) + Logger.recordOutput( "TelescopingArm/leftPositionSetpointInches", leftSetpoint.position.meters.inInches ) - Logger.get() - .recordOutput("TelescopingArm/leftVelocitySetpointMetersPerSec", leftSetpoint.velocity) - Logger.get() - .recordOutput( + Logger.recordOutput("TelescopingArm/leftVelocitySetpointMetersPerSec", leftSetpoint.velocity) + Logger.recordOutput( "TelescopingArm/rightPositionSetpointInches", rightSetpoint.position.meters.inInches ) - Logger.get() - .recordOutput( + Logger.recordOutput( "TelescopingArm/rightVelocitySetpointMetersPerSec", rightSetpoint.velocity ) - Logger.get() - .recordOutput("TelescopingArm/leftForwardLimitReached", leftForwardLimitReached) - Logger.get() - .recordOutput("TelescopingArm/leftReverseLimitReached", leftReverseLimitReached) - Logger.get() - .recordOutput("TelescopingArm/rightForwardLimitReached", rightForwardLimitReached) - Logger.get() - .recordOutput("TelescopingArm/rightReverseLimitReached", rightReverseLimitReached) + Logger.recordOutput("TelescopingArm/leftForwardLimitReached", leftForwardLimitReached) + Logger.recordOutput("TelescopingArm/leftReverseLimitReached", leftReverseLimitReached) + Logger.recordOutput("TelescopingArm/rightForwardLimitReached", rightForwardLimitReached) + Logger.recordOutput("TelescopingArm/rightReverseLimitReached", rightReverseLimitReached) if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { io.configPID(kP.value, kI.value, kD.value) @@ -216,8 +208,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { .volts ) - Logger.get() - .recordOutput( + Logger.recordOutput( "TelescopingArm/leftFeedForwardVolts", noLoadFeedForward.calculate( leftSetpoint.velocity, leftAccel.inMetersPerSecondPerSecond @@ -228,8 +219,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { noLoadFeedForward.calculate(rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond) .volts ) - Logger.get() - .recordOutput( + Logger.recordOutput( "TelescopingArm/rightFeedForwardVolts", noLoadFeedForward.calculate( rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond @@ -241,8 +231,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { loadedFeedForward.calculate(leftSetpoint.velocity, leftAccel.inMetersPerSecondPerSecond) .volts ) - Logger.get() - .recordOutput( + Logger.recordOutput( "TelescopingArm/leftFeedForwardVolts", loadedFeedForward.calculate( leftSetpoint.velocity, leftAccel.inMetersPerSecondPerSecond @@ -253,7 +242,7 @@ class TelescopingArm(val io: TelescopingArmIO) : SubsystemBase() { loadedFeedForward.calculate(rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond) .volts ) - Logger.get().recordOutput( + Logger.recordOutput( "TelescopingArm/rightFeedForwardVolts", loadedFeedForward.calculate( rightSetpoint.velocity, rightAccel.inMetersPerSecondPerSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt index b910cc72..83c9d1d5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArmIOReal.kt @@ -3,23 +3,24 @@ package com.team4099.robot2022.subsystems.climber import com.ctre.phoenix6.configs.TalonFXConfiguration import com.revrobotics.CANSparkMax import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.TelescopingArmConstants import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts -import com.team4099.robot2023.config.constants.TelescopingArmConstants import com.team4099.robot2023.subsystems.TelescopingArm.TelescopingArmIO import org.team4099.lib.units.base.Length import org.team4099.lib.units.sparkMaxLinearMechanismSensor object TelescopingArmIOReal : TelescopingArmIO { private val telescopingLeftArm: CANSparkMax = CANSparkMax( - TelescopingArmConstants.L_ARM_ID, + Constants.TelescopingArm.L_ARM_ID, CANSparkMaxLowLevel.MotorType.kBrushless ) private val telescopingRightArm: CANSparkMax = CANSparkMax( - TelescopingArmConstants.R_ARM_ID, + Constants.TelescopingArm.R_ARM_ID, CANSparkMaxLowLevel.MotorType.kBrushless ) @@ -27,7 +28,7 @@ object TelescopingArmIOReal : TelescopingArmIO { sparkMaxLinearMechanismSensor( telescopingLeftArm, TelescopingArmConstants.SENSOR_CPR, - TelescopingArmConstants.ARM_GEAR_RATIO, + TelescopingArmConstants.GEAR_RATIO, TelescopingArmConstants.LEFT_SPOOL_RADIUS * 2 )