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

stadg

Browse Source
This commit is contained in:
Keshav Anand
2026-01-16 23:06:05 -06:00
parent ef5d615f91
commit de6e7f2910
3 changed files with 106 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,6 +40,6 @@ public class ServoPositions {
public static double turret_detectRedClose = 0.2; public static double turret_detectRedClose = 0.2;
public static double turret_detectBlueClose = 0.6; public static double turret_detectBlueClose = 0.6;
public static double turrDefault = 0.40; public static double turrDefault = 0;
} }

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

@@ -21,7 +21,6 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.Actions;
import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -47,6 +46,9 @@ public class TeleopV3 extends LinearOpMode {
public static double spinningPow = 0.2; public static double spinningPow = 0.2;
public static double spindexPos = spindexer_intakePos1; public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.08; public static double spinPow = 0.08;
public static double bMult = -1, bDiv = 130000;
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static boolean manualTurret = false;
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;
@@ -60,6 +62,7 @@ public class TeleopV3 extends LinearOpMode {
public boolean triangle = false; public boolean triangle = false;
public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
boolean fixedTurret = false;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF); PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
@@ -67,6 +70,8 @@ public class TeleopV3 extends LinearOpMode {
FlywheelV2 flywheel; FlywheelV2 flywheel;
MecanumDrive drive; MecanumDrive drive;
double autoHoodOffset = 0.0; double autoHoodOffset = 0.0;
int shooterTicker = 0;
boolean intake = false; boolean intake = false;
boolean reject = false; boolean reject = false;
double xOffset = 0.0; double xOffset = 0.0;
@@ -88,7 +93,6 @@ public class TeleopV3 extends LinearOpMode {
boolean shootA = true; boolean shootA = true;
boolean shootB = true; boolean shootB = true;
boolean shootC = true; boolean shootC = true;
boolean manualTurret = false;
boolean autoSpintake = true; boolean autoSpintake = true;
List<Integer> shootOrder = new ArrayList<>(); List<Integer> shootOrder = new ArrayList<>();
boolean outtake1 = false; boolean outtake1 = false;
@@ -96,7 +100,7 @@ public class TeleopV3 extends LinearOpMode {
boolean outtake3 = false; boolean outtake3 = false;
boolean overrideTurr = false; boolean overrideTurr = false;
double turretPID = 0.0; double turretPID = 0.0;
double turretPos = 0.5; double turretPos = 0;
double spindexPID = 0.0; double spindexPID = 0.0;
double error = 0.0; double error = 0.0;
double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0; double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0;
@@ -138,6 +142,10 @@ public class TeleopV3 extends LinearOpMode {
flywheel = new FlywheelV2(); flywheel = new FlywheelV2();
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
if (redAlliance) { if (redAlliance) {
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
} else { } else {
@@ -205,11 +213,6 @@ public class TeleopV3 extends LinearOpMode {
} }
// PID SERVOS
turretPID = servo.setTurrPos(turretPos);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
//TODO: Use color sensors to switch between positions...switch after ball detected in //TODO: Use color sensors to switch between positions...switch after ball detected in
if (autoSpintake) { if (autoSpintake) {
@@ -378,7 +381,7 @@ public class TeleopV3 extends LinearOpMode {
desiredTurretAngle += manualOffset; desiredTurretAngle += manualOffset;
offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); offset = (desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)));
if (offset > 135) { if (offset > 135) {
offset -= 360; offset -= 360;
@@ -420,60 +423,79 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("offset", offset); TELE.addData("offset", offset);
pos -= offset * (0.9 / 360); pos -= offset * (4.04 / 360);
if (pos < 0.02) { TELE.addData("posS1", pos);
pos = 0.02;
} else if (pos > 0.97) { if (pos < -1.5) {
pos = 0.97; pos = -1.5;
} else if (pos > 1.8) {
pos = 1.8;
} }
if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving TELE.addData("posS2", pos);
double bearing;
LLResult result = robot.limelight.getLatestResult(); // if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
if (result != null) { // double bearing;
if (result.isValid()) { //
bearing = result.getTx(); // LLResult result = robot.limelight.getLatestResult();
overrideTurr = true; // if (result != null) {
turretPos = servo.getTurrPos() - (bearing / 1300); // if (result.isValid()) {
// bearing = result.getTx() * bMult;
double bearingCorrection = bearing / 1300; // overrideTurr = true;
//
// deadband: ignore tiny noise // double bearingCorrection = bearing / bDiv;
if (Math.abs(bearing) > 0.3 && camTicker < 8) { //
// // deadband: ignore tiny noise
// only accumulate if bearing direction is consistent // if (Math.abs(bearing) > 0.3 && camTicker < 8) {
if (Math.signum(bearingCorrection) == Math.signum(error) || //
error == 0) { // error += bearingCorrection;
error += bearingCorrection; //
} // }
} // camTicker++;
camTicker++; // TELE.addData("tx", bearing);
TELE.addData("tx", bearing); // TELE.addData("ty", result.getTy());
TELE.addData("ty", result.getTy()); // }
} // }
} //
// } else {
} else { // camTicker = 0;
camTicker = 0; // overrideTurr = false;
overrideTurr = false; // }
}
if (manualTurret) {
pos = turrDefault + (manualOffset / 100);
}
if (!overrideTurr) { if (!overrideTurr) {
turretPos = pos; turretPos = pos;
} }
if (gamepad2.dpad_right) { if (gamepad2.dpadRightWasPressed()) {
manualOffset -= 2;
} else if (gamepad2.dpad_left) {
manualOffset += 2; manualOffset += 2;
} else if (gamepad2.dpadLeftWasPressed()) {
manualOffset -= 2;
} }
TELE.addData("posS3", turretPos);
// PID SERVOS
double turrettopos = (double) ((double) robot.turr1Pos.getCurrentPosition() / 1024.0) * ((double) 44.0 / (double) 77.0);
TELE.addData("currentTpos", turrettopos);
double tPid;
if (!fixedTurret) {
tPid = tController.calculate(turrettopos, turretPos);
} else {
tPid = tController.calculate(turrettopos, (manualOffset/90));
}
TELE.addData("tPID", tPid);
robot.turr1.setPower(tPid);
robot.turr2.setPower(-tPid);
//HOOD: //HOOD:
if (autoHood) { if (autoHood) {
@@ -492,17 +514,30 @@ public class TeleopV3 extends LinearOpMode {
} }
// //
// //TODO: FIX THIS GOOFY THING BECAUSE IT SUCKS @KeshavAnandCode
// if (gamepad2.left_stick_x > 0.5) { if (gamepad2.cross) {
// manualTurret = false; manualOffset = 0;
// } else if (gamepad2.left_stick_x < -0.5) { fixedTurret = true;
// manualOffset = 0; }
// manualTurret = false;
// if (gamepad2.left_bumper) { if (gamepad2.squareWasPressed()) {
// drive = new MecanumDrive(hardwareMap, new Pose2d(2, 0, 0)); drive = new MecanumDrive(hardwareMap, new Pose2d(20, 0, 0));
// sleep(1200); sleep(1500);
// } }
// }
if (gamepad2.triangle) {
autoHood = false;
hoodOffset = 0;
}
if (gamepad2.circleWasPressed()) {
xOffset = robotX;
yOffset = robotY;
headingOffset = robotHeading;
autoHood = true;
fixedTurret = false;
}
// //
// if (gamepad2.left_stick_y < -0.5) { // if (gamepad2.left_stick_y < -0.5) {
// autoHood = true; // autoHood = true;
@@ -531,6 +566,8 @@ public class TeleopV3 extends LinearOpMode {
shootStamp = getRuntime(); shootStamp = getRuntime();
shootAll = true; shootAll = true;
spindexPos = spindexer_intakePos1; spindexPos = spindexer_intakePos1;
shooterTicker = 0;
} }
if (shootAll) { if (shootAll) {
@@ -540,9 +577,11 @@ public class TeleopV3 extends LinearOpMode {
intake = false; intake = false;
reject = false; reject = false;
shooterTicker++;
spindexPos = spindexer_intakePos1; spindexPos = spindexer_intakePos1;
if (getRuntime() - shootStamp < 2.5) { if (getRuntime() - shootStamp < 3.5) {
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -9,7 +9,7 @@ public class Servos {
//PID constants //PID constants
// TODO: get PIDF constants // TODO: get PIDF constants
public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02; public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0.01; public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0;
public static double spin_scalar = 1.0086; public static double spin_scalar = 1.0086;
public static double spin_restPos = 0.0; public static double spin_restPos = 0.0;
public static double turret_scalar = 1.009; public static double turret_scalar = 1.009;
@@ -44,7 +44,8 @@ public class Servos {
} }
public double getTurrPos() { public double getTurrPos() {
return robot.turr1Pos.getCurrentPosition(); return (double) ((double) robot.turr1Pos.getCurrentPosition() / 1024.0) * ((double) 44.0 / (double) 77.0);
} }
public double setTurrPos(double pos) { public double setTurrPos(double pos) {