Elevator and Arm Commands

ftc13100 · Dec 16, 2024 · 0b02047 · 0b02047
1 parent fc7777d
commit 0b02047
Show file tree

Hide file tree

Showing 8 changed files with 37 additions and 8 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt
@@ -10,4 +10,10 @@ class ArmCommand(
     override fun initialize() {
         subsystem.setpoint = setpoint
     }
+
+    override fun execute() = subsystem.operateArm()
+
+    override fun isFinished(): Boolean = subsystem.isBusy
+
+    override fun end(interrupted: Boolean) = subsystem.stop()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.commands.elevator
+
+import com.arcrobotics.ftclib.command.CommandBase
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+class ElevatorCommand(
+    private val subsystem: ElevatorSubsystem,
+    private val setpoint: Double
+) : CommandBase() {
+    override fun initialize() {
+        subsystem.setpoint = setpoint
+    }
+
+    override fun execute() = subsystem.operateElevator()
+
+    override fun isFinished(): Boolean = subsystem.isBusy
+
+    override fun end(interrupted: Boolean) = subsystem.stop()
+}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt
@@ -6,8 +6,6 @@ import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 class SpinDownCommand(
     private val subsystem: ElevatorSubsystem
 ) : CommandBase() {
-
-
     override fun execute() {
         subsystem.spinDown()
     }

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt
@@ -6,7 +6,6 @@ import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 class SpinUpCommand(
     private val subsystem: ElevatorSubsystem
 ) : CommandBase() {
-
     override fun execute() {
         subsystem.spinUp()
     }

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
@@ -22,6 +22,7 @@ class ArmPIDTuner : CommandOpMode() {
 
         RunCommand({
             ArmSubsystem.setpoint = Math.toRadians(target)
+            ArmSubsystem.operateArm()
         }).perpetually().schedule()
 
         RunCommand({

diff --git a/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
@@ -36,13 +36,16 @@ class SlidesPIDTuner : CommandOpMode() {
 
         RunCommand({
             ElevatorSubsystem.setpoint = target
+            ElevatorSubsystem.operateElevator()
         }).perpetually().schedule()
 
         RunCommand({
             telemetry.addData("Slides Position", ElevatorSubsystem.position)
             telemetry.addData("Setpoint", target)
+
             telemetry.addData("Arm Angle", ArmSubsystem.angle)
             telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity)
+
             telemetry.update()
         }).perpetually().schedule()
 

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
@@ -34,6 +34,8 @@ object ArmSubsystem : SubsystemBase() {
             ArmConstants.MAX_ACCELERATION.value
         )
     )
+    val isBusy
+        get() = controller.atGoal()
 
     var setpoint = controller.goal.position
         set(value) {
@@ -43,7 +45,7 @@ object ArmSubsystem : SubsystemBase() {
 
     private var enabled = true
 
-    fun initialize(hardwareMap: HardwareMap) : ArmSubsystem {
+    fun initialize(hardwareMap: HardwareMap) {
         val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
         val armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
 
@@ -53,8 +55,6 @@ object ArmSubsystem : SubsystemBase() {
 
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
-
-        return this
     }
 
 
@@ -70,7 +70,7 @@ object ArmSubsystem : SubsystemBase() {
         turnMotors.set(0.0)
     }
 
-    override fun periodic() {
+    fun operateArm() {
         val output = controller.calculate(angle) + feedforward.calculate(angle, velocity)
 
         if (enabled)

diff --git a/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
@@ -38,6 +38,9 @@ object ElevatorSubsystem: SubsystemBase() {
             field = clamped
         }
 
+    val isBusy
+        get() = controller.atGoal()
+
     val position: Double
         get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
 
@@ -69,7 +72,7 @@ object ElevatorSubsystem: SubsystemBase() {
         elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle))
     }
 
-    override fun periodic() {
+    fun operateElevator() {
         val output = controller.calculate(position) +
                 SlidesConstants.kG.value * sin(ArmSubsystem.angle)