Change subsystems to singletons

ftc13100 · Dec 12, 2024 · 0f3cbcc · 0f3cbcc
1 parent ade4fe1
commit 0f3cbcc
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 57 deletions.
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
@@ -4,31 +4,19 @@ 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.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.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem
 
 @TeleOp
 class MainTeleOp : CommandOpMode() {
-    private lateinit var elevatorLeft: Motor
-    private lateinit var elevatorRight: Motor
-    private lateinit var armLeft: Motor
-    private lateinit var armRight: Motor
-    //private lateinit var intake: CRServo
-
-    private lateinit var slidesSubsystem: OpenSlidesSubsystem
-    private lateinit var armSubsystem: OpenArmSubsystem
-    private lateinit var intakeSubsystem: IntakeSubsystem
-
     private lateinit var spinUpCommand: SpinUpCommand
     private lateinit var spinDownCommand: SpinDownCommand
     private lateinit var armUpCommand: OpenArmCommand
@@ -37,31 +25,22 @@ class MainTeleOp : CommandOpMode() {
     private lateinit var intakeCommand: IntakeCommand
     private lateinit var outtakeCommand: IntakeCommand
 
-
     private lateinit var driver: GamepadEx
     private lateinit var operator: GamepadEx
 
     override fun initialize() {
         driver = GamepadEx(gamepad1)
         operator = GamepadEx(gamepad2)
 
-        elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_435)
-        elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_435)
-
-        armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435)
-        armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435)
-
-        //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
-
-        slidesSubsystem = OpenSlidesSubsystem(elevatorRight, elevatorLeft)
-        armSubsystem = OpenArmSubsystem(armRight, armLeft)
+        OpenSlidesSubsystem.initialize(hardwareMap)
+        OpenArmSubsystem.initialize(hardwareMap)
         DriveSubsystem.initialize(hardwareMap)
-        //intakeSubsystem = IntakeSubsystem(intake)
+        IntakeSubsystem.initialize(hardwareMap)
 
-        spinUpCommand = SpinUpCommand(slidesSubsystem)
-        spinDownCommand = SpinDownCommand(slidesSubsystem)
-        armUpCommand = OpenArmCommand(armSubsystem, true)
-        armDownCommand = OpenArmCommand(armSubsystem, false)
+        spinUpCommand = SpinUpCommand(OpenSlidesSubsystem)
+        spinDownCommand = SpinDownCommand(OpenSlidesSubsystem)
+        armUpCommand = OpenArmCommand(OpenArmSubsystem, true)
+        armDownCommand = OpenArmCommand(OpenArmSubsystem, false)
         //intakeCommand = IntakeCommand(true, intakeSubsystem)
         //outtakeCommand = IntakeCommand(false, intakeSubsystem)
         driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
@@ -75,11 +54,10 @@ class MainTeleOp : CommandOpMode() {
         //operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand)
        // operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand)
 
-
         DriveSubsystem.defaultCommand = driveCommand
 
         RunCommand({
-            telemetry.addData("Arm Position: ", armSubsystem.armAngle)
+            telemetry.addData("Arm Position: ", OpenArmSubsystem.armAngle)
             telemetry.update()
         }).perpetually().schedule()
     }

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt
@@ -5,24 +5,27 @@ import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import com.qualcomm.robotcore.hardware.HardwareMap
+import org.firstinspires.ftc.teamcode.constants.ArmConstants
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import kotlin.math.PI
+import kotlin.math.cos
 
 @Config
-class OpenArmSubsystem(
-// Here I am just declaring the motors and what they are called on our driver hub.
-        armRight : Motor,
-        armLeft : Motor,
+object OpenArmSubsystem : SubsystemBase() {
+    private lateinit var turnMotors: MotorGroup
 
-) : SubsystemBase() {
-
-//Here I am making a motor group, as the arm motors are going to work together to to turn the slides.
-
-    private val turnMotors = MotorGroup(armRight, armLeft)
     val armAngle: Double
         get() = turnMotors.positions[0] / GoBILDA.RPM_312.cpr * 2 * PI
 
-    init {
+    fun initialize(hardwareMap: HardwareMap) {
+        val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
+        val armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
+
         armLeft.inverted = true
+
+        turnMotors = MotorGroup(armRight, armLeft)
+
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
     }
@@ -38,11 +41,7 @@ class OpenArmSubsystem(
     }
 
     fun stop() {
-        turnMotors.set(kCos * Math.cos(armAngle))
+        turnMotors.set(ArmConstants.kCos.value * cos(armAngle))
     }
 
-    companion object {
-        @JvmField
-        var kCos = 0.5
-    }
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
@@ -2,10 +2,16 @@ package org.firstinspires.ftc.teamcode.subsystems.intake
 
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.CRServo
+import com.qualcomm.robotcore.hardware.HardwareMap
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+
+object IntakeSubsystem : SubsystemBase() {
+    private lateinit var intake: CRServo
+
+    fun initialize(hardwareMap: HardwareMap) {
+        intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName)
+    }
 
-class IntakeSubsystem(
-    private val intake: CRServo
-) : SubsystemBase() {
     fun intake() = intake.set(1.0)
 
     fun outtake() = intake.set(-0.5)

diff --git a/...e/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt b/...e/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt
@@ -3,20 +3,21 @@ package org.firstinspires.ftc.teamcode.subsystems.slides
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import com.qualcomm.robotcore.hardware.HardwareMap
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
 
-class OpenSlidesSubsystem(
-    //sets them as a private variable thats a motor (same name as in driver hub)
-    elevatorLeft: Motor,
-    elevatorRight: Motor,
-) : SubsystemBase() {
-    //makes a motor group bc you have to move them at the same time
-    private val elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
+object OpenSlidesSubsystem: SubsystemBase() {
+    private lateinit var elevatorMotors: MotorGroup
+
+    fun initialize(hardwareMap: HardwareMap) {
+        val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
 
-    init {
         elevatorLeft.inverted = true
+
+        elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
     }
 
-    // Functions for moving slides up and down, number being speed, 1 being 100%
     fun up() {
         elevatorMotors.set(1.0)
     }