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

??

This commit is contained in:
sihan 2024-03-04 10:25:54 -06:00
parent 0ef239ca91
commit a8e3634faf
5 changed files with 402 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK;
@ -349,6 +350,7 @@ public class Robot {
case NEUTRAL:
if (runtime > delay) {
this.getArm().armRest();
gamepad1.rumble(300);
pickupMacroState = pickupMacroStates.IDLE;
}
break;

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java Normal file
View File

@ -0,0 +1,47 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState;
@Config
public abstract class AutoBase extends LinearOpMode {
protected Pose2d initialPosition;
Robot robot;
String randomization;
String parkLocation;
double runtime;
autoState state = autoState.PURPLE;
int delay;
public abstract void followTrajectories();
@Override
public void runOpMode() {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
while (!this.isStarted()) {
parkLocation = "RIGHT";
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
}
while (state != autoState.STOP) {
followTrajectories();
robot.getDrive().update();
}
}
}

125
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java
View File

@ -1,26 +1,16 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "AutoRedFar2+2")
public class AutoRedFarTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
private int delay = 10000;
@Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp")
public class AutoRedFarTwoPlusTwo extends AutoBase {
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
@ -46,6 +36,15 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode {
final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0));
final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0));
protected enum autoState{
STOP,
PURPLE,
YELLOW,
FLIPYELLOW,
SCOREYELLOW,
PARK,
}
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
@ -206,7 +205,6 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode {
} else if (gamepad2.dpad_right) {
parkLocation = "RIGHT";
}}
protected void delaySet(){
if (gamepad2.dpad_up && delay<13000) {
delay+=1000;
@ -216,37 +214,82 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode {
sleep(100);
}
}
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
parkLocation = "RIGHT";
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
public void followTrajectories(){
TrajectorySequenceBuilder builder;
switch (state){
case PURPLE:
builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
break;
}
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
if (!robot.getDrive().isBusy()){
state = autoState.YELLOW;
}
break;
case YELLOW:
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE);
builder.lineToLinearHeading(READY_TRUSS);
builder.lineToLinearHeading(TO_BOARD);
switch (randomization) {
case "LEFT":
builder.splineToConstantHeading(SCORE_BOARD_LEFT.vec(),Math.toRadians(0));
break;
case "CENTER":
builder.splineToConstantHeading(SCORE_BOARD_MID.vec(),Math.toRadians(0));
break;
case "RIGHT":
builder.splineToConstantHeading(SCORE_BOARD_RIGHT.vec(),Math.toRadians(0));
break;
}
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
runtime = getRuntime();
state = autoState.FLIPYELLOW;
break;
case FLIPYELLOW:
if (getRuntime() > runtime + 5) {
this.robot.getArm().armSecondaryScore();
this.robot.getWrist().wristScore();
state = autoState.SCOREYELLOW;
}
break;
case SCOREYELLOW:
if (!robot.getDrive().isBusy()){
this.robot.getClaw().open();
state = autoState.PARK;
}
break;
case PARK:
}
scorePreloadOne();
goBackToWhereYouCameFrom();
// scoreBoard();
score();
this.robot.getClaw().open();
backTruss();
this.robot.getClaw().close();
sleep(200);
goBackstage();
scoreBoardStack();
scoreTest();
park();
}
}
// scorePreloadOne();
// goBackToWhereYouCameFrom();
//// scoreBoard();
// score();
// this.robot.getClaw().open();
// backTruss();
// this.robot.getClaw().close();
// sleep(200);
// goBackstage();
// scoreBoardStack();
// scoreTest();
// park();
}

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
View File

@ -20,12 +20,12 @@ public class MainTeleOp extends OpMode {
GamepadEx controller2 = new GamepadEx(gamepad2);
public void loop() {
boolean slideUp = gamepad2.dpad_up;
boolean plane = gamepad2.right_bumper;
boolean slideDown = gamepad2.dpad_left;
boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
boolean hang = gamepad2.left_bumper;
boolean hangRelease = gamepad2.y;
boolean hangPlane = gamepad2.dpad_right;
boolean hangRelease = gamepad2.right_bumper;
boolean hangPlane = gamepad2.y;
boolean plane = gamepad2.dpad_right;
//Drive
robot.getDrive().setInput(gamepad1, gamepad2);
//slides
@ -33,26 +33,28 @@ public class MainTeleOp extends OpMode {
this.robot.getSlides().slideUp();
} else if (slideDown) {
this.robot.getSlides().slideDown();
} else {
} else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)
|| controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
this.robot.getSlides().slideStop();
}
//Macros
this.robot.getLed().LED();
this.robot.pickupMacro(controller2,getRuntime());
this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
//Arm and Wrist
if (controller2.wasJustPressed(GamepadKeys.Button.X)){
if (controller2.wasJustPressed(GamepadKeys.Button.X)) {
this.robot.getArm().armSecondaryScore();
this.robot.getWrist().wristScore();
} else if (controller2.wasJustPressed(GamepadKeys.Button.A)){
} else if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
this.robot.getArm().armRest();
this.robot.getWrist().wristPickup();
this.robot.getClaw().close();
}
//Claw
if (controller2.wasJustPressed(GamepadKeys.Button.B)){
if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
gamepad1.rumble(300);
} else if (controller2.isDown(GamepadKeys.Button.B)){
this.robot.getClaw().open();
} else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
this.robot.getClaw().close();
}
//Hang
if (hang) {

254
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java Normal file
View File

@ -0,0 +1,254 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "TestRedFar2+2")
public class RedFarTwoPlusTwoTest extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
private int delay = 10000;
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200));
final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0));
final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270));
//Ready Truss
final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0));
final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0));
final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5));
final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5));
final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0));
final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0));
final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0));
final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0));
final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0));
final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0));
final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0));
final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0));
final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
break;
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE,
MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
builder.lineToLinearHeading(READY_TRUSS,
MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
builder.lineToLinearHeading(TO_BOARD,
MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreBoard(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_SCORE);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void score(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(SCORE_BOARD_LEFT,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToLinearHeading(SCORE_BOARD_MID,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void backTruss(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(TO_BOARD);
builder.lineToLinearHeading(READY_TRUSS,
MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.lineToLinearHeading(GET_STACK);
switch (randomization){
case "MID":
builder.lineToLinearHeading(PICKUP_STACK_MID,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(PICKUP_STACK,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
default:
builder.lineToLinearHeading(PICKUP_STACK_LEFT,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_TRUSS);
builder.lineToLinearHeading(TO_BOARD);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreBoardStack(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_SCORESTACK);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreTest() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)),
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.addTemporalMarker(.3,this::clawSlowOpen);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void clawSlowOpen() {
double currentPos = .81;
double targetPos = .73;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
sleep(30);
}
}
protected void park(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch(parkLocation) {
case "LEFT":
builder.lineToLinearHeading(PARKLEFT);
builder.lineToLinearHeading(PARKLEFT2);
break;
case "RIGHT":
builder.lineToLinearHeading(PARK);
builder.lineToLinearHeading(PARK2);
break;
}
builder.addTemporalMarker(.1, robot.getArm()::armRest);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void parkLocation(){
if (gamepad2.dpad_left) {
parkLocation="LEFT";
} else if (gamepad2.dpad_right) {
parkLocation = "RIGHT";
}}
protected void delaySet(){
if (gamepad2.dpad_up && delay<13000) {
delay+=1000;
sleep(100);
} else if (gamepad2.dpad_down && delay>0) {
delay-=1000;
sleep(100);
}
}
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
parkLocation = "RIGHT";
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
}
scorePreloadOne();
goBackToWhereYouCameFrom();
// scoreBoard();
score();
this.robot.getClaw().open();
backTruss();
this.robot.getClaw().close();
sleep(200);
goBackstage();
scoreBoardStack();
scoreTest();
park();
}
}