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

@KeshavAnandCode I need your help tomorrow to edit auto actions so they are more sensor based

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-07 17:29:34 -06:00
parent de9ce388c4
commit 50060d3812
4 changed files with 106 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -106,10 +106,10 @@ import java.util.Objects;
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.015;
public static double shoot0SpinSpeedIncrease = 0.02;
public static double spindexerSpeedIncrease = 0.03;
public static double finalSpindexerSpeedIncrease = 0.025;
public static double finalSpindexerSpeedIncrease = 0.03;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -348,6 +348,8 @@ public class Auto_LT_Close extends LinearOpMode {
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();
@@ -622,6 +624,8 @@ public class Auto_LT_Close extends LinearOpMode {
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
@@ -693,6 +697,8 @@ public class Auto_LT_Close extends LinearOpMode {
robot.hood.setPosition(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;
@@ -762,6 +768,8 @@ public class Auto_LT_Close extends LinearOpMode {
robot.hood.setPosition(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;
@@ -992,7 +1000,7 @@ public class Auto_LT_Close extends LinearOpMode {
robot.transfer.setPower(1);
startCycle();
startAuto();
if (ballCycles > 0){
cycleStackClose();
@@ -1022,7 +1030,7 @@ public class Auto_LT_Close extends LinearOpMode {
}
void startCycle() {
void startAuto() {
assert shoot0 != null;
Actions.runBlocking(

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

@@ -49,7 +49,7 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
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;
@@ -69,16 +69,18 @@ public class Auto_LT_Far extends LinearOpMode {
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
private double xShoot, yShoot, hShoot;
double firstSpindexShootPos = autoSpinStartPos;
boolean shootForward = true;
double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 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 Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
@@ -97,12 +99,28 @@ public class Auto_LT_Far extends LinearOpMode {
ticker++;
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
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);
@@ -214,6 +232,8 @@ public class Auto_LT_Far extends LinearOpMode {
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();
@@ -314,6 +334,24 @@ public class Auto_LT_Far extends LinearOpMode {
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) {
@@ -369,6 +407,9 @@ public class Auto_LT_Far extends LinearOpMode {
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
@@ -467,6 +508,8 @@ public class Auto_LT_Far extends LinearOpMode {
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
@@ -538,6 +581,8 @@ public class Auto_LT_Far extends LinearOpMode {
robot.hood.setPosition(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;
@@ -607,6 +652,8 @@ public class Auto_LT_Far extends LinearOpMode {
robot.hood.setPosition(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;
@@ -627,6 +674,9 @@ public class Auto_LT_Far extends LinearOpMode {
};
}
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
@Override
public void runOpMode() throws InterruptedException {
@@ -662,8 +712,6 @@ public class Auto_LT_Far extends LinearOpMode {
robot.light.setPosition(1);
TrajectoryActionBuilder leave = null;
while (opModeInInit()) {
// Recalibration in initialization
@@ -673,9 +721,10 @@ public class Auto_LT_Far extends LinearOpMode {
gamepad2.rumble(1000);
}
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
@@ -706,10 +755,9 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + blueTurretShootPos;
}
leave = drive.actionBuilder(teleEnd)
leave3Ball = drive.actionBuilder(teleEnd)
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Start Position", teleEnd);
@@ -726,35 +774,9 @@ public class Auto_LT_Far extends LinearOpMode {
robot.transfer.setPower(1);
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
5,
0.501,
0.501,
shoot0XTolerance,
0.501
)
startAuto();
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
assert leave != null;
Actions.runBlocking(leave.build());
leave3Ball();
// Actual way to end autonomous in to find final position
while (opModeIsActive()) {
@@ -772,4 +794,30 @@ public class Auto_LT_Far extends LinearOpMode {
}
}
void startAuto(){
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
flywheel0Time,
0.501,
0.501,
shoot0XTolerance,
0.501
)
)
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void leave3Ball(){
assert leave3Ball != null;
Actions.runBlocking(leave3Ball.build());
}
}

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

@@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class Back_Poses {
public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50;
public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50;
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50;
public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
@@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (light !=0.501){
robot.light.setPosition(light);
}
// To check configuration of spindexer:
// Set "mode" to 1 and spindexPow to 0.1
// If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
// Do the previous test again to confirm
// Set "mode" to 0 but keep spindexPos at 0.501
// Manually turn the spindexer until "spindexer pos" is set close to 0
// Check each spindexer voltage:
// If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
// If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
//TODO: @KeshavAnandCode do the above please
TELE.addData("spindexer pos", servo.getSpinPos());
TELE.addData("turret pos", robot.turr1.getPosition());