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

Add coloooor sensooooooer!!!!

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-05 21:57:23 -06:00
parent 1642e161c5
commit 2f5d4638ec
4 changed files with 83 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,7 +26,7 @@ public class Poses {
public static double rx1 = 50, ry1 = -4, rh1 = 0;
public static double rx1 = 46, ry1 = -4, rh1 = 0;
public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
@@ -36,7 +36,7 @@ public class Poses {
public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
public static double bx1 = 50, by1 = 4, bh1 = 0;
public static double bx1 = 46, by1 = 4, bh1 = 0;
public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140);

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

@@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
@@ -21,15 +20,12 @@ public class ServoPositions {
public static double transferServo_in = 0.38;
public static double turret_range = 0.9;
public static double hoodDefault = 0.6;
public static double hoodAuto = 0.59;
public static double hoodHigh = 0.21;
public static double hoodLow = 1.0;
@@ -41,5 +37,4 @@ public class ServoPositions {
public static double turret_detectBlue = 0.6;
}

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

@@ -40,6 +40,7 @@ public class TeleopV2 extends LinearOpMode {
public static double hoodDefaultPos = 0.5;
public static double desiredTurretAngle = 180;
public static double velMultiplier = 20;
public static double shootStamp2 = 0.0;
public double vel = 3000;
public boolean autoVel = true;
public double manualOffset = 0.0;
@@ -51,6 +52,7 @@ public class TeleopV2 extends LinearOpMode {
public boolean circle = false;
public boolean square = false;
public boolean triangle = false;
double autoHoodOffset = 0.0;
Robot robot;
MultipleTelemetry TELE;
boolean intake = false;
@@ -399,15 +401,19 @@ public class TeleopV2 extends LinearOpMode {
//HOOD:
if (autoHood) {
robot.hood.setPosition(hoodAnglePrediction(distanceToGoal));
robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset);
}
if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= 0.03;
autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += 0.03;
autoHoodOffset += 0.02;
}
if (gamepad2.left_stick_x > 0.5) {
@@ -415,6 +421,7 @@ public class TeleopV2 extends LinearOpMode {
} else if (gamepad2.left_stick_x < -0.5) {
manualTurret = true;
manualOffset = 0;
autoHoodOffset = 0;
if (gamepad2.left_bumper) {
xOffset = robotX;
yOffset = robotY;
@@ -461,7 +468,6 @@ public class TeleopV2 extends LinearOpMode {
} else if (shootAll) {
TELE.addData("100% works", shootOrder);
TELE.update();
intake = false;
reject = false;
@@ -493,7 +499,7 @@ public class TeleopV2 extends LinearOpMode {
switch (currentSlot) {
case 1:
shootA = shootTeleop(spindexer_outtakeBall1, outtake1);
shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2);
TELE.addData("shootA", shootA);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
@@ -503,7 +509,7 @@ public class TeleopV2 extends LinearOpMode {
}
break;
case 2:
shootB = shootTeleop(spindexer_outtakeBall2, outtake2);
shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2);
TELE.addData("shootB", shootB);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootB;
@@ -512,7 +518,7 @@ public class TeleopV2 extends LinearOpMode {
}
break;
case 3:
shootC = shootTeleop(spindexer_outtakeBall3, outtake3);
shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2);
TELE.addData("shootC", shootC);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootC;
@@ -525,6 +531,8 @@ public class TeleopV2 extends LinearOpMode {
// Remove from the list only if shooting is complete
if (shootingDone) {
shootOrder.remove(0);
shootStamp2 = getRuntime();
}
} else {
@@ -548,6 +556,7 @@ public class TeleopV2 extends LinearOpMode {
if (gamepad2.squareWasPressed()) {
square = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
@@ -556,6 +565,8 @@ public class TeleopV2 extends LinearOpMode {
if (gamepad2.circleWasPressed()) {
circle = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
@@ -565,6 +576,8 @@ public class TeleopV2 extends LinearOpMode {
if (gamepad2.triangleWasPressed()) {
triangle = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
@@ -614,9 +627,39 @@ public class TeleopV2 extends LinearOpMode {
outtake3 = false;
// Fastest order (example: slot 3 → 2 → 1)
shootOrder.add(3);
shootOrder.add(2);
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;
shootPos = drive.localizer.getPose();
@@ -635,9 +678,9 @@ public class TeleopV2 extends LinearOpMode {
hub.clearBulkCache();
}
TELE.addData("Spin1Green", green1);
TELE.addData("Spin2Green", green2);
TELE.addData("Spin3Green", green3);
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
@@ -683,13 +726,13 @@ public class TeleopV2 extends LinearOpMode {
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public boolean shootTeleop(double spindexer, boolean spinOk) {
public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
// Set spin positions
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1 - spindexer);
// Check if spindexer has reached the target position
if (spinOk) {
if (spinOk || getRuntime() - stamp > 1.5) {
if (tickerA == 1) {
transferStamp = getRuntime();
tickerA++;
@@ -743,7 +786,6 @@ public class TeleopV2 extends LinearOpMode {
}
}
void addOddThenRest(List<Integer> order, boolean oddColor) {
// Odd ball first
for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
@@ -816,4 +858,31 @@ public class TeleopV2 extends LinearOpMode {
return false; // default
}
boolean ballIn(int slot) {
switch (slot) {
case 1:
if (!s1T.isEmpty()) {
return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3);
}
case 2:
if (!s2T.isEmpty()) {
return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3);
}
case 3:
if (!s3T.isEmpty()) {
return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3);
}
}
return true; // default
}
}

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

@@ -17,7 +17,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
public static double hoodPos = 0.501;
public static double scalar = 1.112;
public static double restPos = 0.158;
public static double restPos = 0.15;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);