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

before LM3 all subsystems working, auto red 2+0, 2+2 in progress

This commit is contained in:
sihan 2024-01-10 20:05:14 -06:00
parent 24540bf5ca
commit fe7ef634ab
9 changed files with 259 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/build.gradle
View File

@ -26,7 +26,7 @@ android {
dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.ftclib.ftclib:core:2.0.1' // core
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'org.openftc:easyopencv:1.7.0'

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

@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCOREAUTO;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
@ -13,6 +13,8 @@ import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
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.SLIDEUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP;
@ -28,6 +30,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT;
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.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -106,6 +109,10 @@ public class Robot {
public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);}
public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);}
public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);}
public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);}
}
@ -186,15 +193,15 @@ public class Robot {
this.rightArm.setPosition(ARMSCORE);
}
public void armAccurateScore() {
public void armSecondaryScore() {
this.leftArm.setPosition(ARMACCSCORE);
this.rightArm.setPosition(ARMACCSCORE);
}
public void armAccurateScoreAuto() {
this.leftArm.setPosition(ARMACCSCOREAUTO);
this.rightArm.setPosition(ARMACCSCOREAUTO);
public void armPickupStack() {
this.leftArm.setPosition(ARMPICKUPSTACK);
this.rightArm.setPosition(ARMPICKUPSTACK);
}
public void armRest() {
@ -221,7 +228,10 @@ public class Robot {
}
}
@Config
public static class Claw {
private static final double CLAW_KP = 0.15;
private Servo claw;
public Claw init(HardwareMap hardwareMap) {
@ -238,9 +248,14 @@ public class Robot {
this.claw.setPosition(OPEN);
}
public void openScore() {
public void openStack() {
this.claw.setPosition(BIGOPEN);
}
public void setPos(double pos) {
this.claw.setPosition(pos);
}
}
public static class Plane {

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

@ -58,7 +58,7 @@ public class AutoBlue extends LinearOpMode {
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -74,7 +74,7 @@ public class AutoBlue extends LinearOpMode {
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -89,7 +89,7 @@ public class AutoBlue extends LinearOpMode {
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();

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

@ -72,7 +72,7 @@ public class AutoBlueFar extends LinearOpMode {
this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(-50, -28, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -99,7 +99,7 @@ public class AutoBlueFar extends LinearOpMode {
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(-50, -33, Math.toRadians(180)))
.addTemporalMarker(.02, robot.getArm()::armAccurateScore)
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.02, robot.getWrist()::wristScore)
.build();
@ -126,7 +126,7 @@ public class AutoBlueFar extends LinearOpMode {
this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(-50.5, -39, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();

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

@ -60,12 +60,13 @@ public class AutoRed extends LinearOpMode {
.build();
this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
.lineToLinearHeading(new Pose2d(31, -32, Math.toRadians(360)))
.lineToLinearHeading(new Pose2d(29.5, -29, Math.toRadians(360)))
.build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(72, -28, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -76,12 +77,13 @@ public class AutoRed extends LinearOpMode {
//Randomization Two
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(31, -28, Math.toRadians(270)))
.lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270)))
.build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(70.75, -36.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScoreAuto)
.lineToLinearHeading(new Pose2d(75, -36.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -95,8 +97,9 @@ public class AutoRed extends LinearOpMode {
.build();
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(73, -42, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -114,24 +117,6 @@ public class AutoRed extends LinearOpMode {
.lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
.build();
// //Cycle
// this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end())
// .lineToLinearHeading(new Pose2d(-37,-7, Math.toRadians(360)))
// .addTemporalMarker(.3, robot.getArm()::armRest)
// .addTemporalMarker(.3, robot.getWrist()::wristPickup)
// .build();
// this.backGate = this.robot.getDrive().trajectoryBuilder(goGate.end())
// .lineToLinearHeading(new Pose2d(50,-10, Math.toRadians(360)))
// .build();
// this.approachBoard = this.robot.getDrive().trajectoryBuilder(backGate.end())
// .lineToLinearHeading(new Pose2d(68, -28, Math.toRadians(360)))
// .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
// .addTemporalMarker(.2, robot.getWrist()::wristScore)
// .build();
// this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end())
// .lineToLinearHeading(new Pose2d(72.5, -28, Math.toRadians(360)))
// .build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
@ -148,6 +133,7 @@ public class AutoRed extends LinearOpMode {
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffOne);
this.robot.getSlides().slideDown();
sleep(300);
break;
case "CENTER":
@ -157,6 +143,7 @@ public class AutoRed extends LinearOpMode {
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffTwo);
this.robot.getSlides().slideDown();
sleep(300);
break;
case "RIGHT":
@ -166,6 +153,7 @@ public class AutoRed extends LinearOpMode {
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffThree);
this.robot.getSlides().slideDown();
sleep(300);
break;
}

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

@ -73,7 +73,7 @@ public class AutoRedFar extends LinearOpMode {
this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(50, -28, Math.toRadians(360)))
.addTemporalMarker(.02, robot.getArm()::armAccurateScore)
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.02, robot.getWrist()::wristScore)
.build();
@ -101,7 +101,7 @@ public class AutoRedFar extends LinearOpMode {
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(50, -34, Math.toRadians(360)))
.addTemporalMarker(.02, robot.getArm()::armAccurateScore)
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.02, robot.getWrist()::wristScore)
.build();
@ -128,7 +128,7 @@ public class AutoRedFar extends LinearOpMode {
this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(50, -39, Math.toRadians(360)))
.addTemporalMarker(.02, robot.getArm()::armAccurateScore)
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.02, robot.getWrist()::wristScore)
.build();

204
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java Normal file
View File

@ -0,0 +1,204 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@Autonomous(name = "autoRed2+2")
public class AutoRedTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;
protected Trajectory preloadOne;
protected Trajectory scoreOne;
protected Trajectory boardOne;
protected Trajectory backOffOne;
protected Trajectory preloadTwo;
protected Trajectory scoreTwo;
protected Trajectory backOffTwo;
protected Trajectory preloadThree;
protected Trajectory scoreThree;
protected Trajectory boardThree;
protected Trajectory backOffThree;
protected Trajectory park1;
protected Trajectory park2;
protected Trajectory goGate;
protected Trajectory goStack;
protected Trajectory backGate;
protected Trajectory approachBoard;
protected Trajectory scoreStack;
private Robot robot;
private String randomization;
private int random;
@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();
//Trajectories
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
//Randomization One
this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
.build();
this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
.lineToLinearHeading(new Pose2d(29.5, -29, Math.toRadians(360)))
.build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
.lineToLinearHeading(new Pose2d(60, -15, Math.toRadians(360)))
.build();
//Randomization Two
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270)))
.build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(75.7, -36.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
.lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360)))
.build();
//Randomization Three
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270)))
.build();
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armScore)
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
.lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360)))
.build();
//Park
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
.lineToLinearHeading(new Pose2d(65, -10, Math.toRadians(360)))
.addTemporalMarker(.3, robot.getArm()::armRest)
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
.addTemporalMarker(.1,robot.getSlides()::slideDown)
.build();
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
.lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
.build();
// //Cycle
this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end())
.lineToLinearHeading(new Pose2d(-41,-8, Math.toRadians(360)))
.addTemporalMarker(.5,robot.getClaw()::openStack)
.addTemporalMarker(.2,robot.getArm()::armPickupStack)
.addTemporalMarker(.1,robot.getSlides()::slideDown)
.build();
this.backGate = this.robot.getDrive().trajectoryBuilder(goGate.end())
.lineToLinearHeading(new Pose2d(50,-10, Math.toRadians(360)))
.build();
this.approachBoard = this.robot.getDrive().trajectoryBuilder(backGate.end())
.lineToLinearHeading(new Pose2d(73.5, -28, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks)
.build();
this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end())
.lineToLinearHeading(new Pose2d(72.5, -28, 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());
this.telemetry.update();
}
switch (randomization) {
case "LEFT":
this.robot.getDrive().followTrajectory(preloadOne);
this.robot.getDrive().followTrajectory(scoreOne);
this.robot.getDrive().followTrajectory(boardOne);
sleep(500);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffOne);
this.robot.getSlides().slideDown();
sleep(300);
break;
case "CENTER":
this.robot.getDrive().followTrajectory(preloadTwo);
this.robot.getDrive().followTrajectory(scoreTwo);
sleep(500);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffTwo);
this.robot.getSlides().slideDown();
sleep(300);
break;
case "RIGHT":
this.robot.getDrive().followTrajectory(preloadThree);
this.robot.getDrive().followTrajectory(scoreThree);
sleep(500);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffThree);
this.robot.getSlides().slideDown();
sleep(300);
break;
}
//Cycle
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(goGate);
sleep(500);
this.robot.getClaw().close();
sleep(250);
this.robot.getDrive().followTrajectory(backGate);
this.robot.getDrive().followTrajectory(approachBoard);
this.robot.getClaw().setPos(.86);
sleep(200);
double currentPos = .86;
double targetPos = .78;
double delta = (targetPos - currentPos) / 100;
for (int i = 0 ; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
sleep(12);
}
sleep(300);
this.robot.getDrive().followTrajectory(park1);
}
}

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

@ -15,17 +15,18 @@ public class Configurables {
//Servo Positions
public static double ARMREST = 0.8;
public static double ARMSCORE = 0.38;
public static double ARMACCSCORE = 0.6;
public static double ARMACCSCOREAUTO = 0.6;
public static double ARMSCORE = 0.39;
public static double ARMACCSCORE = .39;
public static double ARMPICKUPSTACK = 0.825;
public static double PICKUP = 0.84;
public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96;
public static double OPEN = 0.85;
public static double BIGOPEN = 0.45;
public static double BIGOPEN = 0.73;
public static double CLOSE = 0.95;
public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9;
public static double OPENSTAGEONE = .78;
//Drive Speed
public static double SPEED = 1;
@ -36,9 +37,11 @@ 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 = 300;
public static int SLIDEUP = 1150;
public static int HANGRELEASE = 2500;
public static int HANG = 1000;
public static int HANG = 0;
public static int HANGPLANE = 1900;

2
build.common.gradle
View File

@ -57,6 +57,8 @@ android {
minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28
multiDexEnabled true
/**
* We keep the versionCode and versionName of robot controller applications in sync with