From 645a8154d550b46efd00e083af8998f952f31ea5 Mon Sep 17 00:00:00 2001 From: shriyanreyya27 Date: Tue, 30 Jan 2024 08:07:33 -0500 Subject: [PATCH 1/2] added logging, position tracking, and constants for climber advantage scope simulation --- .../constants/subsystem/ClimberConstants.kt | 3 +++ .../team449/robot2024/subsystems/Climber.kt | 27 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt index c73b295..75d2f69 100644 --- a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt +++ b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt @@ -16,4 +16,7 @@ object ClimberConstants { const val KP = 1.0 const val KI = 0.0 const val KD = 0.0 + + const val MAX_SIM_POS = 1.5 + const val SIM_SPEED = 0.25 } \ No newline at end of file diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt index 35e7f44..c26761f 100644 --- a/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt +++ b/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt @@ -1,15 +1,20 @@ package frc.team449.robot2024.subsystems +import edu.wpi.first.math.MathUtil import edu.wpi.first.math.controller.PIDController import edu.wpi.first.util.sendable.SendableBuilder import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.PIDCommand import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.team449.robot2024.Robot +import frc.team449.robot2024.constants.RobotConstants import frc.team449.robot2024.constants.subsystem.ClimberConstants import frc.team449.system.encoder.NEOEncoder import frc.team449.system.motor.WrappedMotor import frc.team449.system.motor.createSparkMax +import kotlin.math.abs +import kotlin.math.sign + class Climber( private val robot: Robot, @@ -17,6 +22,7 @@ class Climber( private val leftMotor: WrappedMotor, private val controller: PIDController ) : SubsystemBase() { + private var simCurrentPos = 0.0 fun levelClimb() : Command { return PIDCommand( @@ -56,6 +62,27 @@ class Climber( builder.publishConstString("1.0", "Motor Voltages") builder.addDoubleProperty("1.1 Last Right Voltage", { rightMotor.lastVoltage }, null) builder.addDoubleProperty("1.2 Last Left Voltage", { leftMotor.lastVoltage }, null) + builder.publishConstString("2.0", "Advantage Scope 3D Pos") + builder.addDoubleArrayProperty("2.1 3D Position", + { + doubleArrayOf( + 0.0, + 0.0, + simCurrentPos, + 0.0, + 0.0, + 0.0 + ) + }, + null + ) + } + + override fun simulationPeriodic() { + super.simulationPeriodic() + + simCurrentPos += MathUtil.clamp(ClimberConstants.SIM_SPEED * RobotConstants.LOOP_TIME * sign(leftMotor.lastVoltage), 0.0, ClimberConstants.MAX_SIM_POS) + } companion object { From b6f70e23285325e4b6944fb2719e6b9f0fb7ffec Mon Sep 17 00:00:00 2001 From: shriyanreyya27 Date: Tue, 30 Jan 2024 08:17:25 -0500 Subject: [PATCH 2/2] clean build robot --- .../constants/subsystem/ClimberConstants.kt | 2 +- .../team449/robot2024/subsystems/Climber.kt | 43 +++++++++---------- 2 files changed, 21 insertions(+), 24 deletions(-) diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt index 75d2f69..326c1d2 100644 --- a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt +++ b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt @@ -19,4 +19,4 @@ object ClimberConstants { const val MAX_SIM_POS = 1.5 const val SIM_SPEED = 0.25 -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt index c26761f..7044f56 100644 --- a/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt +++ b/src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt @@ -12,10 +12,8 @@ import frc.team449.robot2024.constants.subsystem.ClimberConstants import frc.team449.system.encoder.NEOEncoder import frc.team449.system.motor.WrappedMotor import frc.team449.system.motor.createSparkMax -import kotlin.math.abs import kotlin.math.sign - class Climber( private val robot: Robot, private val rightMotor: WrappedMotor, @@ -24,26 +22,26 @@ class Climber( ) : SubsystemBase() { private var simCurrentPos = 0.0 - fun levelClimb() : Command { - return PIDCommand( - controller, - { robot.drive.roll.degrees }, - { 0.0 }, - { value -> - rightMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT + value) - leftMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT - value) - }, - this - ) - } - - fun extend(): Command { - return this.runOnce { - rightMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE) - leftMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE) - } + fun levelClimb(): Command { + return PIDCommand( + controller, + { robot.drive.roll.degrees }, + { 0.0 }, + { value -> + rightMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT + value) + leftMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT - value) + }, + this + ) + } + fun extend(): Command { + return this.runOnce { + rightMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE) + leftMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE) } + } + fun retract(): Command { return this.runOnce { rightMotor.setVoltage(ClimberConstants.RETRACT_VOLTAGE) @@ -63,7 +61,8 @@ class Climber( builder.addDoubleProperty("1.1 Last Right Voltage", { rightMotor.lastVoltage }, null) builder.addDoubleProperty("1.2 Last Left Voltage", { leftMotor.lastVoltage }, null) builder.publishConstString("2.0", "Advantage Scope 3D Pos") - builder.addDoubleArrayProperty("2.1 3D Position", + builder.addDoubleArrayProperty( + "2.1 3D Position", { doubleArrayOf( 0.0, @@ -82,7 +81,6 @@ class Climber( super.simulationPeriodic() simCurrentPos += MathUtil.clamp(ClimberConstants.SIM_SPEED * RobotConstants.LOOP_TIME * sign(leftMotor.lastVoltage), 0.0, ClimberConstants.MAX_SIM_POS) - } companion object { @@ -109,7 +107,6 @@ class Climber( currentLimit = ClimberConstants.CURRENT_LIM, ) - val controller = PIDController(ClimberConstants.KP, ClimberConstants.KI, ClimberConstants.KD) return Climber(robot, rightMotor, leftMotor, controller)