Run to position

ftc13100 · Dec 14, 2024 · f1ea5db · f1ea5db
1 parent 6a4c647
commit f1ea5db
Show file tree

Hide file tree

Showing 14 changed files with 178 additions and 278 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt
@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode.commands.arm
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 
 class OpenArmCommand(
-    private val subsystem: OpenArmSubsystem,
+    private val subsystem: ArmSubsystem,
     private val turn: Boolean,
 ) : CommandBase() {
 

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
@@ -1,15 +1,15 @@
 package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 class SpinDownCommand(
-    private val subsystem: OpenElevatorSubsystem
+    private val subsystem: ElevatorSubsystem
 ) : CommandBase() {
 
 
     override fun execute() {
-        subsystem.down()
+        subsystem.spinDown()
     }
 
     override fun end(interrupted: Boolean) {

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
@@ -1,14 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 class SpinUpCommand(
-    private val subsystem: OpenElevatorSubsystem
+    private val subsystem: ElevatorSubsystem
 ) : CommandBase() {
 
     override fun execute() {
-        subsystem.up()
+        subsystem.spinUp()
     }
 
     override fun end(interrupted: Boolean) {

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -10,10 +10,10 @@ import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand
 import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand
 import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand
 import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
-import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @TeleOp
 class MainTeleOp : CommandOpMode() {
@@ -32,17 +32,17 @@ class MainTeleOp : CommandOpMode() {
         driver = GamepadEx(gamepad1)
         operator = GamepadEx(gamepad2)
 
-        OpenElevatorSubsystem.initialize(hardwareMap)
-        OpenArmSubsystem.initialize(hardwareMap)
+        ElevatorSubsystem.initialize(hardwareMap)
+        ArmSubsystem.initialize(hardwareMap)
         DriveSubsystem.initialize(hardwareMap)
         IntakeSubsystem.initialize(hardwareMap)
 
-        spinUpCommand = SpinUpCommand(OpenElevatorSubsystem)
-        spinDownCommand = SpinDownCommand(OpenElevatorSubsystem)
-        armUpCommand = OpenArmCommand(OpenArmSubsystem, true)
-        armDownCommand = OpenArmCommand(OpenArmSubsystem, false)
-        //intakeCommand = IntakeCommand(true, intakeSubsystem)
-        //outtakeCommand = IntakeCommand(false, intakeSubsystem)
+        spinUpCommand = SpinUpCommand(ElevatorSubsystem)
+        spinDownCommand = SpinDownCommand(ElevatorSubsystem)
+        armUpCommand = OpenArmCommand(ArmSubsystem, true)
+        armDownCommand = OpenArmCommand(ArmSubsystem, false)
+//        intakeCommand = IntakeCommand(true, intakeSubsystem)
+//        outtakeCommand = IntakeCommand(false, intakeSubsystem)
         driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
 
         operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand)
@@ -57,7 +57,7 @@ class MainTeleOp : CommandOpMode() {
         DriveSubsystem.defaultCommand = driveCommand
 
         RunCommand({
-            telemetry.addData("Arm Position: ", OpenArmSubsystem.armAngle)
+            telemetry.addData("Arm Position: ", ArmSubsystem.angle)
             telemetry.update()
         }).perpetually().schedule()
     }

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
@@ -24,8 +24,8 @@ class IntakeTuner : CommandOpMode() {
     override fun initialize() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName)
-        intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName)
+        intake = hardwareMap[Servo::class.java, ControlBoard.INTAKE.deviceName]
+        intakeBelt = hardwareMap[Servo::class.java, ControlBoard.INTAKE_BELT.deviceName]
 
         IntakeSubsystem.initialize(hardwareMap)
         intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)

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
@@ -7,7 +7,6 @@ import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 
 @TeleOp
@@ -16,27 +15,22 @@ class ArmPIDTuner : CommandOpMode() {
     private lateinit var armLeft: Motor
     private lateinit var armRight: Motor
 
-    private lateinit var armSubsystem: ArmSubsystem
-
     override fun initialize() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_312)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_312)
-
-        armSubsystem = ArmSubsystem(armRight, armLeft)
+        ArmSubsystem.initialize(hardwareMap)
 
         RunCommand({
-            armSubsystem.setpoint = Math.toRadians(target)
+            ArmSubsystem.setpoint = Math.toRadians(target)
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Arm Angle: ", Math.toDegrees(armSubsystem.armAngle))
-            telemetry.addData("Setpoint: ", Math.toDegrees(armSubsystem.setpoint))
+            telemetry.addData("Arm Angle: ", Math.toDegrees(ArmSubsystem.angle))
+            telemetry.addData("Setpoint: ", Math.toDegrees(ArmSubsystem.setpoint))
             telemetry.update()
         }).perpetually().schedule()
 
-        register(armSubsystem)
+        register(ArmSubsystem)
     }
 
     companion object {

diff --git a/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt
@@ -2,31 +2,21 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.slides
 
 import com.acmerobotics.dashboard.FtcDashboard
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
-import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
 import com.qualcomm.robotcore.util.ElapsedTime
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @Autonomous
 class SlidesConstraintsTuner : LinearOpMode() {
-    private lateinit var leftMotor: Motor
-    private lateinit var rightMotor: Motor
-
-    private lateinit var elevator: ElevatorSubsystem
-
     private var maxVel = 0.0
     private var maxAccel = 0.0
 
     private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS)
     override fun runOpMode() {
         telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry)
+        ElevatorSubsystem.initialize(hardwareMap)
 
-        leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-        rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
-
-        elevator = ElevatorSubsystem(rightMotor, leftMotor) { 0.0 }
         waitForStart()
 
         var dt = 0L
@@ -35,16 +25,16 @@ class SlidesConstraintsTuner : LinearOpMode() {
         timer.reset()
         while (opModeIsActive()) {
             while (timer.seconds() < TIME_TO_RUN) {
-                elevator.spinUp()
+                ElevatorSubsystem.spinUp()
 
                 dt = timer.nanoseconds() - dt
-                dv = elevator.slideVelocity - dv / dt
+                dv = ElevatorSubsystem.velocity - dv / dt
 
-                maxVel = elevator.slideVelocity.coerceAtLeast(maxVel)
+                maxVel = ElevatorSubsystem.velocity.coerceAtLeast(maxVel)
                 maxAccel = dv.coerceAtLeast(maxAccel)
 
-                telemetry.addData("Velocity", elevator.slideVelocity)
-                telemetry.addData("Position", elevator.slidePos)
+                telemetry.addData("Velocity", ElevatorSubsystem.velocity)
+                telemetry.addData("Position", ElevatorSubsystem.position)
 
                 telemetry.addData("Max Velocity", maxVel)
                 telemetry.addData("Max Acceleration", maxAccel)

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
@@ -7,25 +7,14 @@ import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
-import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
 import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @TeleOp
 @Config
 class SlidesPIDTuner : CommandOpMode() {
-    private lateinit var slidesLeft: Motor
-    private lateinit var slidesRight: Motor
-
-    private lateinit var armLeft: Motor
-    private lateinit var armRight: Motor
-
-    private lateinit var armSubsystem: OpenArmSubsystem
-    private lateinit var slidesSubsystem: ElevatorSubsystem
-
     private lateinit var armUpCommand: OpenArmCommand
     private lateinit var armDownCommand: OpenArmCommand
 
@@ -36,34 +25,28 @@ class SlidesPIDTuner : CommandOpMode() {
 
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60)
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60)
-
-        slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150)
-        slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150)
-
-        OpenArmSubsystem.initialize(hardwareMap)
-        slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle)
+        ArmSubsystem.initialize(hardwareMap)
+        ElevatorSubsystem.initialize(hardwareMap)
 
-        armUpCommand = OpenArmCommand(OpenArmSubsystem, true)
-        armDownCommand = OpenArmCommand(OpenArmSubsystem, false)
+        armUpCommand = OpenArmCommand(ArmSubsystem, true)
+        armDownCommand = OpenArmCommand(ArmSubsystem, false)
 
         operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand)
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand)
 
         RunCommand({
-            slidesSubsystem.setpoint = target
+            ElevatorSubsystem.setpoint = target
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Slides Position", slidesSubsystem.slidePos)
+            telemetry.addData("Slides Position", ElevatorSubsystem.position)
             telemetry.addData("Setpoint", target)
-            telemetry.addData("arm angle", armSubsystem.armAngle)
-            telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity)
+            telemetry.addData("Arm Angle", ArmSubsystem.angle)
+            telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity)
             telemetry.update()
         }).perpetually().schedule()
 
-        register(slidesSubsystem)
+        register(ElevatorSubsystem)
     }
 
     companion object {

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt
@@ -28,12 +28,13 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 
 object TuningOpModes {
     // TODO: change this to TankDrive.class if you're using tank
-    val DRIVE_CLASS: Class<*> = DriveSubsystem::class.java
+    val DRIVE_CLASS = DriveSubsystem::class.java
 
-    const val GROUP: String = "quickstart"
-    const val DISABLED: Boolean = false
+    val GROUP = "quickstart"
+    val DISABLED = false
 
-    private fun metaForClass(cls: Class<out OpMode?>): OpModeMeta {
+    @JvmStatic
+    private fun metaForClass(cls: Class<out OpMode>): OpModeMeta {
         return OpModeMeta.Builder()
             .setName(cls.simpleName)
             .setGroup(GROUP)
@@ -42,6 +43,7 @@ object TuningOpModes {
     }
 
     @OpModeRegistrar
+    @JvmStatic
     fun register(manager: OpModeManager) {
         if (DISABLED) return
 
@@ -61,15 +63,15 @@ object TuningOpModes {
 
                         when (md.localizer) {
                             is DriveSubsystem.DriveLocalizer -> {
-                                val dl = md.localizer
+                                val dl = md.localizer as DriveSubsystem.DriveLocalizer
                                 leftEncs.add(dl.leftFront)
                                 leftEncs.add(dl.leftBack)
                                 rightEncs.add(dl.rightFront)
                                 rightEncs.add(dl.rightBack)
                             }
 
                             is ThreeDeadWheelLocalizer -> {
-                                val dl = md.localizer
+                                val dl = md.localizer as ThreeDeadWheelLocalizer
                                 parEncs.add(dl.par0)
                                 parEncs.add(dl.par1)
                                 perpEncs.add(dl.perp)