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

Its the most wonderful time of the year

Browse Source
This commit is contained in:
Keshav Anand
2026-01-31 00:05:54 -06:00
parent 75cfc6e220
commit 6e6df07153
8 changed files with 89 additions and 930 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -208,7 +208,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
@@ -272,7 +272,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);

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

@@ -326,7 +326,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
@@ -390,7 +390,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);

804
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java
View File

@@ -1,804 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bHGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShoot1Tangent;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bXGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bYGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rHGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShoot1Tangent;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rXGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rYGate;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2c;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous(preselectTeleOp = "TeleopV3")
@Config
public class Auto_LT_Close_GateCycle extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.014;
public static double spindexerSpeedIncrease = 0.02;
public static double obeliskTurrPos = 0.53;
public static double gatePickupTime = 3.0;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
public static double turretShootPos = 0.72;
public static double shootAllTime = 1.8;
public static double shoot0Time = 1.6;
public static double intake1Time = 3.0;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 80.0;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
public static double shootAllVelocity = 2500;
public static double shootAllHood = 0.78;
// ---- PICKUP TIMING ----
public static double pickup1Time = 3.0;
// ---- PICKUP POSITION TOLERANCES ----
public static double pickup1XTolerance = 2.0;
public static double pickup1YTolerance = 2.0;
// ---- OBELISK DETECTION ----
public static double obelisk1Time = 1.5;
public static double obelisk1XTolerance = 2.0;
public static double obelisk1YTolerance = 2.0;
public static double shoot1ToleranceX = 2.0;
public static double shoot1ToleranceY = 2.0;
public static double shoot1Time = 2.0;
public static double shootCycleTime = 2.5;
public int motif = 0;
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
private double x1, y1, h1;
private double x2a, y2a, h2a, t2a;
private double x2b, y2b, h2b, t2b;
private double x2c, y2c, h2c, t2c;
private double xShoot, yShoot, hShoot;
private double xGate, yGate, hGate;
private double shoot1Tangent;
public Action prepareShootAll(double time) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
return (System.currentTimeMillis() - stamp) < (time * 1000);
}
};
}
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = vel;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
if (getRuntime() - stamp < shootTime) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = 0.0;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
if (getRuntime() - stamp < shootTime) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.processIntake();
robot.intake.setPower(1);
motif = turret.detectObelisk();
spindexer.ballCounterLight();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
}
};
}
public Action detectObelisk(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance,
double turrPos
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
motif = turret.detectObelisk();
robot.turr1.setPosition(turrPos);
robot.turr2.setPosition(1 - turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
return !shouldFinish;
}
};
}
public Action manageFlywheel(
double vel,
double hoodPos,
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
return !shouldFinish;
}
};
}
public Action manageShooterAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - 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
(robotX, robotY, robotHeading, 0.0, false);
turret.trackGoal(deltaPose);
robot.hood.setPosition(targetingSettings.hoodAngle);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
return !shouldFinish;
}
};
}
public Action manageFlywheelAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - 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
(robotX, robotY, robotHeading, 0.0, false);
robot.hood.setPosition(targetingSettings.hoodAngle);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
return !shouldFinish;
}
};
}
@Override
public void runOpMode() throws InterruptedException {
hardwareMap.get(Servo.class, "light").setPosition(0);
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
flywheel = new Flywheel(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap);
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
TrajectoryActionBuilder shoot0 = null;
TrajectoryActionBuilder pickup1 = null;
TrajectoryActionBuilder shoot1 = null;
TrajectoryActionBuilder gatePickup = null;
TrajectoryActionBuilder shootCycle = null;
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out);
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (redAlliance) {
robot.light.setPosition(0.28);
// ---- FIRST SHOT ----
x1 = rx1;
y1 = ry1;
h1 = rh1;
// ---- PICKUP PATH ----
x2a = rx2a;
y2a = ry2a;
h2a = rh2a;
t2a = rt2a;
x2b = rx2b;
y2b = ry2b;
h2b = rh2b;
t2b = rt2b;
x2c = rx2c;
y2c = ry2c;
h2c = rh2c;
t2c = rt2c;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
shoot1Tangent = rShoot1Tangent;
xGate = rXGate;
yGate = rYGate;
hGate = rHGate;
} else {
robot.light.setPosition(0.6);
// ---- FIRST SHOT ----
x1 = bx1;
y1 = by1;
h1 = bh1;
// ---- PICKUP PATH ----
x2a = bx2a;
y2a = by2a;
h2a = bh2a;
t2a = bt2a;
x2b = bx2b;
y2b = by2b;
h2b = bh2b;
t2b = bt2b;
x2c = bx2c;
y2c = by2c;
h2c = bh2c;
t2c = bt2c;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
shoot1Tangent = bShoot1Tangent;
xGate = bXGate;
yGate = bYGate;
hGate = bHGate;
}
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.setReversed(true)
.splineTo(new Vector2d(x2a, y2a), shoot1Tangent)
.splineToSplineHeading(new Pose2d(xShoot, yShoot, hShoot), shoot1Tangent);
gatePickup = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
.strafeToLinearHeading(new Vector2d(xGate, yGate), hGate);
shootCycle = drive.actionBuilder(new Pose2d(xGate, yGate, hGate))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.transfer.setPower(1);
assert shoot0 != null;
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
manageFlywheel(
shoot0Vel,
shoot0Hood,
flywheel0Time,
x1,
0.501,
shoot0XTolerance,
0.501
)
)
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
manageFlywheel(
shootAllVelocity,
shootAllHood,
pickup1Time,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(intake1Time),
detectObelisk(
obelisk1Time,
x2b,
y2c,
obelisk1XTolerance,
obelisk1YTolerance,
obeliskTurrPos
)
)
);
motif = turret.detectObelisk();
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shootAllVelocity,
shootAllHood,
shoot1Time,
0.501,
0.501,
0.501,
0.501
),
shoot1.build(),
prepareShootAll(shoot1Time)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
while (opModeIsActive()) {
Actions.runBlocking(
new ParallelAction(
gatePickup.build(),
manageShooterAuto(
gatePickupTime,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(gatePickupTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shootCycleTime,
0.501,
0.501,
0.501,
0.501
),
shootCycle.build(),
prepareShootAll(shootCycleTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
}
}
}
}

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

@@ -98,7 +98,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double gateIntakeTime = 5.0;
public static double gateIntakeTime = 5.0; //TODO: Increase ig
public static double spindexerSpeedIncrease = 0.03;
@@ -113,7 +113,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
public static double shoot0Time = 1.6;
public static double intake1Time = 3.0;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 19;
public static double pickup1Speed = 25;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
@@ -137,7 +137,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
public static double firstShootTime = 0.3;
public static double shootGateTime = 3.0;
public static double gaitWait = 0.8;
public static double gaitWait = 0.45;
public int motif = 0;
Robot robot;
@@ -321,7 +321,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
@@ -385,7 +385,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);

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

@@ -24,7 +24,7 @@ public class Poses {
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
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);

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java
View File

@@ -10,55 +10,16 @@ public class Poses_V2 {
public static double turretHeight = 12;
public static double relativeGoalHeight = goalHeight - turretHeight;
public static double rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7;
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
public static double rx1 = 20, ry1 = 0, rh1 = 0;
public static double rx2a = 55, ry2a = 39, rh2a = Math.toRadians(140), rt2a = Math.toRadians(Math.PI/2);
public static double rx2b = 33, ry2b = 61, rh2b = Math.toRadians(140), rt2b = Math.toRadians(Math.PI/2);
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c = Math.toRadians(Math.PI/2);
public static double rXGateA = 27, rYGateA = 56, rHGateA = Math.toRadians(160);
public static double rXGateB = 40, rYGateB = 43, rHGateB = Math.toRadians(159);
public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836;
public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140);
public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140);
public static double bx1 = 20, by1 = 0, bh1 = 0;
public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140), bt2a = Math.toRadians(140);
public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140), bt2b = Math.toRadians(140);
public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140), bt2c = Math.toRadians(140);
public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140);
public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
public static double bXGateA = 33, bYGateA = 61, bHGateA = Math.toRadians(165);
public static double bXGateB = 33, bYGateB = 61, bHGateB = Math.toRadians(165);
public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140);
public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static double rShoot1Tangent = Math.toRadians(0);
public static double bShoot1Tangent = Math.toRadians(0);
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -329,6 +329,8 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
//
// TELE.update();
ticker++;
}

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

@@ -1,60 +1,34 @@
package org.firstinspires.ftc.teamcode.utils;
import android.provider.Settings;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class Targeting {
MultipleTelemetry TELE;
double cancelOffsetX = 0.0; // was -40.0
double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95;
int tileSize = 24; //inches
public final int TILE_UPPER_QUARTILE = 18;
public final int TILE_LOWER_QUARTILE = 6;
public double robotInchesX, robotInchesY = 0.0;
public int robotGridX, robotGridY = 0;
public static class Settings {
public double flywheelRPM = 0.0;
public double hoodAngle = 0.0;
public Settings (double flywheelRPM, double hoodAngle) {
this.flywheelRPM = flywheelRPM;
this.hoodAngle = hoodAngle;
}
}
// Known settings discovered using shooter test.
// Keep the fidelity at 1 floor tile for now but we could also half it if more
// accuracy is needed.
public static final Settings[][] KNOWNTARGETING;
static {
KNOWNTARGETING = new Settings[6][6];
// ROW 0 - Closet to the goals
KNOWNTARGETING[0][0] = new Settings (2300.0, 0.93);
KNOWNTARGETING[0][1] = new Settings (2300.0, 0.93);
KNOWNTARGETING[0][2] = new Settings (2500.0, 0.78);
KNOWNTARGETING[0][3] = new Settings (2800.0, 0.68);
KNOWNTARGETING[0][4] = new Settings (3000.0, 0.58);
KNOWNTARGETING[0][5] = new Settings (3000.0, 0.58);
KNOWNTARGETING[0][0] = new Settings(2300.0, 0.93);
KNOWNTARGETING[0][1] = new Settings(2300.0, 0.93);
KNOWNTARGETING[0][2] = new Settings(2500.0, 0.78);
KNOWNTARGETING[0][3] = new Settings(2800.0, 0.68);
KNOWNTARGETING[0][4] = new Settings(3000.0, 0.58);
KNOWNTARGETING[0][5] = new Settings(3000.0, 0.58);
// ROW 1
KNOWNTARGETING[1][0] = new Settings (2300.0, 0.93);
KNOWNTARGETING[1][1] = new Settings (2300.0, 0.93);
KNOWNTARGETING[1][2] = new Settings (2600.0, 0.78);
KNOWNTARGETING[1][0] = new Settings(2300.0, 0.93);
KNOWNTARGETING[1][1] = new Settings(2300.0, 0.93);
KNOWNTARGETING[1][2] = new Settings(2600.0, 0.78);
// KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62);
// KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55);
// KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50);
KNOWNTARGETING[1][3] = new Settings (2800.0, 0.68); // Real settings replaced with (0,3) new Settings (2800.0, 0.62);
KNOWNTARGETING[1][4] = new Settings (3000.0, 0.58); // Real setting replaced with (0,4) new Settings (3000.0, 0.55);
KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50);
KNOWNTARGETING[1][3] = new Settings(2800.0, 0.68); // Real settings replaced with (0,3) new Settings (2800.0, 0.62);
KNOWNTARGETING[1][4] = new Settings(3000.0, 0.58); // Real setting replaced with (0,4) new Settings (3000.0, 0.55);
KNOWNTARGETING[1][5] = new Settings(3200.0, 0.50);
// ROW 2
// KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78);
// KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78);
@@ -62,37 +36,47 @@ public class Targeting {
// KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53);
// KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50);
// KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50);
KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78);
KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78);
KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60);
KNOWNTARGETING[2][3] = new Settings (2800.0, 0.62); // Real settings replaced with (1,3) new Settings (2900.0, 0.53);
KNOWNTARGETING[2][4] = new Settings (3000.0, 0.55); // real settings replaces with (1,4) new Settings (3100.0, 0.50);
KNOWNTARGETING[2][5] = new Settings (3200.0, 0.50); // real settings replaced with (1,5) new Settings (3100.0, 0.50);
KNOWNTARGETING[2][0] = new Settings(2500.0, 0.78);
KNOWNTARGETING[2][1] = new Settings(2500.0, 0.78);
KNOWNTARGETING[2][2] = new Settings(2700.0, 0.60);
KNOWNTARGETING[2][3] = new Settings(2800.0, 0.62); // Real settings replaced with (1,3) new Settings (2900.0, 0.53);
KNOWNTARGETING[2][4] = new Settings(3000.0, 0.55); // real settings replaces with (1,4) new Settings (3100.0, 0.50);
KNOWNTARGETING[2][5] = new Settings(3200.0, 0.50); // real settings replaced with (1,5) new Settings (3100.0, 0.50);
// ROW 3
KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][2] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][3] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][0] = new Settings(2900.0, 0.50);
KNOWNTARGETING[3][1] = new Settings(2900.0, 0.50);
KNOWNTARGETING[3][2] = new Settings(2900.0, 0.50);
KNOWNTARGETING[3][3] = new Settings(3100.0, 0.47);
KNOWNTARGETING[3][4] = new Settings(3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings(3100.0, 0.47);
// ROW 4
KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
KNOWNTARGETING[4][0] = new Settings(3100, 0.49);
KNOWNTARGETING[4][1] = new Settings(3100, 0.49);
KNOWNTARGETING[4][2] = new Settings(3100, 0.5);
KNOWNTARGETING[4][3] = new Settings(3200, 0.5);
KNOWNTARGETING[4][4] = new Settings(3250, 0.49);
KNOWNTARGETING[4][5] = new Settings(3300, 0.49);
// ROW 5
KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
KNOWNTARGETING[5][0] = new Settings(3200, 0.48);
KNOWNTARGETING[5][1] = new Settings(3200, 0.48);
KNOWNTARGETING[5][2] = new Settings(3300, 0.48);
KNOWNTARGETING[5][3] = new Settings(3350, 0.48);
KNOWNTARGETING[5][4] = new Settings(3350, 0.48);
KNOWNTARGETING[5][5] = new Settings(3350, 0.48);
}
public Targeting()
{
public final int TILE_UPPER_QUARTILE = 18;
public final int TILE_LOWER_QUARTILE = 6;
public double robotInchesX, robotInchesY = 0.0;
public int robotGridX, robotGridY = 0;
MultipleTelemetry TELE;
double cancelOffsetX = 0.0; // was -40.0
double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95;
int tileSize = 24; //inches
public Targeting() {
}
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
@@ -111,8 +95,8 @@ public class Targeting {
int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
int remX = Math.floorMod((int)robotInchesX, tileSize);
int remY = Math.floorMod((int)robotInchesX, tileSize);
int remX = Math.floorMod((int) robotInchesX, tileSize);
int remY = Math.floorMod((int) robotInchesX, tileSize);
// Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile.
@@ -122,7 +106,7 @@ public class Targeting {
int y1 = 0;
interpolate = false;
if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
(robotGridX < 5) && (robotGridY <5)) {
(robotGridX < 5) && (robotGridY < 5)) {
// +X, +Y
interpolate = true;
x0 = robotGridX;
@@ -186,12 +170,18 @@ public class Targeting {
}
//clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
if (redAlliance) {
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
} else {
robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
}
// basic search
if(true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) {
if (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
}
@@ -220,9 +210,19 @@ public class Targeting {
// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
// Average target tiles
recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0;
recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0;
recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM) / 2.0;
recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle) / 2.0;
return recommendedSettings;
}
}
public static class Settings {
public double flywheelRPM = 0.0;
public double hoodAngle = 0.0;
public Settings(double flywheelRPM, double hoodAngle) {
this.flywheelRPM = flywheelRPM;
this.hoodAngle = hoodAngle;
}
}
}