eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

auto Drive stuff and such and such

This commit is contained in:
Thomas 2024-01-09 17:31:16 -06:00
parent 35e963b1d7
commit ea7cfae4f6
6 changed files with 211 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -1,7 +1,10 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
@ -9,9 +12,11 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.hardware.Camera;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@ -28,7 +33,9 @@ public class MainTeleOp extends OpMode {
private boolean liftArmShouldBeUp = false;
private boolean screwArmIsMoving = false;
private FtcDashboard dashboard;
private boolean leftWasPressed;
private boolean rightWasPressed;
private boolean isGantryFlipped = false;
@Override
public void init() {
this.dashboard = FtcDashboard.getInstance();
@ -45,12 +52,14 @@ public class MainTeleOp extends OpMode {
this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
// Button Mappings
// Drive
boolean leftPressed = gamepad1.dpad_left;
boolean rightPressed = gamepad1.dpad_right;
// Claw / Pickup
boolean openClaw = gamepad2.b; // B
boolean clawUp = gamepad2.y; // Y
boolean clawDownSafe = gamepad2.dpad_down; // dpad-down
boolean clawDown = gamepad2.a || clawDownSafe; // A
// Robot Drone Launch
boolean robotDroneLaunch = gamepad1.left_bumper && gamepad1.right_bumper; // Change if Merck wants (LT)
@ -71,7 +80,7 @@ public class MainTeleOp extends OpMode {
if (openClaw) {
this.robot.getClaw().open();
this.screwArmIsMoving = false;
} else if (!clawUp && !clawDown && !this.screwArmIsMoving){
} else if (!clawUp && !clawDown && !this.screwArmIsMoving) {
this.robot.getClaw().close();
}
if (clawUp) {
@ -88,13 +97,15 @@ public class MainTeleOp extends OpMode {
}
// Gantry
if (!previousScrewArmToggle && screwArmToggle) {
if (!previousScrewArmToggle && screwArmToggle ) {
this.screwArmIsMoving = true;
if (screwArmPos) {
this.robot.getGantry().armIn();
this.isGantryFlipped = false;
} else {
this.robot.getClaw().open();
this.robot.getGantry().armOut();
this.isGantryFlipped = true;
}
this.robot.getClaw().open();
screwArmPos = !screwArmPos;
@ -107,17 +118,22 @@ public class MainTeleOp extends OpMode {
this.robot.getGantry().stop();
}
if (slideUp) {
if (slideUp && isGantryFlipped) {
this.robot.getGantry().setSlideTarget(SLIDE_UP);
} else if (slideDown) {
this.robot.getGantry().setSlideTarget(0);
} else if (previousSlideUp || previousSlideDown){
} else if (previousSlideUp || previousSlideDown) {
this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition());
}
} else if (!isGantryFlipped)
this.telemetry.addData("GantryNotFlipped!", "Null");
double gantryXPosition = X_CENTER + (gantryXInput * 0.5);
gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
this.robot.getGantry().setX(gantryXPosition);
if (isGantryFlipped) {
double gantryXPosition = X_CENTER + (gantryXInput * 0.5);
gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
this.robot.getGantry().setX(gantryXPosition);
} else {
this.telemetry.addData("GantryNotFlippedGoober", "Null");
}
// Robot Drone
@ -151,6 +167,22 @@ public class MainTeleOp extends OpMode {
this.robot.getLift().stopReset();
}
Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
if (poseFromAprilTag != null) {
if (leftPressed && !leftWasPressed) {
macroToScore(poseFromAprilTag, true);
} else if (rightPressed && !rightWasPressed) {
macroToScore(poseFromAprilTag, false);
}
}
if (!leftPressed && !rightPressed) { // breaks drive as makes it so it continuously brakes, added !this.robot.getDrive().isBusy just for placement
this.robot.getDrive().breakFollowing();
}
this.leftWasPressed = leftPressed;
this.rightWasPressed = rightPressed;
this.previousSlideUp = slideUp;
this.previousScrewArmToggle = screwArmToggle;
@ -159,4 +191,18 @@ public class MainTeleOp extends OpMode {
this.robot.update();
}
private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
Pose2d target; // defines a new pose2d named target, position not yet given
Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
double y = poseEstimate.getY() > 0
? left ? 40 : 30
: left ? -30 : -40;
this.robot.getDrive().setPoseEstimate(poseEstimate);
target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}
}

97
TeamCode/src/main/java/opmodes/Test.java Normal file
View File

@ -0,0 +1,97 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.hardware.Camera;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@TeleOp(name = "Test", group = "Main")
public class Test extends OpMode {
private Robot robot;
private FtcDashboard dashboard;
private boolean leftWasPressed;
private boolean rightWasPressed;
private int targetTagId;
@Override
public void init() {
this.dashboard = FtcDashboard.getInstance();
this.robot = new Robot(this.hardwareMap, telemetry);
telemetry.addData("Status", "Initialized");
}
@Override
public void loop() {
robot.update();
Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
dashboard.getTelemetry().update();
boolean leftPressed = gamepad1.dpad_left;
boolean rightPressed = gamepad1.dpad_right;
if (poseFromAprilTag != null) {
if (leftPressed && !leftWasPressed) {
macroToScore(poseFromAprilTag, true);
} else if (rightPressed && !rightWasPressed) {
macroToScore(poseFromAprilTag, false);
}
} else {
if (leftPressed && !leftWasPressed || rightPressed && !rightWasPressed) {
moveToStartSquare();
}
}
if (!leftPressed && !rightPressed) {
this.robot.getDrive().breakFollowing();
}
leftWasPressed = leftPressed;
rightWasPressed = rightPressed;
}
private void moveToStartSquare() {
if (this.robot.getDrive().isBusy()) {
return;
}
Pose2d currentPoseEstimate = this.robot.getDrive().getPoseEstimate();
if (currentPoseEstimate.getX() < 0) {
return;
}
this.targetTagId = currentPoseEstimate.getY() >= 0 ? 2 : 5;
double y = targetTagId == 2 ? 36f : -36f;
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, y, 0));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}
private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
Pose2d target;
Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
double y = poseEstimate.getY() > 0
? left ? 40 : 30
: left ? -30 : -40;
this.robot.getDrive().setPoseEstimate(poseEstimate);
target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}
}

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -1,20 +1,32 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import android.hardware.GeomagneticField;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.opencv.core.Point;
import org.jetbrains.annotations.NotNull;
import java.util.Arrays;
@Config
public class Camera {
@ -24,7 +36,8 @@ public class Camera {
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
private Telemetry telemetry;
public static final Vector2d tag2Pose = new Vector2d(60, 36);
public static final Vector2d tag5Pose = new Vector2d(60, -36);
private boolean initialized;
public Camera(HardwareMap hardwareMap, Telemetry telemetry) {
@ -62,10 +75,10 @@ public class Camera {
return INVALID_DETECTION;
}
public AprilTagDetection getAprilTag(int id) {
public AprilTagDetection getAprilTag(int ... ids) {
return this.aprilTag.getDetections()
.stream()
.filter(x -> x.id == id)
.filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0)
.findFirst()
.orElse(null);
}
@ -90,4 +103,37 @@ public class Camera {
public CenterStageCommon.Alliance getAlliance() {
return this.prop != null ? this.prop.getAlliance() : null;
}
public Vector2d getPoseFromAprilTag(int ... ids) {
if (ids == null || ids.length == 0) {
ids = new int[]{2, 5};
}
AprilTagDetection aprilTagDetection = getAprilTag(ids);
if (aprilTagDetection == null) {
return null;
}
AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
double ourPoseX;
double ourPoseY;
switch (aprilTagDetection.id) {
case 2:
ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
ourPoseY = tag2Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
break;
case 5:
ourPoseX = tag5Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
ourPoseY = tag5Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
break;
default:
ourPoseX = 0;
ourPoseY = 0;
break;
}
return new Vector2d(ourPoseX, ourPoseY);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -101,11 +101,11 @@ public class Gantry {
}
public void intake() {
this.screwServo.setPower(1);
this.screwServo.setPower(-1);
}
public void deposit() {
this.screwServo.setPower(-1);
this.screwServo.setPower(1);
}
public void stop() {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java
View File

@ -72,4 +72,6 @@ public class RobotConfig {
public static double DETECTION_CENTER_X = .5;
public static double DETECTION_RIGHT_X = 1;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
public static final double FORWARD_OFFSET_IN = 7.75;
public static final double SIDE_OFFSET_IN = 0.5;
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -209,7 +209,11 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
public void breakFollowing() {
trajectorySequenceRunner.breakFollowing();
if (trajectorySequenceRunner.isBusy()) {
setWeightedDrivePower(new Pose2d());
}
}
public Pose2d getLastError() {
return trajectorySequenceRunner.getLastPoseError();
}