From 0f3cbcc1f9f76d6315a273da29703bb34563af84 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 12 Dec 2024 16:15:26 -0500 Subject: [PATCH] Change subsystems to singletons --- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 38 ++++--------------- .../subsystems/arm/OpenArmSubsystem.kt | 29 +++++++------- .../subsystems/intake/IntakeSubsystem.kt | 12 ++++-- .../subsystems/slides/OpenSlidesSubsystem.kt | 19 +++++----- 4 files changed, 41 insertions(+), 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 index 22701ee..4e68b02 100644 --- 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,14 +4,12 @@ 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 @@ -19,16 +17,6 @@ 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,7 +25,6 @@ class MainTeleOp : CommandOpMode() { private lateinit var intakeCommand: IntakeCommand private lateinit var outtakeCommand: IntakeCommand - private lateinit var driver: GamepadEx private lateinit var operator: GamepadEx @@ -45,23 +32,15 @@ class MainTeleOp : CommandOpMode() { 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 index 722f487..3bb696f 100644 --- 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 - } } \ No newline at end of file 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 index 216211c..729d10c 100644 --- 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/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt index 3839f81..9bfdbf0 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt +++ b/TeamCode/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) }