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

Working apriltag positioning logic!

This commit is contained in:
Scott Barnes 2024-01-25 22:23:12 -06:00
parent 1a38bec605
commit 7d67f60cb9
11 changed files with 225 additions and 196 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,5 +1,6 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
@ -62,7 +63,6 @@ public abstract class AutoBase extends LinearOpMode {
// If the prop is visible at this point, then it must be in the center (2) position
determinePropLocation();
TrajectorySequenceBuilder builder;
switch (this.propLocation) {
case Left:
propLeft();
@ -75,7 +75,7 @@ public abstract class AutoBase extends LinearOpMode {
propRight();
break;
}
moveToBackstage();
moveToEasel();
prepareToScore();
scorePreloadedPixel();
park();
@ -108,7 +108,6 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getGantry().stop();
this.robot.getGantry().setSlideTarget(0);
this.robot.getGantry().armInSync();
}
protected void prepareToScore() {
@ -129,30 +128,51 @@ public abstract class AutoBase extends LinearOpMode {
delta = APRIL_TAG_RIGHT_DELTA;
break;
}
double distance = this.robot.getCamera().getDistanceToAprilTag(this.alliance == CenterStageCommon.Alliance.Blue ? 2:5, 25, 5);
Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() +
(distance - SCORING_DISTANCE_FROM_APRIL_TAG),
this.robot.getDrive().getPoseEstimate().getY() + delta);
Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
this.robot.getDrive().setPoseEstimate(inferredPos);
Pose2d target = new Pose2d(
60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
inferredPos.getY() + delta,
0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(target);
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void moveToBackstage() {
protected void moveToEasel() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
if (!this.isBackstage()) {
if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI));
builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI));
} else if (this.alliance == CenterStageCommon.Alliance.Red) {
builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI));
builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI));
}
}
if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToLinearHeading(new Pose2d(35, 11, 0));
builder.lineToLinearHeading(new Pose2d(35, 38, 0));
} else {
builder.lineToLinearHeading(new Pose2d(35, -11, 0));
builder.lineToLinearHeading(new Pose2d(35, -36, 0));
builder.lineToLinearHeading(new Pose2d(35, 36, 0));
} else if (this.alliance == CenterStageCommon.Alliance.Red) {
builder.lineToLinearHeading(new Pose2d(35, -35, 0));
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void determinePropLocation() {
this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN);
while (!this.robot.getClaw().isArmAtPosition()) {
this.robot.update();
sleep(20);
}
sleep(250);
setPropLocationIfVisible(Center, Unknown);
if (this.propLocation != Center) {
peekRight();
@ -160,11 +180,15 @@ public abstract class AutoBase extends LinearOpMode {
}
protected void peekRight() {
TrajectorySequenceBuilder builder = this.robot.getDrive()
.trajectorySequenceBuilder(initialPosition);
builder.forward(5);
builder.turn(Math.toRadians(-33));
Pose2d currentPose = this.robot.getDrive().getPoseEstimate();
final double y = currentPose.getY() > 0 ? -5 : 5;
final double z = Math.toRadians(-25);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(currentPose.plus(new Pose2d(0, y, z)));
this.robot.getDrive().followTrajectorySequence(builder.build());
this.sleep(250);
setPropLocationIfVisible(Right, Left);
}
@ -188,7 +212,11 @@ public abstract class AutoBase extends LinearOpMode {
double currentX = this.robot.getDrive().getPoseEstimate().getX();
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.strafeTo(new Vector2d(currentX, park.getY()));
builder.lineToConstantHeading(park.vec());
builder.lineToLinearHeading(park);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected boolean isBackstage() {
return this.initialPosition.getX() > 0;
}
}

42
TeamCode/src/main/java/opmodes/BlueBackStage.java
View File

@ -11,48 +11,44 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
public class BlueBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 10);
private static final int delay = 0;
public BlueBackStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(12, 63, Math.toRadians(-90)),
new Pose2d(12, 63, Math.toRadians(90)),
new Pose2d(62, 62));
}
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
this.sleep(delay);
openAndLiftClaw();
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
builder.lineToConstantHeading(new Vector2d(24, 50));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propRight() {
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, 34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();

45
TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -11,59 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
public class BlueFrontStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(-36, 11);
private static final int delay = 0;
public BlueFrontStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)),
new Pose2d(62, 12));
new Pose2d(-36, 63, Math.toRadians(90)),
new Pose2d(62, 12, Math.toRadians(0)));
}
protected void propLeft() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-40, 31));
builder.addTemporalMarker(0.2, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propCenter() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propRight() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}

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

@ -1,18 +1,18 @@
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.CAMERA_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.CAMERA_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;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
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;
@ -40,10 +40,12 @@ public class MainTeleOp extends OpMode {
@Override
public void init() {
this.dashboard = FtcDashboard.getInstance();
this.telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap, telemetry);
this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
this.robot.getDrive().setPoseEstimate(new Pose2d(12, 63, Math.toRadians(90)));
telemetry.addData("Status", "Initialized");
}
@ -170,7 +172,7 @@ public class MainTeleOp extends OpMode {
this.robot.getLift().stopReset();
}
Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag();
dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
dashboard.getTelemetry().update();
@ -223,12 +225,11 @@ public class MainTeleOp extends OpMode {
}
Pose2d target; // defines a new pose2d named target, position not yet given
Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), -Math.toRadians(poseFromAprilTag.getHeading()));
double y = poseEstimate.getY() > 0
double y = poseFromAprilTag.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);
this.robot.getDrive().setPoseEstimate(poseFromAprilTag);
target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());

43
TeamCode/src/main/java/opmodes/RedBackStage.java
View File

@ -11,52 +11,47 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
public class RedBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, -11);
private static final int delay = 0;
public RedBackStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(12, -63, Math.toRadians(90)),
new Pose2d(12, -63, Math.toRadians(-90)),
new Pose2d(62, -62));
}
protected void propLeft() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, -34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, -34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(-90));
builder.lineToConstantHeading(new Vector2d(24, -50));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propRight() {
// red back side
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
}

43
TeamCode/src/main/java/opmodes/RedFrontStage.java
View File

@ -11,63 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp")
public class RedFrontStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(-36, -10);
private static final int delay = 0;
public RedFrontStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(-36, -63, Math.toRadians(90)),
new Pose2d(-36, -63, Math.toRadians(-90)),
new Pose2d(61, -12));
}
// propLeft will be a reverse of BlueFrontpropRight
protected void propLeft() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propCenter() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(-90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
// propRight will be the reverse of BlueFrontpropRight
protected void propRight() {
this.sleep(5000);
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, -31));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180)));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}

11
TeamCode/src/main/java/opmodes/Test.java
View File

@ -1,20 +1,17 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_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 static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_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 {
@ -41,7 +38,7 @@ public class Test extends OpMode {
boolean slowmode = gamepad1.right_bumper || gamepad1.y;
this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag();
dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
dashboard.getTelemetry().update();
@ -96,7 +93,7 @@ public class Test extends OpMode {
? 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);
target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());

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

@ -1,19 +1,24 @@
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.CAMERA_FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_ROTATION_DEG;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_MINI_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import static java.lang.Math.cos;
import static java.lang.Math.sin;
import static java.lang.Math.tan;
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.apache.commons.math3.ml.neuralnet.twod.util.TopographicErrorHistogram;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
@ -22,12 +27,14 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import java.util.Arrays;
import java.util.List;
import lombok.NonNull;
@Config
public class Camera {
public static float PROP_REJECTION_VERTICAL_UPPER = 10;
public static float PROP_REJECTION_VERTICAL_LOWER = 470;
public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
public static float PROP_REJECTION_VERTICAL_LOWER = 440;
public static float PROP_REJECTION_HORIZONTAL_LEFT = 10;
public static float PROP_REJECTION_HORIZONTAL_RIGHT = 630;
private PropDetectionPipeline prop;
@ -91,27 +98,13 @@ public class Camera {
return INVALID_DETECTION;
}
public AprilTagDetection getAprilTag(int ... ids) {
public AprilTagDetection getAprilTag() {
return this.aprilTag.getDetections()
.stream()
.filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0)
.findFirst()
.orElse(null);
}
public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) {
for (int i = 0; i < 10; i++) {
AprilTagDetection aprilTagDetection = getAprilTag(id);
if (aprilTagDetection != null) {
if (aprilTagDetection.ftcPose.y < rejectAbove
&& aprilTagDetection.ftcPose.y > rejectBelow) {
return aprilTagDetection.ftcPose.y;
}
}
}
return Double.MAX_VALUE;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.prop.setAlliance(alliance);
}
@ -120,36 +113,71 @@ public class Camera {
return this.prop != null ? this.prop.getAlliance() : null;
}
public Pose2d getPoseFromAprilTag(int ... ids) {
if (ids == null || ids.length == 0) {
ids = new int[]{2, 5};
}
public Pose2d estimatePoseFromAprilTag() {
List<AprilTagDetection> aprilTagDetections = aprilTag.getDetections();
AprilTagDetection aprilTagDetection = getAprilTag(ids);
if (aprilTagDetection == null) {
if (aprilTagDetections == null || aprilTagDetections.isEmpty()) {
return null;
}
AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
// return estimatePoseFromAprilTag(aprilTagDetections.get(0));
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;
int numDetections = aprilTagDetections.size();
Pose2d acc = new Pose2d(0, 0, 0);
for (AprilTagDetection aprilTagDetection : aprilTagDetections) {
acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection));
}
return new Pose2d(ourPoseX, ourPoseY, ftcPose.bearing);
return acc.div(numDetections);
}
private Pose2d estimatePoseFromAprilTag(@NonNull AprilTagDetection aprilTagDetection) {
VectorF reference = aprilTagDetection.metadata.fieldPosition;
AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
double ax = reference.get(0);
double ay = reference.get(1);
double t = -Math.toRadians(ftcPose.yaw);
double r = -ftcPose.range;
double b = Math.toRadians(ftcPose.bearing);
double x1 = r * cos(b);
double y1 = r * sin(b);
double x2 = x1 * cos(t) - y1 * sin(t);
double y2 = x1 * sin(t) + y1 * cos(t);
double cx = ax + x2;
double cy = ay + y2;
double t1 = t + Math.toRadians(CAMERA_ROTATION_DEG);
double h = tan(t1);
double rx = -CAMERA_FORWARD_OFFSET_IN * cos(t1) + CAMERA_SIDE_OFFSET_IN * sin(t1);
double ry = -CAMERA_FORWARD_OFFSET_IN * sin(t1) - CAMERA_SIDE_OFFSET_IN * cos(t1);
rx += cx;
ry += cy;
// AprilTagDetection foo = aprilTagDetection;
// telemetry.addData("id", foo.id);
// telemetry.addData("ax", foo.metadata.fieldPosition.get(0));
// telemetry.addData("ay", foo.metadata.fieldPosition.get(1));
// telemetry.addData("yaw", foo.ftcPose.yaw);
// telemetry.addData("range", foo.ftcPose.range);
// telemetry.addData("bearing", foo.ftcPose.bearing);
// telemetry.addData("x1", x1);
// telemetry.addData("y1", y1);
// telemetry.addData("x2", x2);
// telemetry.addData("y2", y2);
// telemetry.addData("cx", cx);
// telemetry.addData("cy", cy);
// telemetry.addData("t1", t1);
// telemetry.addData("h", h);
// telemetry.addData("rx", rx);
// telemetry.addData("ry", ry);
// telemetry.update();
return new Pose2d(rx, ry, h);
}
}

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
@ -22,6 +23,8 @@ public class Claw implements Updatable {
private Telemetry telemetry;
PController clawController = new PController(CLAW_KP);
private double clawControllerTarget;
PController armController = new PController(CLAW_ARM_KP);
private double armControllerTarget = -1;
public Claw(HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW_NAME);
@ -58,15 +61,32 @@ public class Claw implements Updatable {
this.armRight.setPosition(target);
}
public void setArmPositionAsync(double armControllerTarget) {
this.armControllerTarget = armControllerTarget;
}
public boolean isArmAtPosition() {
return this.armController.atSetPoint();
}
public void update() {
this.clawController.setSetPoint(this.clawControllerTarget);
this.clawController.setTolerance(0.001);
this.clawController.setP(CLAW_KP);
double output = 0;
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setP(CLAW_ARM_KP);
if (!this.clawController.atSetPoint()) {
double output = 0;
output = Math.max(-1 * CLAW_MAX, Math.min(CLAW_MAX, this.clawController.calculate(claw.getPosition())));
this.claw.setPosition(this.claw.getPosition() + output);
}
if (this.armControllerTarget > 0 && !this.armController.atSetPoint()) {
double output = 0;
output = Math.max(-1 * PICKUP_ARM_MAX, Math.min(PICKUP_ARM_MAX, this.armController.calculate(armLeft.getPosition())));
this.armLeft.setPosition(this.armLeft.getPosition() + output);
this.armRight.setPosition(this.armRight.getPosition() + output);
}
}
}

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

@ -32,12 +32,13 @@ public class RobotConfig {
public static double AUTO_STRAFE_SLOWDOWN = 4;
// Arm
public static double PICKUP_ARM_MIN = 0.185;
public static double PICKUP_ARM_MIN = 0.175;
public static double PICKUP_ARM_MAX = 0.760;
public static double CLAW_MIN = 0.89;
public static double CLAW_MAX = 0.65;
public static double CLAW_ARM_DELTA = 0.03;
public static double CLAW_KP = 0.3;
public static double CLAW_ARM_KP = 0.15;
// Gantry
public static double GANTRY_ARM_MIN = 0.42;
@ -68,7 +69,8 @@ public class RobotConfig {
// Vision
public static double DETECTION_AREA_MIN = 0.05f;
public static double DETECTION_AREA_MAX = 0.8f;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 7f;
public static final double FORWARD_OFFSET_IN = 7.75;
public static final double SIDE_OFFSET_IN = 0.5;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.25f;
public static final double CAMERA_FORWARD_OFFSET_IN = 7.77;
public static final double CAMERA_SIDE_OFFSET_IN = 0.707;
public static final double CAMERA_ROTATION_DEG = 0;
}

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

@ -74,9 +74,9 @@ public class DriveConstants {
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
*/
public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
public static double encoderTicksToInches(double ticks) {