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

LUUUUNCH

Browse Source
This commit is contained in:
Keshav Anand
2025-12-06 12:02:00 -06:00
parent d586e3b4df
commit 554204b6d4
6 changed files with 132 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
View File

@@ -29,7 +29,7 @@ import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
@Autonomous @Autonomous(preselectTeleOp = "TeleopV2")
public class Blue_V2 extends LinearOpMode { public class Blue_V2 extends LinearOpMode {
Robot robot; Robot robot;
@@ -44,9 +44,9 @@ public class Blue_V2 extends LinearOpMode {
double velo = 0.0; double velo = 0.0;
double targetVelocity = 0.0; double targetVelocity = 0.0;
public static double intake1Time = 6.5; public static double intake1Time = 2.9;
public static double intake2Time = 6.5; public static double intake2Time = 2.9;
public static double colorDetect = 3.0; public static double colorDetect = 3.0;
@@ -187,6 +187,39 @@ public class Blue_V2 extends LinearOpMode {
}; };
} }
public Action intakeReject() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
double position = 0.0;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.3){
return true;
}else {
robot.intake.setPower(0);
return false;
}
}
};
}
public Action spindex (double spindexer, double vel){ public Action spindex (double spindexer, double vel){
return new Action() { return new Action() {
double currentPos = 0.0; double currentPos = 0.0;
@@ -359,7 +392,6 @@ public class Blue_V2 extends LinearOpMode {
robot.intake.setPower(1); robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
robot.intake.setPower(0);
return false; return false;
} else { } else {
return true; return true;
@@ -567,7 +599,8 @@ public class Blue_V2 extends LinearOpMode {
new ParallelAction( new ParallelAction(
shoot1.build(), shoot1.build(),
ColorDetect(), ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true) steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
) )
); );
@@ -599,7 +632,8 @@ public class Blue_V2 extends LinearOpMode {
new ParallelAction( new ParallelAction(
shoot2.build(), shoot2.build(),
ColorDetect(), ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true) steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
) )
); );

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -28,7 +28,7 @@ import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
@Autonomous @Autonomous(preselectTeleOp = "TeleopV2")
public class Red_V2 extends LinearOpMode { public class Red_V2 extends LinearOpMode {
Robot robot; Robot robot;
@@ -43,9 +43,9 @@ public class Red_V2 extends LinearOpMode {
double velo = 0.0; double velo = 0.0;
double targetVelocity = 0.0; double targetVelocity = 0.0;
public static double intake1Time = 6.5; public static double intake1Time = 2.9;
public static double intake2Time = 6.5; public static double intake2Time = 2.9;
public static double colorDetect = 3.0; public static double colorDetect = 3.0;
@@ -358,7 +358,6 @@ public class Red_V2 extends LinearOpMode {
robot.intake.setPower(1); robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
robot.intake.setPower(0);
return false; return false;
} else { } else {
return true; return true;
@@ -367,6 +366,27 @@ public class Red_V2 extends LinearOpMode {
}; };
} }
public Action intakeReject() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.3){
return true;
}else {
robot.intake.setPower(0);
return false;
}
}
};
}
public Action ColorDetect() { public Action ColorDetect() {
return new Action() { return new Action() {
double stamp = 0.0; double stamp = 0.0;
@@ -567,7 +587,8 @@ public class Red_V2 extends LinearOpMode {
new ParallelAction( new ParallelAction(
shoot1.build(), shoot1.build(),
ColorDetect(), ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true) steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
) )
); );
@@ -599,7 +620,9 @@ public class Red_V2 extends LinearOpMode {
new ParallelAction( new ParallelAction(
shoot2.build(), shoot2.build(),
ColorDetect(), ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true) steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
) )
); );

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

@@ -26,7 +26,7 @@ public class Poses {
public static double rx1 = 46, ry1 = -4, rh1 = 0; public static double rx1 = 45, ry1 = -7, rh1 = 0;
public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
@@ -36,16 +36,16 @@ public class Poses {
public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
public static double bx1 = 46, by1 = 4, bh1 = 0; public static double bx1 = 45, by1 = 6, bh1 = 0;
public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140); public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140);
public static double bx2b = 31, by2b = -32, bh2b = Math.toRadians(-140); public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140);
public static double bx3a = 58, by3a = -42, bh3a = Math.toRadians(-140); public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140);
public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140);
public static Pose2d teleStart = new Pose2d(rx1, 10, 0); public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1);
} }

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

@@ -24,13 +24,13 @@ public class ServoPositions {
public static double hoodDefault = 0.6; public static double hoodDefault = 0.6;
public static double hoodAuto = 0.59; public static double hoodAuto = 0.55;
public static double hoodHigh = 0.21; public static double hoodHigh = 0.21;
public static double hoodLow = 1.0; public static double hoodLow = 1.0;
public static double turret_red = 0.4; public static double turret_red = 0.42;
public static double turret_blue = 0.38; public static double turret_blue = 0.38;
public static double turret_detectRed = 0.2; public static double turret_detectRed = 0.2;

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

@@ -73,13 +73,13 @@ public final class MecanumDrive {
public double kA = 0.000046; public double kA = 0.000046;
// path profile parameters (in inches) // path profile parameters (in inches)
public double maxWheelVel = 120; public double maxWheelVel = 180;
public double minProfileAccel = -30; public double minProfileAccel = -40;
public double maxProfileAccel = 120; public double maxProfileAccel = 180;
// turn profile parameters (in radians) // turn profile parameters (in radians)
public double maxAngVel = 2* Math.PI; // shared with path public double maxAngVel = 4* Math.PI; // shared with path
public double maxAngAccel = 2* Math.PI; public double maxAngAccel = 4* Math.PI;
// path controller gains // path controller gains
public double axialGain = 4; public double axialGain = 4;

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -43,7 +43,6 @@ public class TeleopV2 extends LinearOpMode {
public static double velMultiplier = 20; public static double velMultiplier = 20;
public static double shootStamp2 = 0.0; public static double shootStamp2 = 0.0;
public double vel = 3000; public double vel = 3000;
public boolean autoVel = true; public boolean autoVel = true;
public double manualOffset = 0.0; public double manualOffset = 0.0;
@@ -89,7 +88,6 @@ public class TeleopV2 extends LinearOpMode {
boolean outtake3 = false; boolean outtake3 = false;
boolean overrideTurr = false; boolean overrideTurr = false;
List<Integer> shootOrder = new ArrayList<>(); List<Integer> shootOrder = new ArrayList<>();
boolean emergency = false; boolean emergency = false;
private double lastEncoderRevolutions = 0.0; private double lastEncoderRevolutions = 0.0;
@@ -171,7 +169,6 @@ public class TeleopV2 extends LinearOpMode {
emergency = false; emergency = false;
overrideTurr = false; overrideTurr = false;
} }
if (gamepad1.leftBumperWasPressed()) { if (gamepad1.leftBumperWasPressed()) {
@@ -180,7 +177,6 @@ public class TeleopV2 extends LinearOpMode {
shootAll = false; shootAll = false;
overrideTurr = false; overrideTurr = false;
} }
if (intake) { if (intake) {
@@ -336,8 +332,11 @@ public class TeleopV2 extends LinearOpMode {
double offset; double offset;
double robotX = drive.localizer.getPose().position.x - xOffset; double robX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y - xOffset; double robY = drive.localizer.getPose().position.y;
double robotX = robX - xOffset;
double robotY = robY - yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble(); double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -10; double goalX = -10;
@@ -431,12 +430,11 @@ public class TeleopV2 extends LinearOpMode {
if (gamepad2.left_stick_x > 0.5) { if (gamepad2.left_stick_x > 0.5) {
manualTurret = false; manualTurret = false;
} else if (gamepad2.left_stick_x < -0.5) { } else if (gamepad2.left_stick_x < -0.5) {
manualTurret = true;
manualOffset = 0; manualOffset = 0;
autoHoodOffset = 0; manualTurret = true;
if (gamepad2.left_bumper) { if (gamepad2.left_bumper) {
xOffset = robotX; xOffset = robX;
yOffset = robotY; yOffset = robY;
headingOffset = robotHeading; headingOffset = robotHeading;
} }
} }
@@ -456,7 +454,7 @@ public class TeleopV2 extends LinearOpMode {
//SHOOT ALL:] //SHOOT ALL:]
if (emergency) { if (emergency) {
intake = false; intake = true;
reject = false; reject = false;
if (getRuntime() % 3 > 1.5) { if (getRuntime() % 3 > 1.5) {
@@ -494,25 +492,22 @@ public class TeleopV2 extends LinearOpMode {
if (d20 != null) { if (d20 != null) {
overrideTurr = true; overrideTurr = true;
double bearing = d20.ftcPose.bearing; double bearing = d20.ftcPose.bearing;
double finalPos =robot.turr1.getPosition() - (bearing/1300);
double finalPos = robot.turr1.getPosition() - (bearing / 1300);
robot.turr1.setPosition(finalPos); robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1-finalPos); robot.turr2.setPosition(1 - finalPos);
TELE.addData("Bear", bearing); TELE.addData("Bear", bearing);
} }
if (d24 != null) { if (d24 != null) {
overrideTurr = true; overrideTurr = true;
double bearing = d24.ftcPose.bearing; double bearing = d24.ftcPose.bearing;
double finalPos = turretPos() + (bearing/720); double finalPos = robot.turr1.getPosition() - (bearing / 1300);
robot.turr1.setPosition(finalPos); robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1-finalPos); robot.turr2.setPosition(1 - finalPos);
} }
@@ -577,10 +572,14 @@ public class TeleopV2 extends LinearOpMode {
outtake1 = false; outtake1 = false;
outtake2 = false; outtake2 = false;
outtake3 = false; outtake3 = false;
overrideTurr = false; overrideTurr = false;
} }
} }
@@ -660,17 +659,17 @@ public class TeleopV2 extends LinearOpMode {
// Fastest order (example: slot 3 → 2 → 1) // Fastest order (example: slot 3 → 2 → 1)
if (ballIn(3)){ if (ballIn(3)) {
shootOrder.add(3); shootOrder.add(3);
} }
if (ballIn(2)){ if (ballIn(2)) {
shootOrder.add(2); shootOrder.add(2);
} }
if (ballIn(1)){ if (ballIn(1)) {
shootOrder.add(1); shootOrder.add(1);
} }
@@ -690,38 +689,37 @@ public class TeleopV2 extends LinearOpMode {
shootOrder.add(1); shootOrder.add(1);
} }
shootAll = true;
shootPos = drive.localizer.getPose();
}
// Right bumper shoots all balls fastest, ignoring colors
if (gamepad2.leftBumperWasPressed()) {
shootOrder.clear();
shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
// Fastest order (example: slot 3 → 2 → 1)
if (ballIn(3)) {
shootOrder.add(3);
}
if (ballIn(2)) {
shootOrder.add(2);
}
if (ballIn(1)) {
shootOrder.add(1);
}
shootAll = true; shootAll = true;
shootPos = drive.localizer.getPose(); shootPos = drive.localizer.getPose();
} }
// // Right bumper shoots all balls fastest, ignoring colors
// if (gamepad2.leftBumperWasPressed()) {
// shootOrder.clear();
// shootStamp = getRuntime();
//
// outtake1 = false;
// outtake2 = false;
// outtake3 = false;
//
// // Fastest order (example: slot 3 → 2 → 1)
//
// if (ballIn(3)) {
// shootOrder.add(3);
// }
//
// if (ballIn(2)) {
// shootOrder.add(2);
// }
// if (ballIn(1)) {
// shootOrder.add(1);
// }
// shootAll = true;
// shootPos = drive.localizer.getPose();
//
// }
//
if (gamepad2.crossWasPressed()) { if (gamepad2.crossWasPressed()) {
emergency = !emergency; emergency = !emergency;
@@ -943,7 +941,7 @@ public class TeleopV2 extends LinearOpMode {
} }
public double turretPos() { public double turretPos() {
return (scalar*((robot.turr1Pos.getVoltage()-restPos) /3.3 )); return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
} }
} }