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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-30 20:59:51 -06:00
parent 1b9f10693c
commit 0fc70c5f24
4 changed files with 44 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java
View File

@@ -485,16 +485,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
if (ticker == 0) {
stamp = System.currentTimeMillis();
robot.limelight.pipelineSwitch(1);
}
ticker++;
robot.limelight.pipelineSwitch(1);
motif = turret.detectObelisk();
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
robot.turr1.setPosition(turrPos);
robot.turr2.setPosition(1 - turrPos);
@@ -511,7 +506,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};

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

@@ -20,7 +20,7 @@ public class ServoPositions {
public static double spinEndPos = 0.85;
public static double shootAllAutoSpinStartPos = 0.2;
public static double shootAllSpindexerSpeedIncrease = 0.02;
public static double shootAllSpindexerSpeedIncrease = 0.014;
public static double shootAllTime = 1.8;
public static double transferServo_out = 0.15;

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

@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
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.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
@@ -39,10 +40,12 @@ public class TeleopV3 extends LinearOpMode {
public double vel = 3000;
public boolean autoVel = true;
public boolean targetingHood = true;
public double manualOffset = 0.0;
public static double manualOffset = 0.0;
public boolean autoHood = true;
public double shootStamp = 0.0;
public static double hoodSpeedOffset = 0.01;
public static double turretSpeedOffset = 0.01;
boolean fixedTurret = false;
Robot robot;
@@ -191,43 +194,40 @@ public class TeleopV3 extends LinearOpMode {
//HOOD:
if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle);
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset);
}
if (gamepad2.dpadUpWasPressed() || gamepad1.dpadUpWasPressed()) {
hoodOffset -= 0.03;
autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed() || gamepad1.dpadDownWasPressed()) {
hoodOffset += 0.03;
autoHoodOffset += 0.02;
if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= hoodSpeedOffset;
autoHoodOffset -= hoodSpeedOffset;
gamepad2.rumble(80);
} else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += hoodSpeedOffset;
autoHoodOffset += hoodSpeedOffset;
gamepad2.rumble(80);
}
if (gamepad2.cross) {
manualOffset = 0;
overrideTurr = true;
if (gamepad2.dpadLeftWasPressed()){
manualOffset -= turretSpeedOffset;
gamepad2.rumble(80);
} else if (gamepad2.dpadRightWasPressed()){
manualOffset += turretSpeedOffset;
gamepad2.rumble(80);
}
if (gamepad2.squareWasPressed()) {
drive = new MecanumDrive(hardwareMap, new Pose2d(20, 0, 0));
sleep(1500);
if (gamepad2.rightBumperWasPressed()){
limelightUsed = true;
gamepad2.rumble(80);
} else if (gamepad2.leftBumperWasPressed()){
limelightUsed = false;
gamepad2.rumble(80);
}
if (gamepad2.triangle) {
autoHood = false;
hoodOffset = 0;
}
if (gamepad2.crossWasPressed()){
if (gamepad2.circleWasPressed()) {
xOffset = robotX;
yOffset = robotY;
headingOffset = robotHeading;
autoHood = true;
fixedTurret = false;
}
if (enableSpindexerManager) {
@@ -330,8 +330,6 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
// TELE.update();
ticker++;
}
}

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

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.manualOffset;
import static java.lang.Math.abs;
@@ -26,6 +27,7 @@ public class Turret {
public static double turrDefault = 0.4;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static boolean limelightUsed = true;
public static double visionCorrectionGain = 0.08; // Single tunable gain
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
@@ -54,7 +56,7 @@ public class Turret {
LLResult result;
private PIDController bearingPID;
public static double B_PID_P = 0.15, B_PID_I = 0.0, B_PID_D = 0.025;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
boolean bearingAligned = false;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
@@ -84,11 +86,6 @@ public class Turret {
}
private void limelightRead() { // only for tracking purposes, not general reads
if (redAlliance) {
webcam.pipelineSwitch(4);
} else {
webcam.pipelineSwitch(2);
}
result = webcam.getLatestResult();
if (result != null) {
@@ -223,7 +220,7 @@ public class Turret {
limelightRead();
// Active correction if we see the target
if (result.isValid() && !lockOffset) {
if (result.isValid() && !lockOffset && limelightUsed) {
currentTrackOffset += bearingAlign(result);
currentTrackCount++;
// double bearingError = Math.abs(tagBearingDeg);
@@ -282,8 +279,8 @@ public class Turret {
}
// Set servo positions
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);
robot.turr1.setPosition(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
/* ---------------- TELEMETRY ---------------- */