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

Claw, Gantry, and Lift

This commit is contained in:
Thomas 2023-12-02 12:35:10 -06:00
parent 0e47d892cf
commit 63c998258e
5 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,6 +26,7 @@ public abstract class AutoBase extends LinearOpMode {
public static double SCORING_DURATION_S = 5f; public static double SCORING_DURATION_S = 5f;
public static double APRIL_TAG_RIGHT_DELTA = -8.5; public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 7.0; public static double APRIL_TAG_LEFT_DELTA = 7.0;
protected static double Delay = 5000;
protected Robot robot; protected Robot robot;
protected FtcDashboard dashboard; protected FtcDashboard dashboard;
@ -35,6 +36,7 @@ public abstract class AutoBase extends LinearOpMode {
protected final CenterStageCommon.Alliance alliance; protected final CenterStageCommon.Alliance alliance;
protected final Pose2d park; protected final Pose2d park;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) { protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) {
this.alliance = alliance; this.alliance = alliance;
this.initialPosition = initialPosition; this.initialPosition = initialPosition;
@ -53,7 +55,6 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.update(); this.robot.update();
this.sleep(20); this.sleep(20);
} }
this.sleep(5000);
if (isStopRequested()) { if (isStopRequested()) {
return; return;
} }

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

@ -21,6 +21,7 @@ public class BlueFrontStage extends AutoBase {
} }
protected void propLeft() { protected void propLeft() {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, 31)); builder.lineToConstantHeading(new Vector2d(-42, 31));
@ -36,6 +37,7 @@ public class BlueFrontStage extends AutoBase {
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
} }
protected void propCenter() { protected void propCenter() {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec()); builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> { builder.addDisplacementMarker(10, () -> {
@ -50,6 +52,7 @@ public class BlueFrontStage extends AutoBase {
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
} }
protected void propRight() { protected void propRight() {
this.sleep(5000);
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)));
builder.addDisplacementMarker(10, () -> { builder.addDisplacementMarker(10, () -> {

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

@ -48,8 +48,9 @@ public class RedBackStage extends AutoBase {
} }
protected void propRight() { protected void propRight() {
// red back side
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33))); builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(-33)));
builder.addDisplacementMarker(10, () -> { builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
}); });

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

@ -22,6 +22,7 @@ public class RedFrontStage extends AutoBase {
// propLeft will be a reverse of BlueFrontpropRight // propLeft will be a reverse of BlueFrontpropRight
protected void propLeft() { protected void propLeft() {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123))); builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123)));
builder.addDisplacementMarker(10, () -> { builder.addDisplacementMarker(10, () -> {
@ -37,6 +38,7 @@ public class RedFrontStage extends AutoBase {
} }
protected void propCenter() { protected void propCenter() {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous.vec()); builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> { builder.addDisplacementMarker(10, () -> {
@ -53,6 +55,7 @@ public class RedFrontStage extends AutoBase {
// propRight will be the reverse of BlueFrontpropRight // propRight will be the reverse of BlueFrontpropRight
protected void propRight() { protected void propRight() {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180))); builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, -31)); builder.lineToConstantHeading(new Vector2d(-42, -31));

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

@ -18,8 +18,8 @@ import org.opencv.core.Point;
@Config @Config
public class Camera { public class Camera {
public static float PROP_REJECTION_VERTICAL_UPPER = 250; public static float PROP_REJECTION_VERTICAL_UPPER = 150;
public static float PROP_REJECTION_VERTICAL_LOWER = 370; public static float PROP_REJECTION_VERTICAL_LOWER = 250;
private PropDetectionPipeline prop; private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag; private AprilTagProcessor aprilTag;
private VisionPortal visionPortal; private VisionPortal visionPortal;