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

Auton, hopefully pintpoint works ig

Browse Source
This commit is contained in:
Keshav Anand
2026-02-24 22:22:03 -06:00
parent 2ccd7f04f8
commit 64b2fed8d6
6 changed files with 117 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -13,16 +13,22 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
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.PoseVelocity2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.acmerobotics.roadrunner.ftc.PinpointIMU;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -64,13 +70,13 @@ public class Auto_LT_Close extends LinearOpMode {
public static double colorSenseTime = 1.2;
public static double waitToShoot0 = 0.5;
public static double waitToPickupGate2 = 0.3;
public static double pickupStackGateSpeed = 30;
public static double pickupStackGateSpeed = 13;
public static double intake2TimeGate = 3;
public static double shoot2GateTime = 1.7;
public static double endGateTime = 22;
public static double waitToPickupGateWithPartner = 0.7;
public static double waitToPickupGateWithPartner = 0.01;
public static double waitToPickupGateSolo = 0.01;
public static double intakeGateTime = 8;
public static double intakeGateTime = 5;
public static double shootGateTime = 1.7;
public static double shoot1GateTime = 1.7;
public static double intake1GateTime = 3.3;
@@ -167,6 +173,7 @@ public class Auto_LT_Close extends LinearOpMode {
while (opModeInInit()) {
if (gateCycle) {
servos.setHoodPos(hoodGate0Start);
} else {
@@ -195,6 +202,10 @@ public class Auto_LT_Close extends LinearOpMode {
}
if (gamepad2.squareWasPressed()) {
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
if (!gateCycle) {
turret.pipelineSwitch(1);
} else if (redAlliance) {
@@ -203,6 +214,9 @@ public class Auto_LT_Close extends LinearOpMode {
turret.pipelineSwitch(2);
}
robot.limelight.start();
gamepad2.rumble(500);
}
@@ -320,37 +334,14 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
.strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
} else {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
}
if (gateCycle) {
pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickupStackGateSpeed));
} else {
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
}
if (gateCycle) {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
} else if (ballCycles < 2) {
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));
}
if (gateCycle) {
pickup2 = drive.actionBuilder(new Pose2d(xShoot0, yShoot0, Math.toRadians(hShoot0)))
pickup2 = shoot0.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
@@ -362,7 +353,7 @@ public class Auto_LT_Close extends LinearOpMode {
}
if (gateCycle) {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
shoot2 = pickup2.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
} else if (ballCycles < 3) {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
@@ -372,6 +363,43 @@ public class Auto_LT_Close extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
gateCyclePickup = shoot2.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
.waitSeconds(waitToPickupGate)
.strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
gateCycleShoot = gateCyclePickup.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
if (gateCycle) {
pickup1 = gateCycleShoot.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickupStackGateSpeed));
} else {
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
}
if (gateCycle) {
shoot1 = pickup1.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
} else if (ballCycles < 2) {
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));
}
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
.strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
@@ -386,14 +414,10 @@ public class Auto_LT_Close extends LinearOpMode {
waitToPickupGate = waitToPickupGateSolo;
}
gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
.strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
.waitSeconds(waitToPickupGate)
.strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH)))
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
teleStart = drive.localizer.getPose();
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Ball Cycles", ballCycles);
@@ -411,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode {
robot.transfer.setPower(1);
if (gateCycle) {
startAutoGate();
shoot();
@@ -421,6 +446,7 @@ public class Auto_LT_Close extends LinearOpMode {
cycleGateIntake();
if (getRuntime() - stamp < lastShootTime) {
cycleGateShoot();
shoot();
}
}
cycleStackCloseIntakeGate();
@@ -487,6 +513,8 @@ public class Auto_LT_Close extends LinearOpMode {
);
}
void startAutoGate() {
assert shoot0 != null;
@@ -498,7 +526,7 @@ public class Auto_LT_Close extends LinearOpMode {
xShoot0,
yShoot0,
hShoot0,
false
true
)
)
);
@@ -656,6 +684,11 @@ public class Auto_LT_Close extends LinearOpMode {
}
void cycleStackMiddleGate() {
drive.updatePoseEstimate();
pickup2 = drive.actionBuilder(drive.localizer.getPose())
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
@@ -684,6 +717,12 @@ public class Auto_LT_Close extends LinearOpMode {
}
void cycleGateIntake() {
drive.updatePoseEstimate();
gateCyclePickup = drive.actionBuilder(drive.localizer.getPose())
.strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
.waitSeconds(waitToPickupGate)
.strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
Actions.runBlocking(
new ParallelAction(
gateCyclePickup.build(),
@@ -698,7 +737,13 @@ public class Auto_LT_Close extends LinearOpMode {
}
void cycleGateShoot() {
drive.updatePoseEstimate();
servos.setSpinPos(spinStartPos);
gateCycleShoot = drive.actionBuilder(drive.localizer.getPose())
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
Actions.runBlocking(
new ParallelAction(
gateCycleShoot.build(),
@@ -714,6 +759,14 @@ public class Auto_LT_Close extends LinearOpMode {
}
void cycleStackCloseIntakeGate() {
drive.updatePoseEstimate();
pickup1 = drive.actionBuilder(drive.localizer.getPose())
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickupStackGateSpeed));
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
@@ -729,6 +782,11 @@ public class Auto_LT_Close extends LinearOpMode {
void cycleStackCloseShootGate(){
servos.setSpinPos(spinStartPos);
drive.updatePoseEstimate();
shoot1 = drive.actionBuilder(drive.localizer.getPose())
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
Actions.runBlocking(
new ParallelAction(
shoot1.build(),

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -10,10 +10,10 @@ public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double rx2a = 41, ry2a = 18, rh2a = 155;
public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
public static double rx2b = 21, ry2b = 34, rh2b = 155.1;
public static double bx2b = 23, by2b = -36, bh2b = -140.1;
public static double rx3a = 55, ry3a = 39, rh3a = 140;
@@ -42,8 +42,8 @@ public class Front_Poses {
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1;
public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1;
public static double rShoot0X = 53, rShoot0Y = 10.1, rShoot0H = 80.1;
public static double bShoot0X = 53, bShoot0Y = -10.1, bShoot0H = -80.1;
public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90;
public static double bShootGateX = 50, bShootGateY = -10, bShootGateH = -90;
@@ -51,10 +51,10 @@ public class Front_Poses {
public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
public static double rPickupGateAX = 24, rPickupGateAY = 50, rPickupGateAH = 140;
public static double rPickupGateAX = 26, rPickupGateAY = 48, rPickupGateAH = 140;
public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140;
public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180;
public static double rPickupGateBX = 35, rPickupGateBY = 65, rPickupGateBH = 180;
public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
public static Pose2d teleStart = new Pose2d(0, 0, 0);

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos1 = 0.13; //0.13;
public static double spindexer_intakePos1 = 0.22; //0.13;
public static double spindexer_intakePos2 = 0.32; //0.33;//0.5;
public static double spindexer_intakePos2 = 0.41; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.5; //0.53;//0.66;
public static double spindexer_intakePos3 = 0.60; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24;
public static double spindexer_outtakeBall3 = 0.88; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.31; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4;
public static double spinStartPos = 0.05;
public static double spindexer_outtakeBall1 = 0.51; //0.27; //0.4;
public static double spinStartPos = 0.14;
public static double spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.014;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -210,9 +210,9 @@ public final class MecanumDrive {
public double maxAngAccel = 2 * Math.PI;
// path controller gains
public double axialGain = 6;
public double lateralGain = 6;
public double headingGain = 6; // shared with turn
public double axialGain = 6.0;
public double lateralGain = 6.0;
public double headingGain = 6.0; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
@@ -352,9 +352,9 @@ public final class MecanumDrive {
Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
if ((t >= timeTrajectory.duration && error.position.norm() < 2
if ((t >= timeTrajectory.duration && error.position.norm() < 1
&& robotVelRobot.linearVel.norm() < 0.5)
|| t >= timeTrajectory.duration + 0.5) {
|| t >= timeTrajectory.duration + 0.25) {
leftFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java
View File

@@ -49,6 +49,8 @@ public final class PinpointLocalizer implements Localizer {
txWorldPinpoint = initialPose;
}
@Override
public void setPose(Pose2d pose) {
txWorldPinpoint = pose.times(txPinpointRobot.inverse());

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -187,7 +187,7 @@ public class Spindexer {
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1
if (distanceRearCenter < 65) {
if (distanceRearCenter < 48) {
// Mark Ball Found
newPos1Detection = true;
@@ -209,7 +209,7 @@ public class Spindexer {
// Position 2
// Find which ball position this is in the spindexer
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
if (distanceFrontDriver < 56) {
if (distanceFrontDriver < 50) {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {