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

Working auto with park

This commit is contained in:
Scott Barnes 2023-11-18 15:35:16 -06:00
parent 1aa49e979d
commit 142256c19c
2 changed files with 27 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -34,11 +34,13 @@ public abstract class AutoBase extends LinearOpMode {
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) {
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.rendezvous = rendezvous;
this.park = park;
}
@Override
@ -86,8 +88,7 @@ public abstract class AutoBase extends LinearOpMode {
moveToBackstage();
prepareToScore();
scorePreloadedPixel();
// TODO Tommy: Park
park();
}
private void dislodgePropAndPlacePixelLeft() {
@ -102,8 +103,17 @@ public abstract class AutoBase extends LinearOpMode {
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)));
@ -166,17 +176,6 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getDrive().followTrajectorySequence(builder.build());
}
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 determinePropLocation() {
setPropLocationIfVisible(Center, Unknown);
@ -209,4 +208,12 @@ public abstract class AutoBase extends LinearOpMode {
this.propLocation = ifNotVisible;
}
}
public void park() {
double currentX = this.robot.getDrive().getPoseEstimate().getX();
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.strafeTo(new Vector2d(currentX, park.getY()));
builder.lineToConstantHeading(park.vec());
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}

8
TeamCode/src/main/java/opmodes/LeftAuto.java → TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -1,18 +1,18 @@
package opmodes;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
public class LeftAuto extends AutoBase {
public LeftAuto() {
public class BlueFrontStage extends AutoBase {
public BlueFrontStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)),
new Pose2d(-36, 11));
new Pose2d(-36, 11),
new Pose2d(62, 12));
}
@Override