From 4a08148b1dd83abb4436e57d2e9d7c586a370031 Mon Sep 17 00:00:00 2001 From: Carlos Date: Mon, 16 Sep 2024 07:43:30 -0700 Subject: [PATCH] Commit changes to Control Hub positioning and new sample file --- .../firstinspires/ftc/teamcode/AutoDrive.java | 193 ++++++++++++++++++ .../ftc/teamcode/SensorIMUOrthogonal.java | 6 +- 2 files changed, 195 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoDrive.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoDrive.java new file mode 100644 index 0000000..7ad4f77 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoDrive.java @@ -0,0 +1,193 @@ +package org.firstinspires.ftc.teamcode; + +import androidx.annotation.NonNull; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import org.firstinspires.ftc.teamcode.MecanumDrive; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +@Config +@Autonomous(name = "BLUE_TEST_AUTO_PIXEL", group = "Autonomous") +public class AutoDrive extends LinearOpMode { + public class Lift { + + private DcMotorEx lift; + + public Lift(HardwareMap hardwareMap) { + lift = hardwareMap.get(DcMotorEx.class, "liftMotor"); + lift.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE); + lift.setDirection(DcMotorSimple.Direction.FORWARD); + } + + public class LiftUp implements Action { + private boolean initialized = false; + + @Override + public boolean run(@NonNull TelemetryPacket packet) { + if (!initialized) { + lift.setPower(0.8); + initialized = true; + } + + double pos = lift.getCurrentPosition(); + packet.put("liftPos", pos); + if (pos < 3000.0) { + return true; + } else { + lift.setPower(0); + return false; + } + } + } + public Action liftUp() { + return new LiftUp(); + } + + public class LiftDown implements Action { + private boolean initialized = false; + + @Override + public boolean run(@NonNull TelemetryPacket packet) { + if (!initialized) { + lift.setPower(-0.8); + initialized = true; + } + + double pos = lift.getCurrentPosition(); + packet.put("liftPos", pos); + if (pos > 100.0) { + return true; + } else { + lift.setPower(0); + return false; + } + } + } + public Action liftDown(){ + return new LiftDown(); + } + } + + public class Claw { + private Servo claw; + + public Claw(HardwareMap hardwareMap) { + claw = hardwareMap.get(Servo.class, "claw"); + } + + public class CloseClaw implements Action { + @Override + public boolean run(@NonNull TelemetryPacket packet) { + claw.setPosition(0.55); + return false; + } + } + public Action closeClaw() { + return new CloseClaw(); + } + + public class OpenClaw implements Action { + @Override + public boolean run(@NonNull TelemetryPacket packet) { + claw.setPosition(1.0); + return false; + } + } + public Action openClaw() { + return new OpenClaw(); + } + } + + @Override + public void runOpMode() { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(11.8, 61.7, Math.toRadians(90))); + Claw claw = new Claw(hardwareMap); + Lift lift = new Lift(hardwareMap); + + // vision here that outputs position + int visionOutputPosition = 1; + + Action trajectoryAction1; + Action trajectoryAction2; + Action trajectoryAction3; + Action trajectoryActionCloseOut; + + trajectoryAction1 = drive.actionBuilder(drive.pose) + .lineToYSplineHeading(33, Math.toRadians(0)) + .waitSeconds(2) + .setTangent(Math.toRadians(90)) + .lineToY(48) + .setTangent(Math.toRadians(0)) + .lineToX(32) + .strafeTo(new Vector2d(44.5, 30)) + .turn(Math.toRadians(180)) + .lineToX(47.5) + .waitSeconds(3) + .build(); + trajectoryAction2 = drive.actionBuilder(drive.pose) + .lineToY(37) + .setTangent(Math.toRadians(0)) + .lineToX(18) + .waitSeconds(3) + .setTangent(Math.toRadians(0)) + .lineToXSplineHeading(46, Math.toRadians(180)) + .waitSeconds(3) + .build(); + trajectoryAction3 = drive.actionBuilder(drive.pose) + .lineToYSplineHeading(33, Math.toRadians(180)) + .waitSeconds(2) + .strafeTo(new Vector2d(46, 30)) + .waitSeconds(3) + .build(); + trajectoryActionCloseOut = drive.actionBuilder(drive.pose) + .strafeTo(new Vector2d(48, 12)) + .build(); + + // actions that need to happen on init; for instance, a claw tightening. + Actions.runBlocking(claw.closeClaw()); + + + while (!isStopRequested() && !opModeIsActive()) { + int position = visionOutputPosition; + telemetry.addData("Position during Init", position); + telemetry.update(); + } + + int startPosition = visionOutputPosition; + telemetry.addData("Starting Position", startPosition); + telemetry.update(); + waitForStart(); + + if (isStopRequested()) return; + + Action trajectoryActionChosen; + if (startPosition == 1) { + trajectoryActionChosen = trajectoryAction1; + } else if (startPosition == 2) { + trajectoryActionChosen = trajectoryAction2; + } else { + trajectoryActionChosen = trajectoryAction3; + } + + Actions.runBlocking( + new SequentialAction( + trajectoryActionChosen, + lift.liftUp(), + claw.openClaw(), + lift.liftDown(), + trajectoryActionCloseOut + ) + ); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorIMUOrthogonal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorIMUOrthogonal.java index f143267..8ad688b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorIMUOrthogonal.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorIMUOrthogonal.java @@ -34,7 +34,6 @@ 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.IMU; - import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; @@ -77,7 +76,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; * to use those parameters. */ @TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor") -@Disabled // Comment this out to add to the OpMode list public class SensorIMUOrthogonal extends LinearOpMode { // The IMU sensor object @@ -106,8 +104,8 @@ public class SensorIMUOrthogonal extends LinearOpMode * * To Do: EDIT these two lines to match YOUR mounting configuration. */ - RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; - RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.DOWN; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);