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

Working BlueBackstage auto

This commit is contained in:
Scott Barnes 2023-11-18 16:49:45 -06:00
parent 142256c19c
commit fbb6eeca9b
3 changed files with 130 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,7 +25,7 @@ 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 = 8.0;
public static double APRIL_TAG_LEFT_DELTA = 7.0;
protected Robot robot;
protected FtcDashboard dashboard;
@ -33,13 +33,11 @@ public abstract class AutoBase extends LinearOpMode {
protected CenterStageCommon.PropLocation propLocation;
protected final Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance;
protected final Pose2d rendezvous;
protected final Pose2d park;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) {
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.rendezvous = rendezvous;
this.park = park;
}
@ -56,32 +54,24 @@ public abstract class AutoBase extends LinearOpMode {
this.sleep(20);
}
if (isStopRequested()) {
return;
}
// 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:
dislodgePropAndPlacePixelLeft();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
propLeft();
break;
case Unknown:
case Center:
dislodgePropAndPlacePixelCenter();
builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
this.robot.getDrive().followTrajectorySequence(builder.build());
propCenter();
break;
case Right:
dislodgePropAndPlacePixelRight();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
propRight();
break;
}
@ -91,42 +81,19 @@ public abstract class AutoBase extends LinearOpMode {
park();
}
private void dislodgePropAndPlacePixelLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, 31));
builder.addTemporalMarker(0.2, () -> {
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 dislodgePropAndPlacePixelCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
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 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());
protected abstract void propLeft();
protected abstract void propCenter();
protected abstract void propRight();
protected void openAndLiftClaw() {
this.robot.getClaw().openSync();
this.sleep(100);
this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
}
private void scorePreloadedPixel() {
protected void scorePreloadedPixel() {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) {
@ -144,7 +111,7 @@ public abstract class AutoBase extends LinearOpMode {
}
private void prepareToScore() {
protected void prepareToScore() {
// At this point we know that Y = 38
// For 2 -> Ydelta = 0
// For 3 -> 3 5/8
@ -169,22 +136,21 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void moveToBackstage() {
protected void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void determinePropLocation() {
protected void determinePropLocation() {
setPropLocationIfVisible(Center, Unknown);
if (this.propLocation != Center) {
peekRight();
}
}
private void peekRight() {
protected void peekRight() {
TrajectorySequenceBuilder builder = this.robot.getDrive()
.trajectorySequenceBuilder(initialPosition);
builder.forward(5);

60
TeamCode/src/main/java/opmodes/BlueBackStage.java Normal file
View File

@ -0,0 +1,60 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
public class BlueBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 11);
public BlueBackStage() {
super(
CenterStageCommon.Alliance.Blue,
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());
openAndLiftClaw();
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
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() {
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);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
}

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

@ -1,22 +1,66 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
public class BlueFrontStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(-36, 11);
public BlueFrontStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)),
new Pose2d(-36, 11),
new Pose2d(62, 12));
}
@Override
public void runOpMode() {
super.runOpMode();
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, 31));
builder.addTemporalMarker(0.2, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
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() {
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());
openAndLiftClaw();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(this.rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}