KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

bunch of minor changes plus major change in running auton with customizable settings

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-06 21:39:23 -06:00
parent bf8f9c1129
commit e4dd2147d6
11 changed files with 607 additions and 501 deletions
Download Patch File Download Diff File Expand all files Collapse all files

556
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -1,62 +1,69 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -64,6 +71,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull; import androidx.annotation.NonNull;
@@ -82,7 +90,6 @@ import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -95,7 +102,7 @@ import java.util.Objects;
@Config @Config
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2; public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0SpinSpeedIncrease = 0.015;
@@ -110,7 +117,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double blueObeliskTurrPos1 = -0.12; public static double blueObeliskTurrPos1 = -0.12;
public static double blueObeliskTurrPos2 = -0.13; public static double blueObeliskTurrPos2 = -0.13;
public static double blueObeliskTurrPos3 = -0.14; public static double blueObeliskTurrPos3 = -0.14;
public static double redTurretShootPos = 0.12; public static double redTurretShootPos = 0.1;
public static double blueTurretShootPos = -0.14; public static double blueTurretShootPos = -0.14;
double obeliskTurrPos1 = 0.0; double obeliskTurrPos1 = 0.0;
@@ -163,21 +170,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
Targeting.Settings targetingSettings; Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos; private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true; private boolean shootForward = true;
private double x1, y1, h1; double x1, y1, h1;
private double x2a, y2a, h2a, t2a; double x2a, y2a, h2a, t2a;
private double x2b, y2b, h2b, t2b; double x2b, y2b, h2b, t2b;
private double x2c, y2c, h2c, t2c; double x2c, y2c, h2c, t2c;
private double x3a, y3a, h3a; double x3a, y3a, h3a;
private double x3b, y3b, h3b; double x3b, y3b, h3b;
private double x4a, y4a, h4a; double x4a, y4a, h4a;
private double x4b, y4b, h4b; double x4b, y4b, h4b;
private double xShoot, yShoot, hShoot; double xShoot, yShoot, hShoot;
private double xGate, yGate, hGate; double xGate, yGate, hGate;
private double xPrep, yPrep, hPrep; double xPrep, yPrep, hPrep;
double xLeave, yLeave, hLeave;
private double shoot1Tangent; private double shoot1Tangent;
@@ -186,6 +194,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
private int rearSlotGreen = 0; private int rearSlotGreen = 0;
private int mostGreenSlot = 0; private int mostGreenSlot = 0;
int ballCycles = 3;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() { return new Action() {
@@ -340,7 +350,25 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (getRuntime() - stamp < shootTime) { double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos); robot.spin1.setPosition(autoSpinStartPos);
@@ -403,6 +431,24 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) { if (getRuntime() - stamp < firstShootTime) {
@@ -457,11 +503,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
} }
}; };
} }
@@ -781,6 +824,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
turrDefault += 0.01; turrDefault += 0.01;
} }
if (gamepad2.rightBumperWasPressed()){
ballCycles++;
}
if (gamepad2.leftBumperWasPressed()){
ballCycles--;
}
if (redAlliance) { if (redAlliance) {
robot.light.setPosition(0.28); robot.light.setPosition(0.28);
@@ -814,6 +864,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = rShootX; xShoot = rShootX;
yShoot = rShootY; yShoot = rShootY;
hShoot = rShootH; hShoot = rShootH;
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -854,6 +907,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = bShootX; xShoot = bShootX;
yShoot = bShootY; yShoot = bShootY;
hShoot = bShootH; hShoot = bShootH;
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -863,33 +919,45 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
} }
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1); .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed)); new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) if (ballCycles < 2){
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a) .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b, .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickup1Speed)); new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) if (ballCycles < 3){
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
}
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
new TranslationalVelConstraint(pickup1Speed)); new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
TELE.addData("Red?", redAlliance); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault); TELE.addData("Turret Default", turrDefault);
TELE.addData("Ball Cycles", ballCycles);
TELE.update(); TELE.update();
} }
@@ -924,178 +992,184 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
); );
Actions.runBlocking( if (ballCycles > 0){
new ParallelAction( Actions.runBlocking(
pickup1.build(), new ParallelAction(
manageFlywheel( pickup1.build(),
shootAllVelocity, manageFlywheel(
shootAllHood, shootAllVelocity,
intake1Time, shootAllHood,
x2b, intake1Time,
y2b, x2b,
pickup1XTolerance, y2b,
pickup1YTolerance pickup1XTolerance,
), pickup1YTolerance
intake(intake1Time), ),
detectObelisk( intake(intake1Time),
intake1Time, detectObelisk(
0.501, intake1Time,
0.501, 0.501,
obelisk1XTolerance, 0.501,
obelisk1YTolerance, obelisk1XTolerance,
obeliskTurrPos1 obelisk1YTolerance,
) obeliskTurrPos1
)
) )
); );
motif = turret.getObeliskID(); motif = turret.getObeliskID();
if (motif == 0) motif = 22; if (motif == 0) motif = 22;
int prevMotif = motif; prevMotif = motif;
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageFlywheel( manageFlywheel(
shootAllVelocity, shootAllVelocity,
shootAllHood, shootAllHood,
shoot1Time, shoot1Time,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shoot1.build(), shoot1.build(),
prepareShootAll(colorSenseTime, shoot1Time, motif) prepareShootAll(colorSenseTime, shoot1Time, motif)
) )
); );
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageShooterAuto( manageShooterAuto(
shootAllTime, shootAllTime,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shootAllAuto(shootAllTime, spindexerSpeedIncrease) shootAllAuto(shootAllTime, spindexerSpeedIncrease)
) )
); );
}
Actions.runBlocking( if (ballCycles > 1){
new ParallelAction( Actions.runBlocking(
pickup2.build(), new ParallelAction(
manageShooterAuto( pickup2.build(),
intake2Time, manageShooterAuto(
x2b, intake2Time,
y2b, x2b,
pickup1XTolerance, y2b,
pickup1YTolerance pickup1XTolerance,
), pickup1YTolerance
intake(intake2Time), ),
detectObelisk( intake(intake2Time),
intake2Time, detectObelisk(
0.501, intake2Time,
0.501, 0.501,
obelisk1XTolerance, 0.501,
obelisk1YTolerance, obelisk1XTolerance,
obeliskTurrPos2 obelisk1YTolerance,
) obeliskTurrPos2
)
) )
); );
motif = turret.getObeliskID(); motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif; if (motif == 0) motif = prevMotif;
prevMotif = motif; prevMotif = motif;
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageFlywheelAuto( manageFlywheelAuto(
shoot2Time, shoot2Time,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shoot2.build(), shoot2.build(),
prepareShootAll(colorSenseTime, shoot2Time, motif) prepareShootAll(colorSenseTime, shoot2Time, motif)
) )
); );
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageShooterAuto( manageShooterAuto(
shootAllTime, shootAllTime,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shootAllAuto(shootAllTime, spindexerSpeedIncrease) shootAllAuto(shootAllTime, spindexerSpeedIncrease)
) )
); );
}
Actions.runBlocking( if (ballCycles > 2){
new ParallelAction( Actions.runBlocking(
pickup3.build(), new ParallelAction(
manageShooterAuto( pickup3.build(),
intake3Time, manageShooterAuto(
x2b, intake3Time,
y2b, x2b,
pickup1XTolerance, y2b,
pickup1YTolerance pickup1XTolerance,
), pickup1YTolerance
intake(intake3Time), ),
detectObelisk( intake(intake3Time),
intake3Time, detectObelisk(
0.501, intake3Time,
0.501, 0.501,
obelisk1XTolerance, 0.501,
obelisk1YTolerance, obelisk1XTolerance,
obeliskTurrPos3 obelisk1YTolerance,
) obeliskTurrPos3
)
) )
); );
motif = turret.getObeliskID(); motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif; if (motif == 0) motif = prevMotif;
prevMotif = motif; prevMotif = motif;
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageFlywheelAuto( manageFlywheelAuto(
shoot3Time, shoot3Time,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shoot3.build(), shoot3.build(),
prepareShootAll(colorSenseTime, shoot3Time, motif) prepareShootAll(colorSenseTime, shoot3Time, motif)
) )
); );
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
manageShooterAuto( manageShooterAuto(
finalShootAllTime, finalShootAllTime,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501,
0.501 0.501
), ),
shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
) )
); );
}
while (opModeIsActive()) { while (opModeIsActive()) {

111
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;

112
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -78,6 +78,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.constants.Poses_V2;
@@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects; import java.util.Objects;
@Disabled
@Config @Config
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_GateOpen extends LinearOpMode { public class Auto_LT_Close_GateOpen extends LinearOpMode {

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -1,9 +1,16 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -11,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull; import androidx.annotation.NonNull;
@@ -22,6 +30,8 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -38,7 +48,7 @@ import java.util.Objects;
@Config @Config
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far_3Ball extends LinearOpMode { public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
public static double autoSpinStartPos = 0.2; public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0SpinSpeedIncrease = 0.015;
@@ -46,12 +56,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
public static double redTurretShootPos = 0.05; public static double redTurretShootPos = 0.05;
public static double blueTurretShootPos = -0.05; public static double blueTurretShootPos = -0.05;
public static int fwdTime = 200, strafeTime = 2300; public static int fwdTime = 200, strafeTime = 2300;
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; double xLeave, yLeave, hLeave;
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
public static double rShootX = 1, rShootY = 1, rShootH = 1;
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
public static double bShootX = 1, bShootY = 1, bShootH = 1;
public static int sleepTime = 1300; public static int sleepTime = 1300;
public int motif = 0; public int motif = 0;
double turretShootPos = 0.0; double turretShootPos = 0.0;
@@ -228,7 +233,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (getRuntime() - stamp < shootTime) { double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos); robot.spin1.setPosition(autoSpinStartPos);
@@ -345,11 +368,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
} }
}; };
} }
@@ -642,6 +662,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
robot.light.setPosition(1); robot.light.setPosition(1);
TrajectoryActionBuilder leave = null;
while (opModeInInit()) { while (opModeInInit()) {
// Recalibration in initialization // Recalibration in initialization
@@ -669,37 +691,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
if (redAlliance) { if (redAlliance) {
robot.light.setPosition(0.28); robot.light.setPosition(0.28);
pickupGateX = rPickupGateX; xLeave = rLeaveX;
pickupGateY = rPickupGateY; yLeave = rLeaveY;
pickupGateH = rPickupGateH; hLeave = rLeaveH;
pickupZoneX = rPickupZoneX;
pickupZoneY = rPickupZoneY;
pickupZoneH = rPickupZoneH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
turretShootPos = turrDefault + redTurretShootPos; turretShootPos = turrDefault + redTurretShootPos;
} else { } else {
robot.light.setPosition(0.6); robot.light.setPosition(0.6);
pickupGateX = bPickupGateX; xLeave = bLeaveX;
pickupGateY = bPickupGateY; yLeave = bLeaveY;
pickupGateH = bPickupGateH; hLeave = bLeaveH;
pickupZoneX = bPickupZoneX;
pickupZoneY = bPickupZoneY;
pickupZoneH = bPickupZoneH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
turretShootPos = turrDefault + blueTurretShootPos; turretShootPos = turrDefault + blueTurretShootPos;
} }
leave = drive.actionBuilder(teleEnd)
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
TELE.addData("Red?", redAlliance); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault); TELE.addData("Turret Default", turrDefault);
TELE.addData("Start Position", teleEnd); TELE.addData("Start Position", teleEnd);
@@ -721,7 +731,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
manageFlywheel( manageFlywheel(
shoot0Vel, shoot0Vel,
shoot0Hood, shoot0Hood,
9, 5,
0.501, 0.501,
0.501, 0.501,
shoot0XTolerance, shoot0XTolerance,
@@ -736,9 +746,16 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
Actions.runBlocking( Actions.runBlocking(
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease)
); );
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
assert leave != null;
Actions.runBlocking(leave.build());
// Actual way to end autonomous in to find final position // Actual way to end autonomous in to find final position
while (opModeIsActive()) { while (opModeIsActive()) {

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java
View File

@@ -1,49 +1,49 @@
package org.firstinspires.ftc.teamcode.autonomous.disabled; package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java Normal file
View File

@@ -0,0 +1,10 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
public class Back_Poses {
public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50;
public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50;
}

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java Normal file
View File

@@ -0,0 +1,52 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double rx2b = 19, ry2b = 40, rh2b = 140;
public static double rx3a = 55, ry3a = 39, rh3a = 140;
public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
public static double rx4a = 80, ry4a = 48, rh4a = 140;
public static double rx4b = 45, ry4b = 83, rh4b = 140.1;
public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double bx2b = 19, by2b = -40, bh2b = -140.1;
public static double bx3a = 55, by3a = -39, bh3a = -140;
public static double bx3b = 41, by3b = -59, bh3b = -140.1;
public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
public static double bx4a = 75, by4a = -53, bh4a = -140;
public static double bx4b = 47, by4b = -85, bh4b = -140.1;
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
public static double rShootX = 40, rShootY = 10, rShootH = 50;
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
public static double bShootX = 40, bShootY = 0, bShootH = -50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
}

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -1,49 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1);
public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1);
public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140);
public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140);
public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50);
public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50);
public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
}

31
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,10 +1,10 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
@@ -70,8 +70,6 @@ public class TeleopV3 extends LinearOpMode {
boolean overrideTurr = false; boolean overrideTurr = false;
int intakeTicker = 0; int intakeTicker = 0;
boolean turretInterpolate = false;
private boolean shootAll = false; private boolean shootAll = false;
@Override @Override
@@ -141,7 +139,7 @@ public class TeleopV3 extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
double offset; //TURRET TRACKING
double robX = drive.localizer.getPose().position.x; double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y; double robY = drive.localizer.getPose().position.y;
@@ -318,18 +316,19 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("spinTestCounter", spindexer.counter);
// TELE.addData("autoSpintake", autoSpintake); // TELE.addData("autoSpintake", autoSpintake);
// //
TELE.addData("shootall commanded", shootAll); // TELE.addData("shootall commanded", shootAll);
// Targeting Debug // Targeting Debug
TELE.addData("robotX", robotX); // TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY); // TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData( "robotInchesY", targeting.robotInchesY); // TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate); // TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX); // TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Voltage", robot.voltage.getVoltage());
TELE.update(); TELE.update();

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -56,11 +57,11 @@ public class Robot {
public RevColorSensorV3 color3; public RevColorSensorV3 color3;
public Limelight3A limelight; public Limelight3A limelight;
public Servo light; public Servo light;
public VoltageSensor voltage;
public Robot(HardwareMap hardwareMap) { public Robot(HardwareMap hardwareMap) {
//Define components w/ hardware map //Define components w/ hardware map
//TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
frontRight = hardwareMap.get(DcMotorEx.class, "fr"); frontRight = hardwareMap.get(DcMotorEx.class, "fr");
backLeft = hardwareMap.get(DcMotorEx.class, "bl"); backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -78,7 +79,7 @@ public class Robot {
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
//TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -96,7 +97,6 @@ public class Robot {
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
//TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
spin1 = hardwareMap.get(Servo.class, "spin2"); spin1 = hardwareMap.get(Servo.class, "spin2");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -128,5 +128,6 @@ public class Robot {
} }
light = hardwareMap.get(Servo.class, "light"); light = hardwareMap.get(Servo.class, "light");
voltage = hardwareMap.voltageSensor.iterator().next();
} }
} }

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -77,6 +77,7 @@ public class Targeting {
double cancelOffsetY = 0.0; // was 7.0 double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95; double unitConversionFactor = 0.95;
int tileSize = 24; //inches int tileSize = 24; //inches
public static boolean turretInterpolate = false;
public Targeting() { public Targeting() {
} }