Merge pull request #1 from blair-robot-project/climber

Climber subsystem and constants

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt

@@ -0,0 +1,19 @@
+package frc.team449.robot2024.constants.subsystem
+
+object ClimberConstants {
+  const val RIGHT_ID = 31
+  const val LEFT_ID = 30
+
+  const val RIGHT_INVERTED = true
+  const val LEFT_INVERTED = true
+
+  const val CURRENT_LIM = 15
+
+  const val RETRACT_VOLTAGE = -8.0
+  const val EXTEND_VOLTAGE = 3.0
+
+  const val DEFAULT_PID_RETRACT = -7.0
+  const val KP = 1.0
+  const val KI = 0.0
+  const val KD = 0.0
+}

src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt

@@ -0,0 +1,91 @@
+package frc.team449.robot2024.subsystems
+
+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.subsystem.ClimberConstants
+import frc.team449.system.encoder.NEOEncoder
+import frc.team449.system.motor.WrappedMotor
+import frc.team449.system.motor.createSparkMax
+
+class Climber(
+  private val robot: Robot,
+  private val rightMotor: WrappedMotor,
+  private val leftMotor: WrappedMotor,
+  private val controller: PIDController
+) : SubsystemBase() {
+
+    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)
+      leftMotor.setVoltage(ClimberConstants.RETRACT_VOLTAGE)
+    }
+  }
+
+  fun stop(): Command {
+    return this.runOnce {
+      rightMotor.stopMotor()
+      leftMotor.stopMotor()
+    }
+  }
+
+  override fun initSendable(builder: SendableBuilder) {
+    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)
+  }
+
+  companion object {
+    fun createClimber(robot: Robot): Climber {
+      val rightMotor = createSparkMax(
+        "ProtoUndertaker Motor",
+        ClimberConstants.RIGHT_ID,
+        NEOEncoder.creator(
+          1.0,
+          1.0
+        ),
+        inverted = ClimberConstants.RIGHT_INVERTED,
+        currentLimit = ClimberConstants.CURRENT_LIM,
+      )
+
+      val leftMotor = createSparkMax(
+        "ProtoUndertaker Motor",
+        ClimberConstants.LEFT_ID,
+        NEOEncoder.creator(
+          1.0,
+          1.0
+        ),
+        inverted = ClimberConstants.LEFT_INVERTED,
+        currentLimit = ClimberConstants.CURRENT_LIM,
+      )
+
+
+      val controller = PIDController(ClimberConstants.KP, ClimberConstants.KI, ClimberConstants.KD)
+
+      return Climber(robot, rightMotor, leftMotor, controller)
+    }
+  }
+}