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

night before

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-08 22:29:36 -05:00
parent b2a1ea32d9
commit d0ee294d26
10 changed files with 107 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
View File

@@ -393,7 +393,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()){
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
park.unpark();
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);

77
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java
View File

@@ -14,6 +14,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -39,13 +40,15 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
double runtime = 0;
// Wait Times
public static double rapidWaitTime = 0.4;
public static double rapidShootTime = 0.45;
public static double openGate1Time = 1.5;
public static double openGate2Time = 1.5;
public static double openGateWaitTimeMax = 3;
public static double openGateWaitTimeMin = 1.75;
public static int maxLoopCycles = 3;
public static double rapidWaitTimeShoot0 = 0.2;
public static double rapidWaitTimeShoot1 = 0.2;
public static double rapidWaitTimeShoot2 = 0.2;
public static double rapidWaitTimeShootGate = 0.4;
public static double rapidShootTime = 0.4;
public static double openGate1Time = 1.8;
public static double openGate2Time = 1;
public static double openGateWaitTimeMax = 3.5;
public static int maxLoopCycles = 4;
// Initialize path state machine
private enum PathState {
@@ -70,11 +73,12 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
public static double openGate2ControlX = 45.9782359679267, openGate2ControlY = -15.106643757159245;
public static double openGate2X = 57, openGate2Y = -8, openGate2H = 0;
public static double shoot2ControlX = 57, shoot2ControlY = -8;
public static double shoot2X = 25, shoot2Y = 11, shoot2H = -30;
public static double intakeGateControlX = 61, intakeGateControlY = -11;
public static double intakeGateX = 61, intakeGateY = -11, intakeGateH = 20;
public static double shootGateControlX = 56, shootGateControlY = -10;
public static double shootGateX = 25, shootGateY = 11, shootGateH = -30;
public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30;
public static double intakeGateControlX = 60, intakeGateControlY = -12;
public static double toGateX = 60, toGateY = -10;
public static double intakeGateX = 62, intakeGateY = -12.5, intakeGateH = 25;
public static double shootGateControlX = 40, shootGateControlY = -5;
public static double shootGateX = 16, shootGateY = 4, shootGateH = -30;
public static double shootLeaveControlX = 56, shootLeaveControlY = -10;
public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50;
public static double leaveX = 45, leaveY = 10, leaveH = 0;
@@ -83,22 +87,22 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X,
pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X,
intakeGateControlX, intakeGateX, shootGateControlX, shootGateX,
shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX};
shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX, toGateX};
double[] yPoses = {startPoseY, shoot0Y,
pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y,
pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y,
intakeGateControlY, intakeGateY, shootGateControlY, shootGateY,
shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY};
shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY, toGateY};
double[] headings = {startPoseH, shoot0H,
0, pickup1H, 0, openGate1H, 0, shoot1H,
0, pickup2H, 0, openGate2H, 0, shoot2H,
0, intakeGateH, 0, shootGateH,
0, shootLeaveH, leaveH, awayFromGateH};
0, shootLeaveH, leaveH, awayFromGateH, 0};
Pose startPose, shoot0,
pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1,
pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2,
intakeGateControl, intakeGate, shootGateControl, shootGate,
shootLeaveControl, shootLeave, leave, awayFromGate;
shootLeaveControl, shootLeave, leave, awayFromGate, toGate;
private void initializePoses(){
startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0]));
shoot0 = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1]));
@@ -122,12 +126,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
shootLeave = new Pose(xPoses[19], yPoses[19], Math.toRadians(headings[19]));
leave = new Pose(xPoses[20], yPoses[20], Math.toRadians(headings[20]));
awayFromGate = new Pose(xPoses[21], yPoses[21], Math.toRadians(headings[21]));
toGate = new Pose(xPoses[22], yPoses[22]);
}
//Building Paths
PathChain startPose_shoot0, shoot0_pickup1, pickup1_openGate1, openGate1_shoot1,
shoot1_pickup2, pickup2_openGate2, openGate2_shoot2,
shoot2_intakeGate, intakeGate_shootGate, shootGate_intakeGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate;
shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, shootGate_intakeGate,
shoot2_intakeGate, intakeGate_shootGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate;
private void buildPaths(){
startPose_shoot0 = follower.pathBuilder()
.addPath(new BezierLine(startPose, shoot0))
@@ -160,27 +165,29 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
.build();
openGate2_shoot2 = follower.pathBuilder()
.addPath(new BezierCurve(openGate2, shoot2Control, shoot2))
.setLinearHeadingInterpolation(openGate2.getHeading(), shoot2.getHeading())
.addPath(new BezierLine(openGate2, shoot2))
.setTangentHeadingInterpolation()
.setReversed()
.build();
shoot2_intakeGate = follower.pathBuilder()
.addPath(new BezierCurve(shoot2, intakeGateControl, intakeGate))
.addPath(new BezierCurve(shoot2, intakeGateControl, toGate))
.setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading())
.build();
intakeGate_shootGate = follower.pathBuilder()
.addPath(new BezierCurve(intakeGate, shootGateControl, shootGate))
.setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading())
.setTangentHeadingInterpolation()
.setReversed()
.build();
shootGate_intakeGate = follower.pathBuilder()
.addPath(new BezierCurve(shootGate, intakeGateControl, intakeGate))
.addPath(new BezierLine(shootGate, intakeGate))
.setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading())
.build();
intakeGate_shootLeave = follower.pathBuilder()
.addPath(new BezierCurve(intakeGate, shootLeaveControl, shootLeave))
.addPath(new BezierCurve(intakeGate, pickup2Control, shootLeave))
.setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading())
.build();
@@ -199,6 +206,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
private boolean startAuto = true;
private double timeStamp = 0;
private int cycle = 0;
boolean toGateBool = false;
private void pathStateMachine(){
double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){
@@ -210,11 +218,11 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
follower.followPath(startPose_shoot0, false);
startAuto = false;
}
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
if ((follower.atParametricEnd() || !follower.isBusy()) && currentTime - timeStamp > rapidWaitTimeShoot0){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
} else if (follower.isBusy()){
} else if (follower.isBusy() && !follower.atParametricEnd()){
timeStamp = currentTime;
}
break;
@@ -241,7 +249,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
}
break;
case DRIVE_SHOOT1:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot1){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT1;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -259,6 +267,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
break;
case PICKUP2:
if (!follower.isBusy()){
shooter.setFlywheelVelocity(2500);
robot.setHoodPos(0.6);
follower.followPath(pickup2_openGate2, false);
pathState = PathState.OPENGATE2;
timeStamp = currentTime;
@@ -272,7 +282,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
}
break;
case DRIVE_SHOOT2:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot2){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT2;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -287,17 +297,19 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
follower.followPath(shoot2_intakeGate, false);
pathState = PathState.INTAKE_GATE;
timeStamp = currentTime;
toGateBool = true;
}
break;
case INTAKE_GATE:
if ((currentTime - timeStamp > openGateWaitTimeMax || (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()))
&& (currentTime - timeStamp > openGateWaitTimeMin)){
if ((currentTime - timeStamp > openGateWaitTimeMax)){
if (getRuntime() - runtime > 27){
follower.followPath(intakeGate_awayFromGate, true);
pathState = PathState.WAIT_SHOOT_LEAVE;
} else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){
follower.followPath(intakeGate_shootLeave, true);
pathState = PathState.DRIVE_SHOOT_LEAVE;
shooter.setFlywheelVelocity(2300);
robot.setHoodPos(0.68);
} else {
follower.followPath(intakeGate_shootGate, false);
pathState = PathState.DRIVE_SHOOT_GATE;
@@ -307,7 +319,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
// TODO: add logic to shoot gate
break;
case DRIVE_SHOOT_GATE:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_GATE;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -326,12 +338,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
} else {
follower.followPath(shootGate_intakeGate, false);
pathState = PathState.INTAKE_GATE;
toGateBool = true;
}
timeStamp = currentTime;
}
break;
case DRIVE_SHOOT_LEAVE:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_LEAVE;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);

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

@@ -37,7 +37,7 @@ public class ServoPositions {
public static double transferServo_out = 0.28;
public static double transferServo_in = 0.54;
public static double transferServo_in = 0.52;
public static double hoodAuto = 0.27;

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -133,6 +133,7 @@ public class TeleopV4 extends LinearOpMode {
follower.update();
Pose currentPose = follower.getPose();
teleStart = currentPose;
if (gamepad1.dpadLeftWasPressed()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -259,9 +260,9 @@ public class TeleopV4 extends LinearOpMode {
}
if (gamepad2.dpadRightWasPressed()){
flywheelOffset+=100;
flywheelOffset+=50;
} else if (gamepad2.dpadLeftWasPressed()){
flywheelOffset-=100;
flywheelOffset-=50;
}
if (gamepad2.crossWasPressed()){
@@ -279,6 +280,10 @@ public class TeleopV4 extends LinearOpMode {
// TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("Velocity 1", flywheel.getVelo1());
TELE.addData("Velocity 2", flywheel.getVelo2());
TELE.addData("Flywheel Offset", flywheelOffset);
TELE.addData("Hood Offset", hoodOffset);
//
// TELE.addData("Current Position", currentPose);
//

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
View File

@@ -131,7 +131,7 @@ public class Hardware_Tester extends LinearOpMode {
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red));
TELE.addData("REV Green", revColor.green / (revColor.green + revColor.blue + revColor.red));
TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
View File

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
private boolean shooting = false;
public static int flywheelVelo = 0;
public static double hoodPos = 0.5;
public static double transferPower = -0.8;
public static double transferPower = -1;
@Override
public void runOpMode() throws InterruptedException {

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
View File

@@ -83,11 +83,15 @@ public class Flywheel {
// pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
if (velo1 != 0){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
} else {
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
}
private double prevF = 0;
public static double voltagePIDFDifference = 1;
double averageVoltage = 0;
public void setF(double voltage){
@@ -95,7 +99,11 @@ public class Flywheel {
double f = shooterPIDF_F / voltage;
if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
shooterPIDF.f = f;
if (velo1 != 0){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
} else {
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
}
prevF = f;
}
@@ -134,21 +142,43 @@ public class Flywheel {
double power;
double prevTargetTime = 0;
double prevTargetVelocity = 0;
int veloMode = 0;
public void manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
targetVelocity = commandedVelocity;
}
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
power = robot.shooter1.getPower();
robot.shooter2.setPower(power);
if (velo1 < 100){
}
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
if (velo1 < 100){
velo1 = 0;
velo = velo2;
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
if (veloMode != 2){
robot.shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
veloMode = 2;
}
power = robot.shooter2.getPower();
robot.shooter1.setPower(power);
} else {
velo2 = 0;
velo = velo1;
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
if (veloMode != 1){
robot.shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
veloMode = 1;
}
power = robot.shooter1.getPower();
robot.shooter2.setPower(power);
}
updateVelocityAverage(velo);

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -53,8 +53,8 @@ public class SpindexerTransferIntake {
}
int[] shootOrder = {0, 1, 2};
final double sensorDistanceThreshold = 5.3;
final long pulseTime = 100; // ms
public static final double sensorDistanceThreshold = 5.35;
final long pulseTime = 70; // ms
private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -325,7 +325,7 @@ public class SpindexerTransferIntake {
robot.setSpinPos(
ServoPositions.spindexer_A2
);
robot.setTransferPower(-0.7);
robot.setTransferPower(-1);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
@@ -335,7 +335,7 @@ public class SpindexerTransferIntake {
holdBallsTicker++;
}
if (holdBallsTicker > 10){
if (holdBallsTicker > 20){
setRapidMode(RapidMode.TRANSFER_OFF);
holdBallsTicker = 0;
}
@@ -355,7 +355,7 @@ public class SpindexerTransferIntake {
robot.setIntakePower(1.0);
if (stateTime() >= 300) {
if (stateTime() >= 700) {
setRapidMode(RapidMode.PULSE_OUT);
}
@@ -375,25 +375,23 @@ public class SpindexerTransferIntake {
robot.setIntakePower(1.0);
if (stateTime() >= 200) {
if (stateTime() >= 400) {
setRapidMode(RapidMode.HOLD_BALLS);
}
break;
case HOLD_BALLS:
if (robot.insideBeam.isPressed()
&& robot.outsideBeam.isPressed()) {
&& robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
robot.setTransferPower(0);
robot.setIntakePower(0.1);
robot.setTransferServoPos(ServoPositions.transferServo_in);
} else {
holdBallsTicker++;
robot.setIntakePower(1);
setRapidMode(RapidMode.INTAKE);
}
break;
@@ -679,7 +677,6 @@ public class SpindexerTransferIntake {
robot.setTransferPower(-0.8);
}
switch (shootState) {
case IDLE:
shootOrder = buildShootOrder(

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -17,8 +17,8 @@ public class Turret {
private final double servoTicksPer180 = 0.58;
public static double neutralPosition = 0.51;
private final double turretMin = 0.05;
private final double turretMax = 0.95;
private final double turretMin = 0.08;
private final double turretMax = 0.91;
public static boolean limelightUsed = true;
public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
LLResult result;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
View File

@@ -67,6 +67,10 @@ public class VelocityCommander {
hoodC*Math.pow(dist, 1) +
hoodD;
}
if (lockBack){
hoodPos-=0.04;
}
hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
}
public double getHoodPredicted(){
@@ -75,7 +79,7 @@ public class VelocityCommander {
private void distToTransferPow(double dist, double voltage){
if (dist < 140){
transferPow = -0.75;
transferPow = -1;
} else {
transferPow = -0.6;
}