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

Compare commits

base: KeshavAnandCode:b616a41a088c3d54f2b0a042444eae54775a1ab3
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel
..
compare: KeshavAnandCode:1c3100966c2a52e7214defd56f13bbc4a6078889
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel

5 Commits
b616a41a08 ... 1c3100966c

Author SHA1 Message Date
DanTheMan-byte
1c3100966c far auto in development part 2 2026-02-15 15:47:36 -06:00
DanTheMan-byte
78c65c9d93 far auto in development 2026-02-14 18:27:21 -06:00
DanTheMan-byte
28816a6e34 close auto is pretty close to good 2026-02-14 17:16:25 -06:00
DanTheMan-byte
d0c34132de remodeled close auto and it works except for poses (@KeshavAnandCode that is your job 2026-02-14 15:39:03 -06:00
DanTheMan-byte
04ea56e31d auto actions class tested: works but needs remodeling 2026-02-12 21:26:23 -06:00
9 changed files with 387 additions and 1165 deletions
Expand all files Collapse all files

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

@@ -2,26 +2,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
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_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
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.spinStartPos;
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.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.TrajectoryActionBuilder;
@@ -40,17 +28,11 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.02;
public static double spindexerSpeedIncrease = 0.03;
public static double finalSpindexerSpeedIncrease = 0.03;
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double spindexerSpeedIncrease = 0.008;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -65,40 +47,25 @@ public class Auto_LT_Close extends LinearOpMode {
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double normalIntakeTime = 3.3;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
double turretShootPos = 0.0;
public static double finalShootAllTime = 3.0;
public static double shootAllTime = 1.8;
public static double shoot0Time = 1.6;
public static double shootAllTime = 2;
public static double intake1Time = 3.3;
public static double intake2Time = 3.8;
public static double intake3Time = 4.2;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 15;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shootAllVelocity = 2500;
public static double shootAllHood = 0.78 + hoodOffset;
// ---- PICKUP POSITION TOLERANCES ----
public static double pickup1XTolerance = 2.0;
public static double pickup1YTolerance = 2.0;
public static double flywheel0Time = 1.5;
public static double pickup1Speed = 12;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.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;
public static double shoot2Time = 2;
public static double shoot3Time = 2;
public static double shoot1Time = 2.5;
public static double shoot2Time = 2.5;
public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2;
public static double firstShootTime = 0.3;
public int motif = 0;
Robot robot;
@@ -111,8 +78,6 @@ public class Auto_LT_Close extends LinearOpMode {
Targeting targeting;
Targeting.Settings targetingSettings;
AutoActions autoActions;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
double x1, y1, h1;
double x2a, y2a, h2a, t2a;
@@ -132,11 +97,6 @@ public class Auto_LT_Close extends LinearOpMode {
private double shoot1Tangent;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
int ballCycles = 3;
int prevMotif = 0;
@@ -170,13 +130,11 @@ public class Auto_LT_Close extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
servos.setSpinPos(autoSpinStartPos);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
@@ -209,8 +167,8 @@ public class Auto_LT_Close extends LinearOpMode {
}
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(1);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
@@ -354,17 +312,21 @@ public class Auto_LT_Close extends LinearOpMode {
robot.transfer.setPower(1);
startAuto();
shoot();
if (ballCycles > 0){
cycleStackClose();
shoot();
}
if (ballCycles > 1){
cycleStackMiddle();
shoot();
}
if (ballCycles > 2){
cycleStackFar();
shoot();
}
while (opModeIsActive()) {
@@ -374,6 +336,7 @@ public class Auto_LT_Close extends LinearOpMode {
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
robot.transfer.setPower(0);
TELE.addLine("finished");
TELE.update();
@@ -383,50 +346,64 @@ public class Auto_LT_Close extends LinearOpMode {
}
void shoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
}
void startAuto() {
assert shoot0 != null;
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageFlywheel(
shoot0Vel,
shoot0Hood,
autoActions.manageShooterAuto(
flywheel0Time,
x1,
0.501,
shoot0XTolerance,
0.501
y1,
posXTolerance,
posYTolerance,
h1,
false
)
)
);
Actions.runBlocking(
autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void cycleStackClose(){
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
autoActions.manageFlywheel(
shootAllVelocity,
shootAllHood,
autoActions.manageShooterAuto(
intake1Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x2b,
y2b,
posXTolerance,
posYTolerance,
h2b,
true
),
autoActions.intake(intake1Time),
autoActions.detectObelisk(
intake1Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x2b,
y2b,
posXTolerance,
posYTolerance,
obeliskTurrPos1
)
@@ -438,35 +415,33 @@ public class Auto_LT_Close extends LinearOpMode {
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheel(
shootAllVelocity,
shootAllHood,
shoot1Time,
0.501,
0.501,
0.501,
0.501
),
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
)
);
double posX;
double posY;
double posH;
if (ballCycles > 1){
posX = xShoot;
posY = yShoot;
posH = hShoot;
} else {
posX = xLeave;
posY = yLeave;
posH = hLeave;
}
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
shoot1Time,
posX,
posY,
posXTolerance,
posYTolerance,
posH,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
)
);
}
@@ -476,18 +451,20 @@ public class Auto_LT_Close extends LinearOpMode {
pickup2.build(),
autoActions.manageShooterAuto(
intake2Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x3b,
y3b,
posXTolerance,
posYTolerance,
h3b,
true
),
autoActions.intake(intake2Time),
autoActions.detectObelisk(
intake2Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x3b,
y3b,
posXTolerance,
posYTolerance,
obeliskTurrPos2
)
@@ -499,32 +476,33 @@ public class Auto_LT_Close extends LinearOpMode {
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheelAuto(
shoot2Time,
0.501,
0.501,
0.501,
0.501
),
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
)
);
double posX;
double posY;
double posH;
if (ballCycles > 2){
posX = xShoot;
posY = yShoot;
posH = hShoot;
} else {
posX = xLeave;
posY = yLeave;
posH = hLeave;
}
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
shoot2Time,
posX,
posY,
posXTolerance,
posYTolerance,
posH,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
)
);
}
@@ -534,18 +512,20 @@ public class Auto_LT_Close extends LinearOpMode {
pickup3.build(),
autoActions.manageShooterAuto(
intake3Time,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
x4b,
y4b,
posXTolerance,
posYTolerance,
h4b,
true
),
autoActions.intake(intake3Time),
autoActions.detectObelisk(
intake3Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
x4b,
y4b,
posXTolerance,
posYTolerance,
obeliskTurrPos3
)
@@ -559,30 +539,18 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageFlywheelAuto(
autoActions.manageShooterAuto(
shoot3Time,
0.501,
0.501,
0.501,
0.501
xLeave,
yLeave,
posXTolerance,
posYTolerance,
hLeave,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
finalShootAllTime,
0.501,
0.501,
0.501,
0.501
),
autoActions.shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
)
);
}
}

817
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -3,8 +3,8 @@ package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
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.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
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_outtakeBall2;
@@ -13,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
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.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
@@ -31,6 +32,7 @@ import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -44,15 +46,10 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0XTolerance = 1.0;
public static double shoot0Vel = 3300, shoot0Hood = 0.48;
public static double redTurretShootPos = 0.05;
public static double blueTurretShootPos = -0.05;
public static int fwdTime = 200, strafeTime = 2300;
double xLeave, yLeave, hLeave;
public static int sleepTime = 1300;
public int motif = 0;
double turretShootPos = 0.0;
Robot robot;
@@ -64,613 +61,43 @@ public class Auto_LT_Far extends LinearOpMode {
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
double firstSpindexShootPos = autoSpinStartPos;
boolean shootForward = true;
AutoActions autoActions;
double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
int rearSlotGreen = 0;
int mostGreenSlot = 0;
double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
public static double firstShootTime = 0.3;
public static double flywheel0Time = 3.5;
public static double shoot0Time = 2;
public static double flywheel0Time = 1.5;
boolean gatePickup = false;
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15;
public static int pickupStackSpeed = 12;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spindexerWiggle = 0.01;
boolean decideGreenSlot = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
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);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
decideGreenSlot = true;
return true;
} else if (decideGreenSlot) {
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
mostGreenSlot = 3;
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
mostGreenSlot = 2;
} else {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
}
}
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
return false;
}
}
};
}
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", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
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 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
servos.setSpinPos(autoSpinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
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 < firstShootTime) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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();
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
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();
robot.limelight.pipelineSwitch(1);
}
ticker++;
motif = turret.detectObelisk();
turret.setTurret(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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};
}
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++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
public static double spindexerSpeedIncrease = 0.008;
public static double shootAllTime = 2;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0;
public static double shootStackTime = 2;
public static double shootGateTime = 2;
public static double colorSenseTime = 1;
public static double intakeStackTime = 3.3;
public static double intakeGateTime = 3;
public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14;
public static double blueObeliskTurrPos1 = -0.12;
public static double blueObeliskTurrPos2 = -0.13;
public static double blueObeliskTurrPos3 = -0.14;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
TrajectoryActionBuilder leaveFromShoot = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
Pose2d autoStart = new Pose2d(0,0,0);
@Override
public void runOpMode() throws InterruptedException {
@@ -698,27 +125,12 @@ public class Auto_LT_Far extends LinearOpMode {
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, autoStart);
servos.setSpinPos(autoSpinStartPos);
servos.setSpinPos(spinStartPos);
servos.setTransferPos(transferServo_out);
while (opModeInInit()) {
// Recalibration in initialization
drive.updatePoseEstimate();
if (gamepad2.triangle) {
autoStart = drive.localizer.getPose(); // use this position as starting position
gamepad2.rumble(1000);
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
@@ -745,6 +157,10 @@ public class Auto_LT_Far extends LinearOpMode {
if (redAlliance) {
robot.light.setPosition(0.28);
autoStart = new Pose2d(autoStartRX, autoStartRY, Math.toRadians(autoStartRH));
drive = new MecanumDrive(hardwareMap, autoStart);
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
@@ -761,10 +177,23 @@ public class Auto_LT_Far extends LinearOpMode {
yStackPickupB = rStackPickupBY;
hStackPickupB = rStackPickupBH;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(4);
robot.limelight.start();
gamepad2.rumble(500);
}
} else {
robot.light.setPosition(0.6);
autoStart = new Pose2d(autoStartBX, autoStartBY, Math.toRadians(autoStartBH));
drive = new MecanumDrive(hardwareMap, autoStart);
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
@@ -781,7 +210,16 @@ public class Auto_LT_Far extends LinearOpMode {
yStackPickupB = bStackPickupBY;
hStackPickupB = bStackPickupBH;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(2);
robot.limelight.start();
gamepad2.rumble(500);
}
}
leave3Ball = drive.actionBuilder(autoStart)
@@ -798,6 +236,8 @@ public class Auto_LT_Far extends LinearOpMode {
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
limelightUsed = true;
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup);
@@ -814,16 +254,18 @@ public class Auto_LT_Far extends LinearOpMode {
// Currently only shoots; keep this start and modify times and then add extra paths
if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1);
startAuto();
shoot();
if (stack3){
//cycleStackFar();
cycleStackFar();
shoot();
}
//TODO: insert code here for gate auto
if (gatePickup || stack3){
leave();
} else {
@@ -838,6 +280,7 @@ public class Auto_LT_Far extends LinearOpMode {
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
robot.transfer.setPower(0);
TELE.addLine("finished");
TELE.update();
@@ -847,25 +290,39 @@ public class Auto_LT_Far extends LinearOpMode {
}
void shoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
}
void startAuto(){
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
autoActions.manageShooterAuto(
flywheel0Time,
0.501,
0.501,
shoot0XTolerance,
0.501
0.501,
0.501,
0.501,
false
)
)
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void leave3Ball(){
@@ -878,61 +335,51 @@ public class Auto_LT_Far extends LinearOpMode {
Actions.runBlocking(leaveFromShoot.build());
}
// void cycleStackFar(){
// Actions.runBlocking(
// new ParallelAction(
// pickup3.build(),
// manageShooterAuto(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// intake(intake3Time),
// detectObelisk(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501,
// obeliskTurrPos3
// )
//
// )
// );
//
// motif = turret.getObeliskID();
//
// if (motif == 0) motif = prevMotif;
// prevMotif = motif;
//
// Actions.runBlocking(
// new ParallelAction(
// manageFlywheelAuto(
// shoot3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shoot3.build(),
// prepareShootAll(colorSenseTime, shoot3Time, motif)
// )
// );
//
// Actions.runBlocking(
// new ParallelAction(
// manageShooterAuto(
// finalShootAllTime,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
// )
//
// );
// }
void cycleStackFar(){
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
autoActions.manageShooterAuto(
intakeStackTime,
xStackPickupB,
yStackPickupB,
posXTolerance,
posYTolerance,
hStackPickupB,
true
),
autoActions.intake(intakeStackTime),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupB,
yStackPickupB,
posXTolerance,
posYTolerance,
obeliskTurrPos3
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootStackTime,
xShoot,
yShoot,
posXTolerance,
posYTolerance,
hShoot,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
)
);
}
}

406
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
View File

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
@@ -15,11 +14,13 @@ import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import androidx.annotation.NonNull;
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.Pose2d;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -30,6 +31,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Config
public class AutoActions{
Robot robot;
MultipleTelemetry TELE;
@@ -48,6 +50,7 @@ public class AutoActions{
private boolean shootForward = true;
public static double firstShootTime = 0.3;
public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos;
public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){
this.robot = rob;
@@ -60,6 +63,7 @@ public class AutoActions{
this.targetingSettings = tS;
this.turret = tur;
}
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
@@ -69,6 +73,36 @@ public class AutoActions{
boolean decideGreenSlot = false;
void spin1PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = spindexer_outtakeBall3 + 0.1;
}
void spin2PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = spindexer_outtakeBall3b - 0.1;
}
void reverseSpin2PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
spinEndPos = 0.95;
}
void spin3PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = spindexer_outtakeBall1 - 0.1;
}
void oddSpin3PosFirst(){
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = spindexer_outtakeBall2 + 0.1;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
@@ -80,29 +114,6 @@ public class AutoActions{
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);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
@@ -142,45 +153,33 @@ public class AutoActions{
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spin1PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spin2PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spin2PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
reverseSpin2PosFirst();
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spin3PosFirst();
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
oddSpin3PosFirst();
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spin1PosFirst();
}
}
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
@@ -194,85 +193,7 @@ public class AutoActions{
};
}
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", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
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 ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
@@ -285,9 +206,6 @@ public class AutoActions{
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
@@ -295,51 +213,37 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
spindexer.setIntakePower(-0.1);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
double prevSpinPos = servos.getSpinCmdPos();
teleStart = drive.localizer.getPose();
boolean end;
if (shootForward){
end = prevSpinPos > spinEndPos;
} else {
end = prevSpinPos < spinEndPos;
}
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
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 (System.currentTimeMillis() - stamp < shootTime) {
if (System.currentTimeMillis() - stamp < firstShootTime) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
@@ -349,6 +253,7 @@ public class AutoActions{
spindexer.resetSpindexer();
spindexer.processIntake();
doneShooting = true;
return false;
@@ -377,14 +282,12 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
private boolean detectingObelisk = false;
public Action detectObelisk(
double time,
double posX,
@@ -405,13 +308,13 @@ public class AutoActions{
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
detectingObelisk = true;
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
robot.limelight.pipelineSwitch(1);
turret.pipelineSwitch(1);
}
ticker++;
@@ -423,20 +326,17 @@ public class AutoActions{
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;
drive.updatePoseEstimate();
boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
teleStart = currentPose;
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
turret.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
turret.pipelineSwitch(2);
}
detectingObelisk = false;
return false;
} else {
return true;
@@ -446,76 +346,26 @@ public class AutoActions{
};
}
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++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
public Action manageShooterAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
double posYTolerance,
double posH,
boolean whileIntaking
) {
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;
int shootingTicker = 0;
double shootingStamp = 0;
final boolean timeFallback = (time != 0.501);
final boolean posXFallback = (posX != 0.501);
final boolean posYFallback = (posY != 0.501);
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -529,24 +379,31 @@ public class AutoActions{
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotX = currentPose.position.x;
double robotY = currentPose.position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double robotHeading = currentPose.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);
Pose2d deltaPose;
if (posX != 0.501) {
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
} else {
deltaPose = new Pose2d(robotX, robotY, robotHeading);
}
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
if (!detectingObelisk) {
turret.trackGoal(deltaPose);
}
servos.setHoodPos(targetingSettings.hoodAngle);
@@ -555,89 +412,26 @@ public class AutoActions{
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 xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
boolean shouldFinish;
if (whileIntaking) {
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
} else {
shouldFinish = timeDone || (xDone && yDone) || doneShooting;
}
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = currentPose;
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("Steady?", flywheel.getSteady());
TELE.update();
return !shouldFinish;
if (shouldFinish) {
doneShooting = false;
return false;
} else {
return true;
}
};
}
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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};

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

@@ -5,11 +5,11 @@ import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Back_Poses {
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
public static double rShootX = 95, rShootY = 85, rShootH = 90;
public static double bShootX = 95, bShootY = -85, bShootH = -90;
public static double rShootX = 100, rShootY = 55, rShootH = 90;
public static double bShootX = 100, bShootY = -55, bShootH = -90;
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
@@ -17,7 +17,7 @@ public class Back_Poses {
public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;
}

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

@@ -116,10 +116,10 @@ public class TeleopV3 extends LinearOpMode {
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
robot.limelight.pipelineSwitch(4);
turret.pipelineSwitch(4);
light.setManualLightColor(Color.LightRed);
} else {
robot.limelight.pipelineSwitch(2);
turret.pipelineSwitch(2);
light.setManualLightColor(Color.LightBlue);
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -38,8 +38,8 @@ public class Flywheel {
}
// Set the robot PIDF for the next cycle.
private double prevF = 0.501;
public static int voltagePIDFDifference = 5;
private double prevF = 0;
public static double voltagePIDFDifference = 0.8;
public void setPIDF(double p, double i, double d, double f) {
shooterPIDF1.p = p;
shooterPIDF1.i = i;
@@ -78,7 +78,7 @@ public class Flywheel {
}
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 200.0);
steady = (Math.abs(commandedVelocity - velo) < 200.0);
return powPID;
}

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

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -72,7 +74,7 @@ public class Servos {
public double setHoodPos(double pos){
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
robot.hood.setPosition(pos);
robot.hood.setPosition(pos + hoodOffset);
firstHoodPos = false;
}

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

@@ -186,7 +186,7 @@ public class Targeting {
if (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
}
return recommendedSettings;
} else {

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import java.util.List;
@@ -24,7 +25,7 @@ public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.39;
public static double turrDefault = 0.37;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static boolean limelightUsed = true;
@@ -48,6 +49,7 @@ public class Turret {
double limelightPosX = 0.0;
double limelightPosY = 0.0;
LLResult result;
boolean bearingAligned = false;
private boolean lockOffset = false;
private int obeliskID = 0;
@@ -56,6 +58,7 @@ public class Turret {
private double lightColor = Color.LightRed;
private int currentTrackCount = 0;
private double permanentOffset = 0.0;
private int prevPipeline = -1;
private PIDController bearingPID;
private double prevTurretPos = 0.0;
@@ -81,14 +84,22 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
private double prevTurrPos = 0.501;
private double prevTurrPos = 0;
private boolean isFirstTurretPos = true;
public void setTurret(double pos) {
if (prevTurrPos != 0.501 && prevTurrPos != pos){
if (isFirstTurretPos || prevTurrPos != pos){
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
isFirstTurretPos = false;
}
prevTurrPos = pos;
}
public void pipelineSwitch(int pipeline){
if (prevPipeline != pipeline){
robot.limelight.pipelineSwitch(pipeline);
}
prevPipeline = pipeline;
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;