Skip to content

Commit

Permalink
Red Left - Right with Cycle Complete
Browse files Browse the repository at this point in the history
  • Loading branch information
@0zgeada
0zgeada committed Jan 18, 2024
1 parent f2af4d3 commit 5ba70e5
Show file tree
Hide file tree
Showing 7 changed files with 25 additions and 62 deletions.
14 changes: 5 additions & 9 deletions MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,16 +123,12 @@ public static void main(String[] args) {
*******************************/

Action trajectory =
myBot.getDrive().actionBuilder(pStartingPose_RedLeft)
.setReversed(true)
// Replace prop with your purple pixel (the offset is to adjust pixel's landing position after spit)
// .lineToY(constants.vRedLeftSpike_Center.y + constants.robot_length/2.0 - 4.0)
.lineToY(-35)
.lineToY(-25)
.endTrajectory()
.setReversed(true)
.turnTo(Math.toRadians(0))
myBot.getDrive().actionBuilder(new Pose2d(50,-40, Math.toRadians(-150)))
.setTangent(90)
.splineToLinearHeading(new Pose2d(TILE_CENTER_TO_CENTER,-TILE_CENTER_TO_CENTER/2, Math.toRadians(-180)), Math.toRadians(-180))
.splineToLinearHeading(new Pose2d(-TILE_CENTER_TO_CENTER*2.5, -TILE_CENTER_TO_CENTER/2, Math.toRadians(-180)), Math.toRadians(-180))
.build();

myBot.runAction(trajectory);


Expand Down
31 changes: 7 additions & 24 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/AutoAbstractOpMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,7 @@ public abstract class AutoAbstractOpMode extends LinearOpMode {
public abstract Action traj_left(BrainSTEMRobotA robot);
public abstract Action traj_center(BrainSTEMRobotA robot);
public abstract Action traj_right(BrainSTEMRobotA robot);
public abstract Action cycle_red(BrainSTEMRobotA robot);
public abstract Action cycle_blue(BrainSTEMRobotA robot);
public abstract Action cycle(BrainSTEMRobotA robot);

public abstract Action parking_traj(BrainSTEMRobotA robot);

Expand Down Expand Up @@ -79,7 +78,7 @@ public void runOpMode() {
int zDirection = 0;
boolean foundX = false;
boolean foundY = false;
boolean foundZ = false;
boolean foundZ = true; //false;
int position_error;


Expand Down Expand Up @@ -151,7 +150,7 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
));
runBlocking(new SequentialAction(
getTrajectory(robot, targetAprilTagNum),
traj_right(robot),
// traj_right(robot),
telemetryPacket -> {
telemetry.addLine("Ending pose:");
telemetry.addData("x", robot.drive.pose.position.x);
Expand Down Expand Up @@ -331,7 +330,7 @@ else if (robot.drive.pose.position.y < constants.vRedBackdrop_Center.y) {
0.27 * xDirection,
0.32 * yDirection
),
0.25 * zDirection
0.0 //25 * zDirection
));


Expand All @@ -344,6 +343,7 @@ else if (robot.drive.pose.position.y < constants.vRedBackdrop_Center.y) {
telemetry.update();
}

/*
// Arrived at position. Place pixel and park
runBlocking(new SequentialAction(
new Action() { // TODO: This action may not be needed if the grabber is squeezed at the start via golden gear
Expand Down Expand Up @@ -380,21 +380,20 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
}
}
));
*/

runBlocking(new SequentialAction(
new SleepAction(2.0),

new Action() {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
runBlocking(cycle_red(robot));
runBlocking(cycle(robot));
return false;
}
})
);

// Actions.runBlocking(getCycle(robot, alliance()));

// GO TO PARK
// TODO: In future revisions, add time check to park within 30 seconds

Expand Down Expand Up @@ -561,22 +560,6 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
// is finished and current pose is estimated.
// Note 2: This method uses public targetTagPos and modifies its value

/*
private Action getCycle(BrainSTEMRobotA robot, Alliance alliance) {
Action cycle;
robot.drive.updatePoseEstimate();
if (alliance == Alliance.RED){
cycle = cycle_red(robot);
}
else {
cycle = cycle_blue(robot);
}
return cycle;
}
*/

private Action getTrajectory(BrainSTEMRobotA robot, int targetTagNum) {
Action trajectory;
Expand Down
7 changes: 1 addition & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/AutoBL.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,12 +114,7 @@ public Action traj_right(BrainSTEMRobotA robot) {
}

@Override
public Action cycle_red(BrainSTEMRobotA robot) {
return null;
}

@Override
public Action cycle_blue(BrainSTEMRobotA robot) {
public Action cycle(BrainSTEMRobotA robot) {
return null;
}

Expand Down
7 changes: 1 addition & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/AutoBR.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,12 +138,7 @@ public Action traj_right(BrainSTEMRobotA robot) {
}

@Override
public Action cycle_red(BrainSTEMRobotA robot) {
return null;
}

@Override
public Action cycle_blue(BrainSTEMRobotA robot) {
public Action cycle(BrainSTEMRobotA robot) {
return null;
}

Expand Down
19 changes: 9 additions & 10 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/AutoRL.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ public Action traj_init(BrainSTEMRobotA robot) {
.setReversed(true)

// Move close enough to center spike
.lineToY(-38)
.lineToY(-30.5)

.stopAndAdd(telemetryPacket -> {
telemetry.addLine("Pose before findSpike:");
Expand Down Expand Up @@ -156,7 +156,7 @@ public Action traj_right(BrainSTEMRobotA robot) {
// Goto Backdrop to place your yellow pixel
.setReversed(true)
.setTangent(Math.toRadians(135))
.splineToLinearHeading(new Pose2d(-constants.TILE_CENTER_TO_CENTER*1.75, -constants.TILE_CENTER_TO_CENTER / 2.0, Math.toRadians(180.00001)), Math.toRadians(0))
.splineToLinearHeading(new Pose2d(-constants.TILE_CENTER_TO_CENTER*2.25, -constants.TILE_CENTER_TO_CENTER / 2.0, Math.toRadians(180.00001)), Math.toRadians(180))
.setTangent(0)
// .splineToLinearHeading(new Pose2d(12, -constants.TILE_CENTER_TO_CENTER / 2.0, Math.toRadians(180)), Math.toRadians(0))
.splineToLinearHeading(new Pose2d(constants.vRedClearStageGate.x-8, constants.vRedClearStageGate.y, Math.toRadians(180)), Math.toRadians(0)) // added delta to x so we don't un-score partner's pixel
Expand All @@ -167,19 +167,18 @@ public Action traj_right(BrainSTEMRobotA robot) {
}

@Override
public Action cycle_red(BrainSTEMRobotA robot) {
public Action cycle(BrainSTEMRobotA robot) {
return robot.drive.actionBuilder(robot.drive.pose)
.setTangent(90)
.splineTo(new Vector2d(-constants.TILE_CENTER_TO_CENTER*2.5, -constants.TILE_CENTER_TO_CENTER/2), Math.toRadians(-180))
.splineToLinearHeading(new Pose2d(constants.TILE_CENTER_TO_CENTER / 2+6, -constants.TILE_CENTER_TO_CENTER / 2, Math.toRadians(-180)), Math.toRadians(-180))
.splineToLinearHeading(new Pose2d(-constants.TILE_CENTER_TO_CENTER * 2.5, -constants.TILE_CENTER_TO_CENTER / 2, Math.toRadians(-180)), Math.toRadians(-180))
.waitSeconds(2.0)
.setTangent(0)
.splineToLinearHeading(new Pose2d(constants.TILE_CENTER_TO_CENTER / 2+6, -constants.TILE_CENTER_TO_CENTER / 2, Math.toRadians(180)), Math.toRadians(0))
.splineToLinearHeading(new Pose2d(constants.vRedBackdrop_Left.x+2, constants.vRedBackdrop_Left.y-2, Math.toRadians(180)), Math.toRadians(-90))
.build();
}

@Override
public Action cycle_blue(BrainSTEMRobotA robot) {
return null;
}


@Override
public Action parking_traj(BrainSTEMRobotA robot) {
return robot.drive.actionBuilder(robot.drive.pose)
Expand Down
7 changes: 1 addition & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/AutoRR.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,12 +115,7 @@ public Action traj_right(BrainSTEMRobotA robot) {
}

@Override
public Action cycle_red(BrainSTEMRobotA robot) {
return null;
}

@Override
public Action cycle_blue(BrainSTEMRobotA robot) {
public Action cycle(BrainSTEMRobotA robot) {
return null;
}

Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoClasses/IntakeA.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
return false;
}
},
new SleepAction(1.0),
new SleepAction(0.5),
new Action() {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
Expand Down

0 comments on commit 5ba70e5

Please sign in to comment.