lift hits the limit switich

GearUp12499-org · Feb 19, 2024 · 00d07c0 · 00d07c0
1 parent bbfa092
commit 00d07c0
Show file tree

Hide file tree

Showing 5 changed files with 198 additions and 106 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configurations/Robot.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configurations/Robot.kt
@@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.DistanceSensor
 import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.IMU
 import com.qualcomm.robotcore.hardware.Servo
+import com.qualcomm.robotcore.hardware.TouchSensor
 import dev.aether.collaborative_multitasking.Scheduler
 import dev.aether.collaborative_multitasking.SharedResource
 import dev.aether.collaborative_multitasking.Task
@@ -97,6 +98,10 @@ class Robot(map: HardwareMap) : RobotConfiguration() {
         map.typedMaybeGet("dropDown")
     }
 
+    override val liftLimitSwitch: TouchSensor? by lazy {
+        map.typedMaybeGet("liftSwitch")
+    }
+
 
     init {
         setReverse(frontLeft)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configurations/RobotConfiguration.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configurations/RobotConfiguration.kt
@@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DistanceSensor
 import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.IMU
 import com.qualcomm.robotcore.hardware.Servo
+import com.qualcomm.robotcore.hardware.TouchSensor
 import dev.aether.collaborative_multitasking.SharedResource
 import org.firstinspires.ftc.robotcore.external.Telemetry
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
@@ -86,6 +87,10 @@ abstract class RobotConfiguration {
     fun dropDownServo(): Servo = dropDownServo
         ?: throw NullPointerException("Robot configuration has no Drop Down Servo.")
 
+    protected abstract val liftLimitSwitch: TouchSensor?
+    fun liftLimitSwitch(): TouchSensor = liftLimitSwitch
+        ?: throw NullPointerException("Robot configuration has no limit switch.")
+
     private fun deviceStatus(name: String, device: Any?): String = "$name: " + when (device) {
         (device == null) -> "DISCONNECTED"
         is DcMotor -> "${device.mode} P${device.power}"

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/HWTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/HWTest.java
@@ -9,6 +9,8 @@
 import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration;
 import org.firstinspires.ftc.teamcode.utilities.MotorSet;
 
+import java.util.function.Supplier;
+
 class Panic extends RuntimeException {
 }
 
@@ -147,6 +149,43 @@ void testEncoder(String label, DcMotor encoded) {
         throw new Panic();
     }
 
+    void testButton(String label, Supplier<Boolean> enabled) {
+        lastA = gamepad1.a;
+        lastB = gamepad1.b;
+
+        telemetry.addLine("Activate the " + label + " sensor...");
+        telemetry.addLine();
+        telemetry.addLine("[B]: STOP");
+        telemetry.update();
+
+        while (opModeIsActive()) {
+            if (enabled.get()) {
+                break;
+            }
+            if (gamepad1.b && !lastB) {
+                throw new Panic();
+            }
+            lastA = gamepad1.a;
+            lastB = gamepad1.b;
+        }
+
+        telemetry.addLine("Deactivate the " + label + " sensor...");
+        telemetry.addLine();
+        telemetry.addLine("[B]: STOP");
+        telemetry.update();
+
+        while (opModeIsActive()) {
+            if (!enabled.get()) {
+                break;
+            }
+            if (gamepad1.b && !lastB) {
+                throw new Panic();
+            }
+            lastA = gamepad1.a;
+            lastB = gamepad1.b;
+        }
+    }
+
     void testEncoders() {
         if (!confirm("Test encoders?")) return;
         testEncoder("Strafe odo", robot.odoPerpendicular());
@@ -183,6 +222,10 @@ void testSensors() {
         if (!confirm("Test sensors?")) return;
         testDistance("Left", robot.distanceLeft());
         testDistance("Right", robot.distanceRight());
+        robot.liftLeft().setPower(0.0);
+        robot.liftLeft().setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.liftLeft().setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        testButton("Lift switch", robot.liftLimitSwitch()::isPressed);
     }
 
     @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TeleOp.java
@@ -90,6 +90,32 @@ public void runOpMode() {
 
         //Reset odometry pods
         robot.clearEncoders();
+        robot.liftLeft().setPower(0.0);
+        robot.liftLeft().setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.liftRight().setPower(0.0);
+        robot.liftRight().setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        scheduler.task(e -> {
+            e.onStart(() -> {
+                robot.liftLeft().setPower(-0.4);
+                robot.liftRight().setPower(-0.4);
+                return kvoid;
+            });
+            e.isCompleted(() -> robot.liftLimitSwitch().isPressed());
+            e.onFinish(() -> {
+                robot.liftLeft().setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.liftLeft().setTargetPosition(0);
+                robot.liftLeft().setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                robot.liftLeft().setPower(1.0);
+                robot.liftRight().setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.liftRight().setTargetPosition(0);
+                robot.liftRight().setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                robot.liftRight().setPower(1.0);
+                return kvoid;
+            });
+            return kvoid;
+        });
+        scheduler.runToCompletion(this::opModeInInit);
 
         waitForStart();
         AtomicInteger targetLeft = new AtomicInteger();