diff --git a/FtcRobotController/src/main/assets/RedCupPrototype.tflite b/FtcRobotController/src/main/assets/RedCupPrototype.tflite deleted file mode 100644 index 80f87261d4b2..000000000000 Binary files a/FtcRobotController/src/main/assets/RedCupPrototype.tflite and /dev/null differ diff --git a/FtcRobotController/src/main/assets/whitePixel.tflite b/FtcRobotController/src/main/assets/whitePixel.tflite deleted file mode 100644 index 0332ed9d3918..000000000000 Binary files a/FtcRobotController/src/main/assets/whitePixel.tflite and /dev/null differ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DumperTestBench.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateDropBox.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DumperTestBench.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateDropBox.java index 734f3575ae2d..a684626fa6a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DumperTestBench.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateDropBox.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; @TeleOp -public class DumperTestBench extends LinearOpMode { +public class ActuateDropBox extends LinearOpMode { @Override public void runOpMode() { Servo dumperRotate = hardwareMap.get(Servo.class, "dumperRotate"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropDownTestBench.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateIntakeDropDown.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropDownTestBench.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateIntakeDropDown.java index a24f63074a61..18e462921e75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropDownTestBench.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuateIntakeDropDown.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; @TeleOp -public class DropDownTestBench extends LinearOpMode { +public class ActuateIntakeDropDown extends LinearOpMode { @Override public void runOpMode() { Servo dumperRotate = hardwareMap.get(Servo.class, "dropDown"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropperTestBench.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuatePurpleDropper.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropperTestBench.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuatePurpleDropper.java index 1a8eff6b864e..9ff3d1830fb3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DropperTestBench.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ActuatePurpleDropper.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; @TeleOp -public class DropperTestBench extends LinearOpMode { +public class ActuatePurpleDropper extends LinearOpMode { @Override public void runOpMode() { Servo purpleDropper = hardwareMap.get(Servo.class, "purpleDropper"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AprilTagSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AprilTagSample.java deleted file mode 100644 index e5114b091a42..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AprilTagSample.java +++ /dev/null @@ -1,191 +0,0 @@ -/* Copyright (c) 2023 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode.opmodes; - -import android.annotation.SuppressLint; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; - -import java.util.List; - -/* - * This OpMode illustrates the basics of AprilTag recognition and pose estimation, - * including Java Builder structures for specifying Vision parameters. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. - */ -@TeleOp(name = "AprilTagSample") -public class AprilTagSample extends LinearOpMode { - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - /** - * The variable to store our instance of the AprilTag processor. - */ - private AprilTagProcessor aprilTag; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initAprilTag(); - - // Wait for the DS start button to be touched. - telemetry.addData("Access preview", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryAprilTag(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end method runOpMode() - - /** - * Initialize the AprilTag processor. - */ - private void initAprilTag() { - - // Create the AprilTag processor. - aprilTag = new AprilTagProcessor.Builder() - .setDrawAxes(true) - .setDrawCubeProjection(true) - .setDrawTagOutline(true) - //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) - //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) - //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) - - // == CAMERA CALIBRATION == - // If you do not manually specify calibration parameters, the SDK will attempt - // to load a predefined calibration for your camera. - //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) - - // ... these parameters are fx, fy, cx, cy. - - .build(); - - // Create the vision portal by using a builder. - VisionPortal.Builder builder = new VisionPortal.Builder(); - - // Set the camera (webcam vs. built-in RC phone camera). - if (USE_WEBCAM) { - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - } else { - builder.setCamera(BuiltinCameraDirection.BACK); - } - - // Choose a camera resolution. Not all cameras support all resolutions. - //builder.setCameraResolution(new Size(640, 480)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - //builder.enableCameraMonitoring(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - //builder.setAutoStopLiveView(false); - - // Set and enable the processor. - builder.addProcessor(aprilTag); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - // Disable or re-enable the aprilTag processor at any time. - //visionPortal.setProcessorEnabled(aprilTag, true); - - } // end method initAprilTag() - - - /** - * Add telemetry about AprilTag detections. - */ - @SuppressLint("DefaultLocale") - private void telemetryAprilTag() { - - List currentDetections = aprilTag.getDetections(); - telemetry.addData("# AprilTags Detected", currentDetections.size()); - - // Step through the list of detections and display info for each one. - for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); - telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); - } - } // end for() loop - - // Add "key" information to telemetry - telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); - telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); - telemetry.addLine("RBE = Range, Bearing & Elevation"); - - } // end method telemetryAprilTag() - -} // end class diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Crab.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Crab.java index 8b2d444d2fbc..3ca328090b8f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Crab.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Crab.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -9,6 +10,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +@Disabled @TeleOp public class Crab extends LinearOpMode { DcMotor.RunMode baseMode = DcMotor.RunMode.RUN_WITHOUT_ENCODER; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagAuto.java deleted file mode 100644 index 911fe4fd6227..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagAuto.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -@TeleOp -public class DriveToTagAuto extends LinearOpMode { - @Override - public void runOpMode() { - // weirdo delegation thing. we're rewriting this anyway - DriveToTagBacking backingScript = new DriveToTagBacking(hardwareMap, telemetry, gamepad1); - waitForStart(); - telemetry.addLine("press X to start"); - telemetry.addLine(""); - telemetry.update(); - while (!gamepad1.x && opModeIsActive()) { - sleep(20); - } - backingScript.start(this::opModeIsActive); - while (opModeIsActive()) { - sleep(20); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagBacking.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagBacking.kt deleted file mode 100644 index 7c16743076bf..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveToTagBacking.kt +++ /dev/null @@ -1,152 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes - -import android.util.Size -import com.qualcomm.robotcore.hardware.Gamepad -import com.qualcomm.robotcore.hardware.HardwareMap -import dev.aether.collaborative_multitasking.MultitaskScheduler -import dev.aether.collaborative_multitasking.ext.minTicks -import org.firstinspires.ftc.robotcore.external.Telemetry -import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName -import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration -import org.firstinspires.ftc.teamcode.detectPairToPose -import org.firstinspires.ftc.teamcode.detectSingleToPose -import org.firstinspires.ftc.teamcode.tagPositions -import org.firstinspires.ftc.teamcode.utilities.CollectionUtils.pairs -import org.firstinspires.ftc.teamcode.utilities.Pose -import org.firstinspires.ftc.teamcode.utilities.degrees -import org.firstinspires.ftc.teamcode.utilities.inches -import org.firstinspires.ftc.teamcode.utilities.radians -import org.firstinspires.ftc.teamcode.utilities.typedGet -import org.firstinspires.ftc.vision.VisionPortal -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor -import kotlin.math.abs - -class DriveToTagBacking( - hardwareMap: HardwareMap, - private val telemetry: Telemetry, - private val gamepad1: Gamepad -) { - companion object { - const val TARGET_DISTANCE = 10.5 //in - const val THRESHOLD_DISTANCE = 0.5 //in - const val THRESHOLD_ANGLE = 2.5 //degrees - const val ROBOT_SIZE = 20.794 // in - - const val MIN_SPEED = 0.1 - - const val STOPPING_DISTANCE = 8 //in - const val STOPPING_ANGLE = 22.5 - const val YAW_FACTOR = 0.5 - - const val MAX_SPEED = 0.25 - - val TargetPosition = Pose(tagPositions[2]!!, 0.radians) + Pose( - -(STOPPING_DISTANCE).inches, - 0.inches, - 90.degrees - ) - - fun cvtRangeToPower(distance: Double): Double { - if (distance < 0) return -cvtRangeToPower(-distance) - - if (distance < THRESHOLD_DISTANCE) return 0.0 - if (distance >= STOPPING_DISTANCE) return MAX_SPEED - - val rampDistance = distance - THRESHOLD_DISTANCE - val gain = (MAX_SPEED - MIN_SPEED) / (STOPPING_DISTANCE - THRESHOLD_DISTANCE) - return gain * rampDistance + MIN_SPEED - } - - fun cvtAngleToPower(angle: Double): Double { - if (angle < 0) return -cvtAngleToPower(-angle) - - if (angle < THRESHOLD_ANGLE) return 0.0 - if (angle >= STOPPING_ANGLE) return MAX_SPEED - - val rampAngle = angle - THRESHOLD_ANGLE - val gain = (MAX_SPEED - MIN_SPEED) / (STOPPING_ANGLE - THRESHOLD_ANGLE) - return gain * rampAngle + MIN_SPEED - } - } - - private val configuration = RobotConfiguration.currentConfiguration()(hardwareMap) - - private val scheduler = MultitaskScheduler() - private val driveMotors = configuration.driveMotors() - private val camera: CameraName = hardwareMap.typedGet("Webcam 1") - - private val aprilTag: AprilTagProcessor = AprilTagProcessor.Builder() - .build() - private val visionPortal = VisionPortal.Builder() - .setCamera(camera) - .addProcessor(aprilTag) - .setCameraResolution(Size(864, 480)) - .build() - - init { - telemetry.addData("state", "Setting up") - telemetry.update() - - var est: Pose? = null - scheduler.task { - +configuration.driveMotorLock - onTick { -> - val detections = aprilTag.detections - val positionEstimates = mutableListOf() - for (pair in pairs(detections)) { - positionEstimates.add( - detectPairToPose(pair.first, pair.second) - ) - } - for (detection in detections) { - positionEstimates.add( - detectSingleToPose(detection) - ) - } - - if (detections.size == 0) { - driveMotors.setAll(0.0) -// telemetry.addData("Status", "Tags Lost") -// telemetry.update() - return@onTick - } - - val average = positionEstimates.reduce { a, b -> a + b } / positionEstimates.size - telemetry.addData("Status", "OK") - telemetry.addData("PoseEst", average.toString()) - est = average - val fromTo = average.toPose(TargetPosition) - telemetry.addData("Move", fromTo.toString()) - val motion = fromTo.getSpeeds(ROBOT_SIZE) - telemetry.addData("Power", motion.toString()) - -// motion.apply(driveMotors) - telemetry.update() - } - isCompleted { -> - // (read this as reassigning est to "it" if it isn't nul.) - est?.let { - val k = (it - TargetPosition) - (abs(k.x.value) <= THRESHOLD_DISTANCE) - && (abs(k.y.value) <= THRESHOLD_DISTANCE) - && (abs(k.theta.value) <= THRESHOLD_ANGLE.degrees.to.radians.value) - } ?: false // or, if it's null, use false instead - } - onFinish { -> - // set all to 0 - driveMotors.setAll(0.0) - } - minTicks(1) - } - telemetry.addData("state", "Ready") - telemetry.update() - } - - fun start(opModeIsActive: () -> Boolean) { - // Finish tasks OR stop the OpMode OR press B to stop the event loop - while (scheduler.hasJobs() && opModeIsActive() && !gamepad1.b) { - scheduler.tick() - } - visionPortal.close() - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ExampleScheduledAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ExampleScheduledAuto.java deleted file mode 100644 index 86d5487ae00f..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ExampleScheduledAuto.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import static dev.aether.collaborative_multitasking.KotlinHelper.kbu; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.util.ElapsedTime; - -import java.util.concurrent.TimeUnit; - -import dev.aether.collaborative_multitasking.MultitaskScheduler; -import dev.aether.collaborative_multitasking.SharedResource; - -@Autonomous -public class ExampleScheduledAuto extends LinearOpMode { - MultitaskScheduler scheduler; - public static final SharedResource EXAMPLE_LOCK = new SharedResource("example"); - - @Override - public void runOpMode() { - scheduler = new MultitaskScheduler(); - waitForStart(); - ElapsedTime timer = new ElapsedTime(); - scheduler.task(kbu(that -> { - that.require(EXAMPLE_LOCK); - that.isCompleted((that2, scheduler) -> { - if (that2.getStartedAt() != null) { - return timer.time(TimeUnit.SECONDS) - that2.getStartedAt() > 3; - } else { - return false; - } - }); - })); - scheduler.task(kbu(that -> { - that.require(EXAMPLE_LOCK); - that.isCompleted((that2, scheduler) -> { - if (that2.getStartedAt() != null) { - return timer.time(TimeUnit.SECONDS) - that2.getStartedAt() > 3; - } else { - return false; - } - }); - })); - scheduler.runToCompletion(this::opModeIsActive); - //noinspection StatementWithEmptyBody - while (!isStopRequested()) { - // idk - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MoveSlidesToHang.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MoveSlidesToHang.java deleted file mode 100644 index 05023215b51a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MoveSlidesToHang.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; - -@TeleOp -public class MoveSlidesToHang extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - DcMotor liftRight = hardwareMap.get(DcMotor.class, "slideRight"); - DcMotor liftLeft = hardwareMap.get(DcMotor.class, "slideLeft"); - //This is the ideal position in ticks for the slides --> 1400 - int hangTarget = 0; - RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); - robot.liftRight().setPower(0.3); - robot.liftLeft().setPower(0.3); -// Stuff above this runs once and stuff after waitForStart() runs until I press stop - waitForStart(); - while (opModeIsActive()) { - telemetry.addData("Slide Motor Ticks: ", hangTarget); - telemetry.update(); - if (gamepad1.right_trigger > 0.9) { - hangTarget = 1400; - } else if (gamepad1.left_trigger > 0.8) { - hangTarget = 0; - } - robot.liftRight().setTargetPosition(hangTarget); - robot.liftLeft().setTargetPosition(hangTarget); - - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/NewDriveToTag.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/NewDriveToTag.kt deleted file mode 100644 index 39d45ccca2ef..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/NewDriveToTag.kt +++ /dev/null @@ -1,132 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes - -import android.util.Log -import android.util.Size -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import dev.aether.collaborative_multitasking.MultitaskScheduler -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName -import org.firstinspires.ftc.teamcode.Var -import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration -import org.firstinspires.ftc.teamcode.odo.AprilTagUpdateTool -import org.firstinspires.ftc.teamcode.odo.ControlRamps -import org.firstinspires.ftc.teamcode.odo.OdoTracker -import org.firstinspires.ftc.teamcode.tagPositions -import org.firstinspires.ftc.teamcode.utilities.Move -import org.firstinspires.ftc.teamcode.utilities.Pose -import org.firstinspires.ftc.teamcode.utilities.Vector2 -import org.firstinspires.ftc.teamcode.utilities.degrees -import org.firstinspires.ftc.teamcode.utilities.inches -import org.firstinspires.ftc.teamcode.utilities.typedGet -import org.firstinspires.ftc.vision.VisionPortal -import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor -import kotlin.math.sqrt - -@TeleOp -class NewDriveToTag : LinearOpMode() { - private var portal: VisionPortal? = null - private var aprilTag: AprilTagProcessor? = null - - companion object { - private val tagOffset = Vector2(0.0, -20.0) - val tagTargets = mapOf( - 1 to tagPositions[1]!! + tagOffset, - 2 to tagPositions[2]!! + tagOffset, - 3 to tagPositions[3]!! + tagOffset, - ) - val xOkWithin = 1.inches - val yOkWithin = 6.inches - val yOkPreciseWithin = 1.inches - const val ROBOT_SIZE = 20.794 // in - val ramp = ControlRamps( - 0.2, - 0.4, - 0.0, - 24.0 - ) - } - - override fun runOpMode() { - val scheduler = MultitaskScheduler() - val robot = RobotConfiguration.currentConfiguration()(hardwareMap) - initVisionPortal(hardwareMap.typedGet("Webcam 1")) - val tagMotion = AprilTagUpdateTool(aprilTag!!, 2) - val odo = OdoTracker(robot, Var.AutoPositions.BlueLeft) - val motors = robot.driveMotors() - - scheduler.task(odo.taskFactory) - val toolTask = scheduler.task(tagMotion.updateTool(odo)) - scheduler.task { - +robot.driveMotorLock - val target = Pose(tagTargets[2]!! + tagOffset, (-90.0).degrees) - onTick { -> -// if (!tagMotion.acquired) return@onTick - - val pose = odo.currentPose - telemetry.addData("current est", pose) - telemetry.addData("target", target) - - // This turns the From pose and the To pose into a Move. - // THIS IS THE PROBLEM PART. - // vvvvvv - val fromTo = pose.toPose(target) - telemetry.addData("from/to", fromTo) - - // Pythagorean theorem. - val distance = - sqrt( - fromTo.forward.value * fromTo.forward.value - + fromTo.right.value * fromTo.right.value - ) - - // THIS IS THE PROBLEM PART. (2) - // vvvvvvvvv - val motion = fromTo.getSpeeds(ROBOT_SIZE) * ramp.ramp(0.0, distance) - telemetry.addData("speeds", motion) - val correctedMotion = motion.map(Move::rampSpeedToPower) - telemetry.addData("powers", correctedMotion) - correctedMotion.apply(motors) - telemetry.update() - Log.i( - "NewDriveToTag", - String.format( - "Current est: %s\nTarget : %s\nFrom/To : %s\nSpeeds : %s\nPowers : %s", - pose, target, fromTo, motion, correctedMotion - ) - ) - } - isCompleted { -> - (odo.currentPose.x - target.x).abs < xOkWithin && (odo.currentPose.y - target.y).abs < yOkPreciseWithin - } - onFinish { -> - motors.setAll(0.0) - } - } - - waitForStart() - if (!opModeIsActive()) return - scheduler.runToCompletion(::opModeIsActive) - while (opModeIsActive()) { - sleep(20) - } - } - - private fun initVisionPortal(webcam: WebcamName) { - aprilTag = AprilTagProcessor.Builder() - .setDrawAxes(true) - .setDrawCubeProjection(true) - .setDrawTagOutline(true) - .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) - .setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) - .build() - val visionPortalBuilder = VisionPortal.Builder() - visionPortalBuilder.setCamera(webcam) - // Medium-sized 16:9ish. See-also builtinwebcamcalibrations.xml - // Logitech HD Pro Webcam C920 - // use Search Anything in "include non-project" mode to find - visionPortalBuilder.setCameraResolution(Size(864, 480)) - visionPortalBuilder.addProcessor(aprilTag) - portal = visionPortalBuilder.build() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/OdoTrackerTest.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/OdoTrackerTest.kt index c73ce3ee60c6..b9198e077613 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/OdoTrackerTest.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/OdoTrackerTest.kt @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes +import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.util.RobotLog @@ -8,6 +9,7 @@ import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration import org.firstinspires.ftc.teamcode.odo.OdoTracker import org.firstinspires.ftc.teamcode.utilities.Pose +@Disabled @TeleOp class OdoTrackerTest : LinearOpMode() { override fun runOpMode() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DistanceSensorTestBench.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadDistanceSensor.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DistanceSensorTestBench.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadDistanceSensor.java index aa56b2f0c43d..0dd840babb7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DistanceSensorTestBench.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadDistanceSensor.java @@ -8,7 +8,7 @@ import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; @TeleOp -public class DistanceSensorTestBench extends LinearOpMode { +public class ReadDistanceSensor extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadOdo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadOdo.java index 0212901c490a..6eebb1c73b12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadOdo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ReadOdo.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -7,14 +8,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; -// Parallel 1 -46289 -// Parallel 2 +39138 -// Perpendicular -32947 - -// Parallel 1 -45182 -// Parallel 2 +40068 -// Perpendicular -34579 - +@Disabled @TeleOp public class ReadOdo extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ScoreCenter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ScoreCenter.java deleted file mode 100644 index 0b7978befa4c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ScoreCenter.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.Var; -import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; -import org.firstinspires.ftc.teamcode.odo.DriveForwardPID; - -@Autonomous -public class ScoreCenter extends LinearOpMode { - @Override - public void runOpMode() throws InterruptedException { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); - robot.clearEncoders(); - DriveForwardPID driveForward = new DriveForwardPID(robot); - waitForStart(); - if (!opModeIsActive()) return; - driveForward.DriveReverse(27.0, telemetry); - robot.purpleDropper().setPosition(Var.PixelDropper.down); - sleep(2000); - robot.purpleDropper().setPosition(Var.PixelDropper.back); - - while (opModeIsActive()) { - telemetry.addData("TOTAL E", driveForward.sumOfError); - telemetry.update(); - sleep(50); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ServoReflectionHax.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ServoReflectionHax.java index 7f0d7e3ca87d..0fb1a9a77e1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ServoReflectionHax.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ServoReflectionHax.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.qualcomm.hardware.lynx.LynxServoController; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Servo; @@ -16,6 +17,7 @@ * We have reached the point in the season that java.lang.reflect is fair game. * P.S. Hey, SDK authors - why didn't you expose this method? lul */ +@Disabled @TeleOp public class ServoReflectionHax extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TFODObjectDetectionSpikes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TFODObjectDetectionSpikes.java deleted file mode 100644 index df915b8869de..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TFODObjectDetectionSpikes.java +++ /dev/null @@ -1,154 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -@Autonomous -public class TFODObjectDetectionSpikes extends LinearOpMode { - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - // TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage, - // this is used when uploading models directly to the RC using the model upload interface. - private static final String TFOD_MODEL_ASSET = "RedCupPrototype.tflite"; - // Define the labels recognized in the model for TFOD (must be in training order!) - private static final String[] LABELS = { - "Red Cup", - }; - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initTfod(); - - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryTfod(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end runOpMode() - - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor by using a builder. - tfod = new TfodProcessor.Builder() - - // With the following lines commented out, the default TfodProcessor Builder - // will load the default model for the season. To define a custom model to load, - // choose one of the following: - // Use setModelAssetName() if the custom TF Model is built in as an asset (AS only). - // Use setModelFileName() if you have downloaded a custom team model to the Robot Controller. - .setModelAssetName(TFOD_MODEL_ASSET) - //.setModelFileName(TFOD_MODEL_FILE) - - // The following default settings are available to un-comment and edit as needed to - // set parameters for custom models. - .setModelLabels(LABELS) - //.setIsModelTensorFlow2(true) - //.setIsModelQuantized(true) - //.setModelInputSize(300) - //.setModelAspectRatio(16.0 / 9.0) - - .build(); - - // Create the vision portal by using a builder. - VisionPortal.Builder builder = new VisionPortal.Builder(); - - // Set the camera (webcam vs. built-in RC phone camera). - if (USE_WEBCAM) { - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - } else { - builder.setCamera(BuiltinCameraDirection.BACK); - } - - // Choose a camera resolution. Not all cameras support all resolutions. - //builder.setCameraResolution(new Size(640, 480)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - //builder.enableLiveView(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - //builder.setAutoStopLiveView(false); - - // Set and enable the processor. - builder.addProcessor(tfod); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - // Set confidence threshold for TFOD recognitions, at any time. - tfod.setMinResultConfidence(0.5f); - - // Disable or re-enable the TFOD processor at any time. - //visionPortal.setProcessorEnabled(tfod, true); - - } // end method initTfod() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2; - double y = (recognition.getTop() + recognition.getBottom()) / 2; - - telemetry.addData("", " "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f,%.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - -} // end class diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/WhitePixelSpikeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/WhitePixelSpikeDetection.java deleted file mode 100644 index 1bd7610a06a6..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/WhitePixelSpikeDetection.java +++ /dev/null @@ -1,158 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import android.util.Size; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -import java.util.List; - -@Autonomous -public class WhitePixelSpikeDetection extends LinearOpMode { - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - // TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage, - // this is used when uploading models directly to the RC using the model upload interface. - private static final String TFOD_MODEL_ASSET = "whitePixel.tflite"; - // Define the labels recognized in the model for TFOD (must be in training order!) - private static final String[] LABELS = { - "WhitePixel", - }; - - /** - * The variable to store our instance of the TensorFlow Object Detection processor. - */ - private TfodProcessor tfod; - private VisionPortal visionPortal; - - @Override - public void runOpMode() { - - initTfod(); - - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - if (opModeIsActive()) { - while (opModeIsActive()) { - - telemetryTfod(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - } - } - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - - } // end runOpMode() - - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor by using a builder. - tfod = new TfodProcessor.Builder() - - // With the following lines commented out, the default TfodProcessor Builder - // will load the default model for the season. To define a custom model to load, - // choose one of the following: - // Use setModelAssetName() if the custom TF Model is built in as an asset (AS only). - // Use setModelFileName() if you have downloaded a custom team model to the Robot Controller. - .setModelAssetName(TFOD_MODEL_ASSET) - //.setModelFileName(TFOD_MODEL_FILE) - - // The following default settings are available to un-comment and edit as needed to - // set parameters for custom models. - .setModelLabels(LABELS) - //.setIsModelTensorFlow2(true) - //.setIsModelQuantized(true) - //.setModelInputSize(300) - //.setModelAspectRatio(16.0 / 9.0) - - .build(); - - // Create the vision portal by using a builder. - VisionPortal.Builder builder = new VisionPortal.Builder(); - - // Set the camera (webcam vs. built-in RC phone camera). - if (USE_WEBCAM) { - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - } else { - builder.setCamera(BuiltinCameraDirection.BACK); - } - - // Choose a camera resolution. Not all cameras support all resolutions. - builder.setCameraResolution(new Size(864, 480)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - //builder.enableLiveView(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - //builder.setAutoStopLiveView(false); - - // Set and enable the processor. - builder.addProcessor(tfod); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - // Set confidence threshold for TFOD recognitions, at any time. - //tfod.setMinResultConfidence(0.75f); - - // Disable or re-enable the TFOD processor at any time. - //visionPortal.setProcessorEnabled(tfod, true); - - } // end method initTfod() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void telemetryTfod() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2; - double y = (recognition.getTop() + recognition.getBottom()) / 2; - - //if else statement - - telemetry.addData("", " "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f,%.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } // end for() loop - - } // end method telemetryTfod() - -} // end class diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/tests/TestDriveToTag.kt b/TeamCode/src/test/java/org/firstinspires/ftc/tests/TestDriveToTag.kt deleted file mode 100644 index 6faae0a8276c..000000000000 --- a/TeamCode/src/test/java/org/firstinspires/ftc/tests/TestDriveToTag.kt +++ /dev/null @@ -1,93 +0,0 @@ -package org.firstinspires.ftc.tests - -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.MAX_SPEED -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.MIN_SPEED -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.STOPPING_ANGLE -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.THRESHOLD_ANGLE -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.THRESHOLD_DISTANCE -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.cvtAngleToPower -import org.firstinspires.ftc.teamcode.opmodes.DriveToTagBacking.Companion.cvtRangeToPower -import kotlin.math.abs -import kotlin.test.Test - -class TestDriveToTag { - @Test - fun `test power function at zero distance`() { - assert(cvtRangeToPower(0.0) near 0.0) - } - - @Test - fun `test power function at threshold distance`() { - val actual = cvtRangeToPower(THRESHOLD_DISTANCE) - val expected = MIN_SPEED - assert( - actual near expected - ) { "expected $expected, got $actual" } - } - - @Test - fun `test negative threshold distance`() { - val actual = cvtRangeToPower(-THRESHOLD_DISTANCE) - val expected = -MIN_SPEED - assert( - actual near expected - ) { "expected $expected, got $actual" } - } - - @Test - fun `test power function at extreme distance`() { - assert( - cvtRangeToPower(DriveToTagBacking.STOPPING_DISTANCE + 1.0) - near MAX_SPEED - ) - } - - @Test - fun `test power function at negative extreme distance`() { - assert( - cvtRangeToPower(-DriveToTagBacking.STOPPING_DISTANCE - 1.0) - near -MAX_SPEED - ) - } - - @Test - fun `test angle function at zero`() { - assert( - cvtAngleToPower(0.0) near 0.0 - ) - } - - @Test - fun `test angle function at threshold angle`() { - assert( - cvtAngleToPower(THRESHOLD_ANGLE) near MIN_SPEED - ) - } - - @Test - fun `test angle function at negative threshold angle`() { - assert( - cvtAngleToPower(-THRESHOLD_ANGLE) near -MIN_SPEED - ) - } - - @Test - fun `test angle function at extreme angle`() { - assert( - cvtAngleToPower(STOPPING_ANGLE + 1.0) near MAX_SPEED - ) - } - - @Test - fun `test angle function at negative extreme angle`() { - assert( - cvtAngleToPower(-STOPPING_ANGLE - 1.0) near -MAX_SPEED - ) - } -} - -private const val EPSILON = 0.0001 -private infix fun Double.near(other: Double): Boolean { - return abs(this - other) < EPSILON -}