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

ok

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-30 19:20:44 -06:00
parent 03ae41c19b
commit 8686b79314
10 changed files with 74 additions and 325 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View File

@@ -1,89 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.Objects;
public class Drivetrain {
private final GamepadEx gamepad;
private final DcMotorEx fl;
private final DcMotorEx fr;
private final DcMotorEx bl;
private final DcMotorEx br;
public MultipleTelemetry TELE;
private String Mode = "Default";
private double defaultSpeed = 0.7;
private double slowSpeed = 0.3;
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) {
this.fl = robot.frontLeft;
this.fr = robot.frontRight;
this.br = robot.backRight;
this.bl = robot.backLeft;
this.gamepad = gamepad1;
this.TELE = tele;
}
public void setMode(String mode) {
this.Mode = mode;
}
public void setDefaultSpeed(double speed) {
this.defaultSpeed = speed;
}
public void setSlowSpeed(double speed) {
this.slowSpeed = speed;
}
public void RobotCentric(double fwd, double strafe, double turn, double turbo) {
double y = -fwd; // Remember, Y stick value is reversed
double x = strafe * 1.1; // Counteract imperfect strafing
double rx = turn;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
fl.setPower(frontLeftPower * turbo);
bl.setPower(backLeftPower * turbo);
fr.setPower(frontRightPower * turbo);
br.setPower(backRightPower * turbo);
}
public void update() {
if (Objects.equals(Mode, "Default")) {
RobotCentric(
gamepad.getRightY(),
gamepad.getRightX(),
gamepad.getLeftX(),
(gamepad.getTrigger(
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed)
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
+ defaultSpeed
)
);
}
}
}

162
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -1,162 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
public class Shooter {
// ================================================================
// ------------------- DASHBOARD CONSTANTS -----------------------
// ================================================================
public static int mode = 0; // 0 = manual, 1 = velocity PID, 2 = autoTrack
public static double parameter = 0.0; // manual: power, vel: target RPM, auto: target RPM
public static double MAX_RPM = 2500;
public static double kP = 0.01;
public static double maxStep = 0.2;
public static double transferPower = 0.0;
public static double hoodPos = 0.501;
// ================================================================
// ------------- AUTO TRACK TUNING CONSTANTS -------------------
// ================================================================
// z offset between shooter and goal
public static double dz = 30; // inches
// Quadratic fit for shooter velocity vs distance
// v = A*d^2 + B*d + C
public static double A = 0.0004;
public static double B = 0.9;
public static double C = 1200;
// Hood angle trig model
// hood = HOOD_A * atan(d * HOOD_B) + HOOD_C
public static double HOOD_A = 0.42;
public static double HOOD_B = 0.012;
public static double HOOD_C = 0.22;
// ================================================================
// ---------------------- INTERNAL STATE ------------------------
// ================================================================
private DcMotorEx leftShooter, rightShooter, encoder;
private double lastRevolutions = 0.0;
private double lastTime = 0.0;
private MultipleTelemetry TELE;
private Robot robot;
// ================================================================
// --------------------------- INIT -------------------------------
// ================================================================
public void init(Robot robot, MultipleTelemetry TELE) {
this.robot = robot;
this.TELE = TELE;
leftShooter = robot.shooter1;
rightShooter = robot.shooter2;
encoder = robot.shooter1;
lastTime = 0.0;
lastRevolutions = 0.0;
}
// ================================================================
// -------------------------- UPDATE ------------------------------
// ================================================================
public void update(double runtimeSec) {
double kF = 1.0 / MAX_RPM;
double rev = encoder.getCurrentPosition() / 2048.0;
double velocity = -60 * (rev - lastRevolutions) / (runtimeSec - lastTime);
TELE.addLine("Mode: 0=Manual, 1=Vel, 2=AutoTrack");
TELE.addData("Parameter", parameter);
TELE.addData("Velocity", velocity);
if (mode == 0) {
// Manual
leftShooter.setPower(parameter);
rightShooter.setPower(parameter);
}
else if (mode == 1 || mode == 2) {
// Velocity PID (shared logic)
double feed = kF * parameter;
double error = parameter - velocity;
double correction = kP * error;
correction = Math.max(-maxStep, Math.min(maxStep, correction));
double finalPower = Math.max(0, Math.min(1, feed + correction));
leftShooter.setPower(finalPower);
rightShooter.setPower(finalPower);
}
robot.hood.setPosition(hoodPos);
robot.transfer.setPower(transferPower);
lastTime = runtimeSec;
lastRevolutions = rev;
TELE.update();
}
// ================================================================
// ------------------------ AUTO TRACK ----------------------------
// ================================================================
public void autoTrack(Pose2d robotPose, Pose2d goalPose) {
mode = 2; // Auto tracking → velocity PID
// Compute 3D distance
double dx = goalPose.position.x - robotPose.position.x;
double dy = goalPose.position.y - robotPose.position.y;
double distance = Math.sqrt(dx*dx + dy*dy + dz*dz);
// ---- Velocity Fit ----
double targetVelocity = A * distance * distance + B * distance + C;
parameter = targetVelocity;
// ---- Hood Fit ----
hoodPos = HOOD_A * Math.atan(distance * HOOD_B) + HOOD_C;
// Telemetry
TELE.addLine("AUTO TRACK ACTIVE");
TELE.addData("Distance", distance);
TELE.addData("Target Velocity", targetVelocity);
TELE.addData("Hood", hoodPos);
}
// ================================================================
// --------------------- USER CALL METHODS ------------------------
// ================================================================
public void setManualPower(double p) {
mode = 0;
parameter = p;
}
public void setVelocity(double rpm) {
mode = 1;
parameter = rpm;
}
public void setHood(double pos) { hoodPos = pos; }
public void setTransfer(double p) { transferPower = p; }
}

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

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.teleop;
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.tests.ShooterTest.*;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -9,6 +11,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
@@ -16,16 +19,13 @@ import java.util.List;
public class TeleopV2 extends LinearOpMode {
public static double vel = 3000;
public static double hood = 0.5;
Robot robot;
MultipleTelemetry TELE;
boolean intake = false;
boolean reject = false;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
private double vel = 3000;
int ticker = 0;
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
List<Double> s1G = new ArrayList<>();
List<Double> s2G = new ArrayList<>();
@@ -33,6 +33,15 @@ public class TeleopV2 extends LinearOpMode {
List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
private double velo1, velo;
private double stamp1, stamp, initPos;
private boolean shootAll = false;
double desiredTurretAngle = 180;
MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart);
@Override
public void runOpMode() throws InterruptedException {
@@ -50,7 +59,7 @@ public class TeleopV2 extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()) {
//DRIVETRAIN:
//DRIVETRAIN:
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
@@ -67,7 +76,7 @@ public class TeleopV2 extends LinearOpMode {
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
//INTAKE:
//INTAKE:
if (gamepad1.rightBumperWasPressed()) {
intake = true;
@@ -96,7 +105,7 @@ public class TeleopV2 extends LinearOpMode {
robot.intake.setPower(0);
}
//COLOR:
//COLOR:
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -151,38 +160,73 @@ public class TeleopV2 extends LinearOpMode {
boolean green2 = s2.get(s2.size() - 1);
boolean green3 = s3.get(s3.size() - 1);
//SHOOTER:
//SHOOTER:
double kF = 1.0 / MAX_RPM; // baseline feedforward
double penguin = 0;
double encoderRevolutions = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
if (ticker % 8 == 0) {
penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
double stamp = getRuntime();
velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
initPos = penguin;
stamp1 = stamp;
}
double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
velo = velo1;
double velPID;
double feed = vel / 4500;
// --- FEEDFORWARD BASE POWER ---
double feed = kF * vel; // Example: vel=2500 → feed=0.5
if (vel > 500) {
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
}
// --- PROPORTIONAL CORRECTION ---
double error = vel - velocity;
double error = vel - velo1;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
velPID = feed + correction;
double powPID = feed + correction;
// clamp to allowed range
velPID = Math.max(0, Math.min(1, velPID));
powPID = Math.max(0, Math.min(1, powPID));
robot.shooter1.setPower(velPID);
robot.shooter2.setPower(velPID);
if (vel - velo > 1000) {
powPID = 1;
} else if (velo - vel > 1000) {
powPID = 0;
}
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.hood.setPosition(hood);
//TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
//MISC:
//TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
//SHOOT ALL:
if (gamepad2.rightBumperWasPressed()){
shootAll = true;
}
if (shootAll){
intake = false;
reject = false;
}
//MISC:
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
@@ -192,8 +236,13 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("Spin2Green", s2.get(s2.size() - 1));
TELE.addData("Spin3Green", s3.get(s3.size() - 1));
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.update();
ticker++;
}
}
}

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt
View File

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt
View File

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

@@ -6,7 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt
View File

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java
View File

@@ -1,46 +0,0 @@
package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class TrackingTest extends LinearOpMode {
Robot robot;
Drivetrain drivetrain;
MultipleTelemetry TELE;
GamepadEx g1;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
g1 = new GamepadEx(gamepad1);
drivetrain = new Drivetrain(robot, TELE, g1);
waitForStart();
if(isStopRequested()) return;
while (opModeIsActive()){
drivetrain.update();
}
}
}

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt
View File

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java
View File

@@ -1,14 +1,11 @@
package org.firstinspires.ftc.teamcode.subsystems;
package org.firstinspires.ftc.teamcode.utils;
import android.util.Size;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;