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 Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance; protected final CenterStageCommon.Alliance alliance;
protected final Pose2d rendezvous; 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.alliance = alliance;
this.initialPosition = initialPosition; this.initialPosition = initialPosition;
this.rendezvous = rendezvous; this.rendezvous = rendezvous;
this.park = park;
} }
@Override @Override
@ -86,8 +88,7 @@ public abstract class AutoBase extends LinearOpMode {
moveToBackstage(); moveToBackstage();
prepareToScore(); prepareToScore();
scorePreloadedPixel(); scorePreloadedPixel();
park();
// TODO Tommy: Park
} }
private void dislodgePropAndPlacePixelLeft() { private void dislodgePropAndPlacePixelLeft() {
@ -102,8 +103,17 @@ public abstract class AutoBase extends LinearOpMode {
this.sleep(100); this.sleep(100);
this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); 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() { private void dislodgePropAndPlacePixelRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); 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()); 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() { private void determinePropLocation() {
setPropLocationIfVisible(Center, Unknown); setPropLocationIfVisible(Center, Unknown);
@ -209,4 +208,12 @@ public abstract class AutoBase extends LinearOpMode {
this.propLocation = ifNotVisible; 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; package opmodes;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp") @Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
public class LeftAuto extends AutoBase { public class BlueFrontStage extends AutoBase {
public LeftAuto() { public BlueFrontStage() {
super( super(
CenterStageCommon.Alliance.Blue, CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)), new Pose2d(-36, 63, Math.toRadians(-90)),
new Pose2d(-36, 11)); new Pose2d(-36, 11),
new Pose2d(62, 12));
} }
@Override @Override