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

2+1(blue works not red)

This commit is contained in:
ImposterVarunoverlord 2024-03-21 08:24:29 -05:00
parent 28e6aa7836
commit 3c4b14dc33
7 changed files with 356 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
View File

@ -90,13 +90,42 @@ public class MeepMeepTesting {
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
//Go back to white stack
.splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
.lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
.splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
//Go back to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
.build()
);
meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
.setDarkMode(true)
.setBackgroundAlpha(0.95f)
//.addEntity(myBot)
//.addEntity(BlueFrontStage)
//.addEntity(BlueFrontStage3)
.addEntity(BlueFrontStage1)
//.addEntity(BlueFrontStage1)
.addEntity(RedFrontStage)
.start();
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -49,7 +49,7 @@ public class Arm {
public static double wristScore_highMacro = 0.3;
public enum Position {
INTAKE, SCORE, SCOREHI
INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
}
public enum DoorPosition {
@ -61,7 +61,7 @@ public class Arm {
doorServo = hardwareMap.get(Servo.class, "Door");
lAServo = hardwareMap.get(Servo.class, "LeftArm");
rAServo = hardwareMap.get(Servo.class, "RightArm");
// elbow = hardwareMap.get(Servo.class, "Elbow");
elbow = hardwareMap.get(Servo.class, "Elbow");
// lAServo.setDirection(Servo.Direction.REVERSE);
rAServo.setDirection(Servo.Direction.REVERSE);
doorServo.setDirection(Servo.Direction.REVERSE);
@ -106,11 +106,13 @@ public class Arm {
public void setElbowPos (Position pos){
if (pos == Position.INTAKE) {
wristPos = elbow_mid;
elbowPos = elbow_mid;
} else if (pos == Position.SCORE) {
wristPos = elbow_right;
} else if (pos == Position.SCOREHI) {
wristPos = elbow_mid;
} else if (pos == Position.SCORELEFT) {
wristPos = elbow_left;
} else if (pos == Position.SCORERIGHT) {
wristPos = elbow_right;
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java
View File

@ -43,7 +43,7 @@ public abstract class AutoBase extends LinearOpMode {
telemetry.addLine("Initialized");
// Detection vndafds = robot.camera.getProp() <- returns a detection
// int fdsf = detection.getCenter().x <- x value on the screen between -50,50
double PropDetection = robot.camera.getProp().getCenter().x;
double PropDetection = robot.camera.getProp().getCenter().x;
if (PropDetection <= -DetectionTest ) {
teamPropLocation = 1;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java
View File

@ -125,11 +125,11 @@ public class BlueFrontPreload extends AutoBase {
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
break;
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java
View File

@ -11,6 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@ -19,13 +20,13 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
@ -35,13 +36,13 @@ public class RedFrontPreload extends AutoBase {
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-55, -40, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
@ -51,7 +52,7 @@ public class RedFrontPreload extends AutoBase {
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(12, -58.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
@ -124,11 +125,11 @@ public class RedFrontPreload extends AutoBase {
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01);
break;
}
@ -165,7 +166,7 @@ public class RedFrontPreload extends AutoBase {
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180));
break;
case 3:
builder.setTangent(Math.toRadians(-90));

304
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java Normal file
View File

@ -0,0 +1,304 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
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.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontStageme extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
builder.lineToLinearHeading(DROP_2_PART_3);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
//robot.arm.setDoor(OPEN);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.1) {
//robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.1);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 7:
robot.resetMacro(0,getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
case 9:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 10:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
case 11:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 12:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//Park
// case 7:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToLinearHeading(PARK_1);
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
//
// case 8:
// // if drive is done move on
// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
// macroState++;
// }
// break;
//
// case 9:
// robot.resetMacro(0, getRuntime());
// if (robot.macroState == 0) {
// macroState = -1;
// }
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java
View File

@ -179,6 +179,9 @@ public class BlueTeleop extends OpMode {
}
//Elbowpos