diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Drone.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Drone.kt new file mode 100644 index 0000000..1d2dd63 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Drone.kt @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode.api + +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo +import org.firstinspires.ftc.teamcode.utils.RobotConfig + +/** + * An API to control the drone launcher. + */ +object Drone : API() { + private lateinit var pin: Servo + + override fun init(opMode: OpMode) { + super.init(opMode) + + this.pin = this.opMode.hardwareMap.get(Servo::class.java, "pin") + + this.resetPin() + } + + /** Moves the pin to the open position, releasing the drone. */ + fun releasePin() { + this.pin.position = RobotConfig.Drone.OPEN_PIN + } + + /** Resets the pin to its default, closed, position. */ + private fun resetPin() { + this.pin.position = RobotConfig.Drone.CLOSE_PIN + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt index 33fe556..78434b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.api.Box +import org.firstinspires.ftc.teamcode.api.Drone import org.firstinspires.ftc.teamcode.api.Telemetry import org.firstinspires.ftc.teamcode.api.TriWheels import org.firstinspires.ftc.teamcode.api.linear.Encoders @@ -31,6 +32,7 @@ abstract class AutoMain : LinearOpMode() { TriWheels.init(this) Encoders.init(this) Box.init(this) + Drone.init(this) // Vision APIs // AprilVision.init(this) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt index c11f5c6..60e3839 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.DcMotor import org.firstinspires.ftc.teamcode.api.Box +import org.firstinspires.ftc.teamcode.api.Drone import org.firstinspires.ftc.teamcode.api.GamepadEx import org.firstinspires.ftc.teamcode.api.LinearSlide import org.firstinspires.ftc.teamcode.api.Telemetry @@ -18,9 +19,6 @@ class TeleOpMain : OpMode() { // Whether the linear slide should remain locked in place or not. private var slideLocked = false - // Whether the robot should drive along the main axis or the pushbot axis. - private var pushBotAxis = false - // init will run once override fun init() { // Setup special telemetry @@ -37,6 +35,9 @@ class TeleOpMain : OpMode() { // Box controls Box.init(this) + // Drone controls + Drone.init(this) + // Log that we are initialized Telemetry.sayInitialized() } @@ -54,13 +55,7 @@ class TeleOpMain : OpMode() { // angle and strength // PI / 3 because 0 radians is right, not forward - val joyRadians = - atan2(joyY, joyX) - (PI / 3.0) + - if (this.pushBotAxis) { - 2.0 * PI / 3.0 - } else { - 0.0 - } + val joyRadians = atan2(joyY, joyX) - (PI / 3.0) val joyMagnitude = sqrt(joyY * joyY + joyX * joyX) @@ -107,10 +102,9 @@ class TeleOpMain : OpMode() { Box.pickUpBox() } - if (gamepad.y) { - this.pushBotAxis = true - } else if (gamepad.x) { - this.pushBotAxis = false + // Input to launch drone. + if (gamepad.x && gamepad.y) { + Drone.releasePin() } // movement of all wheels diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/RobotConfig.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/RobotConfig.kt index 9e79bfd..2b14bb1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/RobotConfig.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/RobotConfig.kt @@ -155,6 +155,16 @@ object RobotConfig { var SLIDE_DOWN_POWER: Double = -0.4 } + /** Configurations related to the Drone API. */ + @Config + object Drone { + @JvmField + var OPEN_PIN: Double = 0.5 + + @JvmField + var CLOSE_PIN: Double = 0.0 + } + /** * Represents what model of robot is running the code. *