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

final robot code for Ostrick O7

This commit is contained in:
sihan 2024-02-17 10:48:33 -06:00
parent f8c5ba3296
commit 712fd3e0a5
14 changed files with 914 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -15,6 +15,8 @@ import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH;
import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEAUTOSTACKS;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERONE;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERTWO;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEPICKUPSTACKSTWO;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP;
@ -24,6 +26,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.LIGHTS;
import static org.firstinspires.ftc.teamcode.util.Constants.PLANE;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT;
@ -31,6 +34,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -45,8 +49,8 @@ import lombok.Getter;
public class Robot {
@Getter
private MecanumDrive drive;
// @Getter
// private Intake intake;
@Getter
private Led led;
@Getter
private Arm arm;
@Getter
@ -71,6 +75,7 @@ public class Robot {
this.camera = new Camera(hardwareMap);
this.plane = new Plane().init(hardwareMap);
this.slides= new Slides().init(hardwareMap);
this.led = new Led().init(hardwareMap);
return this;
}
@ -112,8 +117,12 @@ public class Robot {
public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);}
public void slideScoreStack(){this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);}
public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);}
public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);}
public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);}
}
@ -258,6 +267,22 @@ public class Robot {
}
public static class Led {
private RevBlinkinLedDriver led;
public Led init(HardwareMap hardwareMap) {
this.led = hardwareMap.get(RevBlinkinLedDriver.class, LIGHTS);
return this;
}
public void gold() {
this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET);
}
public void white() {
this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.STROBE_WHITE);
}
}
public static class Plane {
private Servo plane;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -57,7 +57,7 @@ public class DriveConstants {
* motor encoders or have elected not to use them for velocity control, these values should be
* empirically tuned.
*/
public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kV = 1 / rpmToVelocity(MAX_RPM);
public static double kA = 0;
public static double kStatic = 0;
@ -68,8 +68,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 40;
public static double MAX_ACCEL = 40;
public static double MAX_VEL = 50;
public static double MAX_ACCEL = 50;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java
View File

@ -19,51 +19,38 @@ public class AutoBlue extends LinearOpMode {
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
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(220));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
//Board Scores
final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185));
final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185));
final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185));
//Park
final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -26.5, Math.toRadians(180));
final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(178));
final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(180));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
protected void boardScore() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
builder.lineToLinearHeading(RIGHT_BOARD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
builder.lineToLinearHeading(CENTER_BOARD);
break;
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
builder.lineToLinearHeading(LEFT_BOARD);
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
builder.addTemporalMarker(.75, robot.getArm()::armScore);
builder.addTemporalMarker(2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -88,23 +75,18 @@ public class AutoBlue extends LinearOpMode {
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
this.telemetry.update();
}
scorePreloadOne();
boardScore();
sleep(250);
sleep(100);
this.robot.getClaw().open();
sleep(250);
sleep(100);
park();
sleep(300);
}
}

69
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java
View File

@ -1,69 +0,0 @@
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.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoBlueFar")
public class AutoBlueFar extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD);
break;
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(initialPosition);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@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()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
this.telemetry.update();
}
scorePreloadOne();
goBackToWhereYouCameFrom();
}
}

159
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java Normal file
View File

@ -0,0 +1,159 @@
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.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "autoBlueFar")
public class AutoBlueFarStem extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
private int delay = 10000;
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
final static Pose2d LEFT_PRELOAD_GETOUT = new Pose2d(43, -42, Math.toRadians(330));
final static Pose2d CENTER_PRELOAD = new Pose2d(35, -27, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(44, -35, Math.toRadians(270));
//Ready Truss
final static Pose2d READY_TRUSS = new Pose2d(43, -57, Math.toRadians(180));
final static Pose2d READY_TRUSSTEMP = new Pose2d(35, -57, Math.toRadians(180));
final static Pose2d TO_BOARD = new Pose2d(-35, -57, Math.toRadians(180));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(-52, -38, Math.toRadians(180));
final static Pose2d SCORE_BOARD_MID = new Pose2d(-52, -32, Math.toRadians(180));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(-52, -27, Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-55, -56, Math.toRadians(180));
final static Pose2d PARK2 = new Pose2d(-60, -59, Math.toRadians(180));
final static Pose2d PARKLEFT = new Pose2d(-45, -10, Math.toRadians(180));
final static Pose2d PARKLEFT2 = new Pose2d(-60, -10, Math.toRadians(180));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
builder.lineToLinearHeading(LEFT_PRELOAD_GETOUT);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD);
break;
}
builder.addTemporalMarker(.3, this.robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_TRUSSTEMP);
// builder.lineToLinearHeading(TO_BOARD,
// MecanumDrive.getVelocityConstraint(70, 70, DriveConstants.TRACK_WIDTH),
// MecanumDrive.getAccelerationConstraint(70));
builder.addTemporalMarker(.3,robot.getArm()::armRest);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
// protected void scoreBoard(){
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// switch (randomization) {
// case "LEFT":
// builder.lineToLinearHeading(SCORE_BOARD_LEFT,
// MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
// MecanumDrive.getAccelerationConstraint(20));
// break;
// case "CENTER":
// builder.lineToLinearHeading(SCORE_BOARD_MID,
// MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
// MecanumDrive.getAccelerationConstraint(20));
// break;
// case "RIGHT":
// builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
// MecanumDrive.getVelocityConstraint(30, 30, DriveConstants.TRACK_WIDTH),
// MecanumDrive.getAccelerationConstraint(30));
// break;
// }
// builder.addTemporalMarker(.2, robot.getArm()::armScore);
// builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
// builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
// this.robot.getDrive().followTrajectorySequence(builder.build());
// }
//
// protected void park(){
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// switch(parkLocation) {
// case "RIGHT":
// builder.lineToLinearHeading(PARKLEFT);
// builder.lineToLinearHeading(PARKLEFT2);
// break;
// case "LEFT":
// builder.lineToLinearHeading(PARK);
// builder.lineToLinearHeading(PARK2);
// break;
// }
// builder.addTemporalMarker(.1, robot.getArm()::armRest);
// builder.addTemporalMarker(.1, robot.getClaw()::close);
// builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
// builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
// this.robot.getDrive().followTrajectorySequence(builder.build());
//
// }
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()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
if (gamepad2.dpad_left) {
parkLocation="LEFT";
} else if (gamepad2.dpad_right) {
parkLocation = "RIGHT";
}
delaySet();
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
}
sleep(delay);
scorePreloadOne();
goBackToWhereYouCameFrom();
// scoreBoard();
// sleep(500);
// this.robot.getClaw().open();
// sleep(500);
// park();
}
}

91
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java
View File

@ -21,23 +21,25 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(180));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(210));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-30.5, -31, Math.toRadians(210));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26.8, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
//Board Scores
final static Pose2d RIGHT_BOARD = new Pose2d(-75.7, -25.7, Math.toRadians(185));
final static Pose2d CENTER_BOARD = new Pose2d(-75.7, -35, Math.toRadians(185));
final static Pose2d LEFT_BOARD = new Pose2d(-75.7, -42, Math.toRadians(185));
final static Pose2d RIGHT_BOARD = new Pose2d(-75.5, -26.5, Math.toRadians(182));
final static Pose2d CENTER_BOARD = new Pose2d(-77, -33.5, Math.toRadians(185));
final static Pose2d LEFT_BOARD = new Pose2d(-76, -41, Math.toRadians(185));
//Stack Cycle
final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180));
final static Pose2d TO_STACK = new Pose2d(40.75, -7.25, Math.toRadians(180));
final static Pose2d TO_STACK = new Pose2d(35, -6.5, Math.toRadians(180));
final static Pose2d TO_STACK_SLOW = new Pose2d(40, -7.5, Math.toRadians(180));
final static Pose2d TO_STACK_SLOW2 = new Pose2d(40, -8.5, Math.toRadians(183));
final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180));
final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -28, Math.toRadians(180));
final static Pose2d SCORE_STACK = new Pose2d(-73, -31, Math.toRadians(180));
final static Pose2d SCORE_STACK = new Pose2d(-73.5, -29, Math.toRadians(180));
//Park
final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-80, -64, Math.toRadians(180));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
@ -53,20 +55,27 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
}
builder.addTemporalMarker(.5, robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void boardScore() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD);
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));;
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD);
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD);
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
@ -84,6 +93,24 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void toStackNoDrift() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE_BOARD);
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()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW2,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -94,13 +121,13 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(SCORE_STACK);
builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreTest() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-77, -31, Math.toRadians(180)),
builder.lineToLinearHeading(new Pose2d(-77.5, -31, Math.toRadians(183)),
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.addTemporalMarker(.2,this::clawSlowOpen);
@ -108,14 +135,12 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
}
protected void clawSlowOpen() {
double currentPos = .86;
double targetPos = .8;
double currentPos = .8;
double targetPos = .73;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
// int Position = this.robot.getSlides().slidesLeft.getCurrentPosition();
this.robot.getClaw().setPos(currentPos + (delta * i));
// this.robot.getSlides().slideTo(Position + 14,1);
sleep(35);
sleep(30);
}
}
@ -140,32 +165,38 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
this.telemetry.update();
}
scorePreloadOne();
boardScore();
sleep(100);
this.robot.getClaw().open();
toStack();
switch (randomization) {
case "RIGHT":
toStackNoDrift();
break;
case "CENTER":
toStack();
break;
case "LEFT":
toStack();
break;
}
sleep(500);
this.robot.getClaw().close();
sleep(250);
this.robot.getArm().armRest();
scoreStack();
this.robot.getClaw().setPos(.86);
this.robot.getClaw().setPos(.83);
scoreTest();
park();
}
}

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -8,6 +8,8 @@ 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
@ -16,21 +18,24 @@ public class AutoRed extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -35, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355));
final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355));
final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355));
final static Pose2d LEFT_BOARD = new Pose2d(75.8, -26.5, Math.toRadians(358));
final static Pose2d CENTER_BOARD = new Pose2d(75.8, -36.3, Math.toRadians(358));
final static Pose2d RIGHT_BOARD = new Pose2d(75.8, -40, Math.toRadians(358));
//Park
final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360));
final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
final static Pose2d PARK = new Pose2d(60,-58,Math.toRadians(360));
final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
final static Pose2d PARKLEFT = new Pose2d(60,-15,Math.toRadians(360));
final static Pose2d PARKLEFT2 = new Pose2d(80, -10, Math.toRadians(360));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
@ -53,13 +58,19 @@ public class AutoRed extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD);
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD);
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD);
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
@ -70,14 +81,28 @@ public class AutoRed extends LinearOpMode {
protected void park() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(BACK_OFF);
builder.lineToLinearHeading(PARK);
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";
}}
@Override
public void runOpMode() throws InterruptedException {
@ -88,14 +113,12 @@ public class AutoRed extends LinearOpMode {
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
parkLocation();
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update();
}

131
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java
View File

@ -8,67 +8,164 @@ 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 = "autRedFar")
@Autonomous(name = "AutoRedFar")
public class AutoRedFar 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, -37.5, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
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(230));
final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
//Ready Truss
final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0));
final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0));
final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5));
final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, 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 "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT);
break;
}
builder.addTemporalMarker(.4,robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(initialPosition);
builder.lineToLinearHeading(LEAVE);
builder.lineToLinearHeading(READY_TRUSS);
builder.lineToLinearHeading(TO_BOARD);
builder.addTemporalMarker(.3,robot.getArm()::armRest);
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(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "CENTER":
builder.lineToLinearHeading(SCORE_BOARD_MID,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(1.8, robot.getClaw()::open);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
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);
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
parkLocation();
delaySet();
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();
}
}
sleep(delay);
scorePreloadOne();
goBackToWhereYouCameFrom();
scoreBoard();
score();
sleep(500);
park();
}
}

252
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java Normal file
View File

@ -0,0 +1,252 @@
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;
//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();
}
}

170
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java Normal file
View File

@ -0,0 +1,170 @@
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 = "AutoRedFar1+0")
public class AutoRedFaronepluszero 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(230));
final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
//Ready Truss
final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0));
final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0));
final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5));
final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, 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);
builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT);
break;
}
builder.addTemporalMarker(.4,robot.getArm()::armScore);
// builder.addTemporalMarker(2,robot.getArm()::armRest);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE);
// builder.lineToLinearHeading(READY_TRUSS);
// builder.lineToLinearHeading(TO_BOARD);
builder.addTemporalMarker(.3,robot.getArm()::armRest);
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(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "CENTER":
builder.lineToLinearHeading(SCORE_BOARD_MID,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(1.8, robot.getClaw()::open);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
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();
delaySet();
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();
}
// sleep(delay);
scorePreloadOne();
goBackToWhereYouCameFrom();
// scoreBoard();
// score();
// sleep(500);
// park();
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java
View File

@ -22,15 +22,17 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
final static Pose2d CENTER_PRELOAD = new Pose2d(34, -28, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(75.3, -26.5, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(75.3, -36.3, Math.toRadians(360));
final static Pose2d RIGHT_BOARD = new Pose2d(75.3, -40, Math.toRadians(355));
final static Pose2d LEFT_BOARD = new Pose2d(76.7, -27, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(76.5, -32, Math.toRadians(355));
final static Pose2d RIGHT_BOARD = new Pose2d(76.5, -40, Math.toRadians(355));
//Stack Cycle
final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360));
final static Pose2d TO_STACK = new Pose2d(-40, -8.4, Math.toRadians(360));
final static Pose2d TO_STACK = new Pose2d(-35, -8, Math.toRadians(360));
final static Pose2d TO_STACK_SLOW = new Pose2d(-38.5, -8.5, Math.toRadians(360));
final static Pose2d TO_STACK_SLOW2 = new Pose2d(-38.5, -8, Math.toRadians(360));
final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360));
final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360));
@ -52,6 +54,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(RIGHT_PRELOAD);
break;
}
builder.addTemporalMarker(.5, robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -59,13 +62,19 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD);
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));;
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD);
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD);
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
@ -83,6 +92,24 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void toStackNoDrift() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE_BOARD);
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()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW2,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -93,7 +120,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(SCORE_STACK);
builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -107,8 +134,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
}
protected void clawSlowOpen() {
double currentPos = .86;
double targetPos = .78;
double currentPos = .8;
double targetPos = .7;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
@ -121,6 +148,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(BACK_OFF);
builder.lineToLinearHeading(PARK);
builder.addTemporalMarker(.1, robot.getArm()::armRest);
builder.addTemporalMarker(.1, robot.getClaw()::close);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequence(builder.build());
@ -136,10 +164,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
@ -150,17 +174,28 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
scorePreloadOne();
boardScore();
sleep(100);
sleep(150);
this.robot.getClaw().open();
sleep(150);
toStack();
switch (randomization) {
case "LEFT":
toStackNoDrift();
break;
case "CENTER":
toStack();
break;
case "RIGHT":
toStack();
break;
}
sleep(500);
this.robot.getClaw().close();
sleep(250);
this.robot.getArm().armRest();
scoreStack();
this.robot.getClaw().setPos(.86);
this.robot.getClaw().setPos(.83);
scoreTest();
park();
}

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

@ -45,13 +45,15 @@ public class MainTeleOp extends OpMode {
} else if (restArm) {
this.robot.getArm().armRest();
} else if (scoreArm) {
this.robot.getArm().armScore();
this.robot.getArm().armSecondaryScore();
}
//Claw
if (claw) {
this.robot.getClaw().open();
this.robot.getLed().white();
} else {
this.robot.getClaw().close();
this.robot.getLed().gold();
}
//Wrist
if (pickupWrist) {

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -15,18 +15,17 @@ public class Configurables {
//Servo Positions
public static double ARMREST = 0.8;
public static double ARMSCORE = 0.39;
public static double ARMACCSCORE = .38;
public static double ARMPICKUPSTACK = 0.8415;
public static double PICKUP = 0.84;
public static double ARMSCORE = 0.4;
public static double ARMACCSCORE = .37;
public static double ARMPICKUPSTACK = 0.815;
public static double PICKUP = 0.835;
public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96;
public static double OPEN = 0.85;
public static double BIGOPEN = 0.67;
public static double CLOSE = 0.95;
public static double OPEN = 0.82;
public static double BIGOPEN = 0.65;
public static double CLOSE = 0.91;
public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9;
public static double OPENSTAGEONE = .78;
//Drive Speed
public static double SPEED = 1;
@ -37,12 +36,14 @@ public class Configurables {
//Motor Positions
public static double SLIDE_POWER_UP = 1;
public static double SLIDE_POWER_DOWN = .7;
public static int SLIDELAYERONE = 150;
public static int SLIDEAUTOSTACKS = 350;
public static int SLIDELAYERONE = 60;
public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 1150;
public static int HANGRELEASE = 2500;
public static int HANG = 0;
public static int HANGPLANE = 1900;
public static int HANGPLANE = 1800;
public static int SLIDELAYERTWO = 350;
public static int SLIDESTACK = 80;
public static int SLIDEPICKUPSTACKSTWO = 30;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -61,4 +61,6 @@ public class Constants {
public static final String SLIDELEFT = "slideleft";
public static final String HANGRIGHT = "hangright";
public static final String HANGLEFT = "hangleft";
public static final String LIGHTS = "lights";
}