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

Working right auto

This commit is contained in:
Scott Barnes 2023-11-18 13:59:57 -06:00
parent cc8cf18ff0
commit 3f9e39e45b
4 changed files with 54 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,15 +23,18 @@ import org.firstinspires.ftc.teamcode.vision.Detection;
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static double SCORING_DURATION_S = 5f;
public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 4.0;
protected Robot robot;
protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry;
protected CenterStageCommon.PropLocation propLocation;
protected final Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance;
protected final Vector2d rendezvous;
protected final Pose2d rendezvous;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) {
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.rendezvous = rendezvous;
@ -53,17 +56,26 @@ 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:
// TODO Tommy: Place the pixel on the left tape and move to rendezvous position
return;
case Unknown:
case Center:
dislodgePropAndPlacePixel();
dislodgePropAndPlacePixelCenter();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
this.robot.getDrive().followTrajectorySequence(builder.build());
break;
case Right:
// TODO Tommy: Place the pixel on the right tape and move to rendezvous position
return;
dislodgePropAndPlacePixelRight();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
break;
}
moveToBackstage();
@ -73,6 +85,19 @@ public abstract class AutoBase extends LinearOpMode {
// TODO Tommy: Park
}
private void dislodgePropAndPlacePixelRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getClaw().openSync();
this.sleep(100);
this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
}
private void scorePreloadedPixel() {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut();
@ -92,23 +117,40 @@ public abstract class AutoBase extends LinearOpMode {
}
private void prepareToScore() {
// At this point we know that Y = 38
// For 2 -> Ydelta = 0
// For 3 -> 3 5/8
// For 1 -> - 3 5/8
double delta = 0;
switch (this.propLocation) {
case Left:
delta = APRIL_TAG_LEFT_DELTA;
break;
case Unknown:
case Center:
delta = 0;
break;
case Right:
delta = APRIL_TAG_RIGHT_DELTA;
break;
}
double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5);
Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + (distance - SCORING_DISTANCE_FROM_APRIL_TAG), this.robot.getDrive().getPoseEstimate().getY() + delta);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
builder.lineToConstantHeading(target);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void dislodgePropAndPlacePixel() {
private void dislodgePropAndPlacePixelCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous);
builder.lineToLinearHeading(rendezvous);
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});

2
TeamCode/src/main/java/opmodes/LeftAuto.java
View File

@ -12,7 +12,7 @@ public class LeftAuto extends AutoBase {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)),
new Vector2d(-36, 11));
new Pose2d(-36, 11));
}
@Override

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -54,6 +54,7 @@ public class Robot {
}
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
this.drive.update();
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}

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

@ -66,5 +66,5 @@ public class RobotConfig {
public static double DETECTION_LEFT_X = 0;
public static double DETECTION_CENTER_X = .5;
public static double DETECTION_RIGHT_X = 1;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.5;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
}