ripark2/FTC-Code
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added redtele, minor tweaks on TurretLock

Browse Source
This commit is contained in:
sanjeevgedela
2026-02-14 01:22:52 -06:00
parent bf43291fc2
commit b89cca22af
4 changed files with 242 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java
View File

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PE
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.fasterxml.jackson.databind.node.POJONode;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -15,9 +16,10 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
@Config
@TeleOp
@TeleOp (name = "TeleOp BLUE")
public class DriverControlsV1 extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
Limelight3A limelight;
@@ -37,7 +39,8 @@ public class DriverControlsV1 extends LinearOpMode {
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d(0,0,0));
drive.setPoseEstimate(PoseStorage.currentPose);
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
Pose2d driveDirection;
@@ -49,7 +52,7 @@ public class DriverControlsV1 extends LinearOpMode {
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
waitForStart();

198
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java Normal file
View File

@@ -0,0 +1,198 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Config
@TeleOp (name = "TeleOp RED")
public class DriverControlsV1Red extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
Limelight3A limelight;
LLResult result = null;
public static double visionMult = 1.5;
@Override
public void runOpMode() throws InterruptedException {
Hware robot = new Hware();
robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
double SPEEDCONTROL = 1;
Boolean adjust = false;
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d(0,0,0));
Pose2d driveDirection;
ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime();
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
waitForStart();
limelight.pipelineSwitch(1);
limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
while (opModeIsActive()) {
robot.leftPTO.setPosition(0.2);
robot.rightPTO.setPosition(0.2);
double currentVoltage = robot.batteryVoltageSensor.getVoltage();
if (currentVoltage > 13.5) {
pidf = new PIDFCoefficients(3, 0, 0, 13.8
);
} else if (currentVoltage > 12.7) {
pidf = new PIDFCoefficients(3, 0, 0, 14);
} else if (currentVoltage > 11.2) {
pidf = new PIDFCoefficients(3, 0, 0, 14.5 );
} else {
pidf = new PIDFCoefficients(3, 0, 0, 14 );
}
Pose2d myPose = drive.getPoseEstimate();
double turrPos = robot.turret.getCurrentPosition();
result = limelight.getLatestResult();
double vision = result.getTx() * visionMult;
// if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){
// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos)));
// }
double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
// telemetry.update();\
//
double targetTicks = turretLock.update(myPose, result, vision, turrPos);
robot.turret.setTargetPosition((int) targetTicks);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1);
if (gamepad1.right_bumper) {
SPEEDCONTROL= 0.5;
}
if (!shooting) {
if (gamepad1.right_trigger > 0.3) {
robot.intake.set(0.8);
robot.activeTransfer.setPosition(0.7);
} else if (gamepad1.left_trigger > 0.5) {
robot.intake.set(-0.8);
robot.activeTransfer.setPosition(0.7);
} else {
robot.intake.set(0);
robot.activeTransfer.setPosition(0.7);
}
}
driveDirection = new Pose2d(
-gamepad1.left_stick_y * SPEEDCONTROL,
-gamepad1.left_stick_x * SPEEDCONTROL,
-gamepad1.right_stick_x * SPEEDCONTROL
);
// Arm
// 2. Get Targets based on current X, Y
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
double targetVelocity = targets[0];
double targetHood = targets[1];
if (shooting) {
robot.launchHood.setPosition(targetHood);
}
if(gamepad2.dpadUpWasPressed()) {
adjust = true;
} else if (gamepad2.dpadDownWasPressed()){
adjust = false;
}
if (adjust) {
robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity);
} else {
robot.bottomFly.setVelocity(0);
robot.topFly.setVelocity(0);
}
// 1. TRIGGER: Start the sequence
if (gamepad2.xWasPressed()) {
shooting = true;
shootStatus = ShootState.START_SHOOT;
shootTimer.reset();
}
switch (shootStatus) {
case START_SHOOT:
// Initial actions
robot.activeTransfer.setPosition(0.95);
robot.intake.set(0.55);
robot.launchHood.setPosition(targetHood);
// Wait 250ms for the shot to physically fire before moving the hood
if (shootTimer.milliseconds() > 100) {
shootStatus = ShootState.LOWER_HOOD;
}
break;
case LOWER_HOOD:
targetHood = Math.max(0.34, targetHood - 0.2);
targetVelocity = targetVelocity + 100;
robot.launchHood.setPosition(targetHood);
robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity);
shootStatus = ShootState.RESET;
break;
case RESET:
if (shootTimer.milliseconds() > 3000) {
shootStatus = ShootState.IDLE;
shooting = false;
}
break;
}
if (gamepad2.yWasPressed()) {
shooting = false;
robot.activeTransfer.setPosition(.7);
}
drive.setWeightedDrivePower(driveDirection);
drive.update();
telemetry.addData("Vision", vision);
telemetry.addData("Voltage Sensor", currentVoltage);
telemetry.update();
}
}
}

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

@@ -70,7 +70,7 @@ public class TeleOpTurretLock extends LinearOpMode {
// Start pose (you said 0,0)
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
// limelight

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java
View File

@@ -50,6 +50,11 @@ public class TurretLock {
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
public static double cameraBearingEqual = 0.5; // Deadband
public enum SIDE {
RED,
BLUE
}
// TODO: tune these values for limelight
Hware robot;
Limelight3A webcam;
@@ -78,10 +83,19 @@ public class TurretLock {
// private final ElapsedTime tagTimer = new ElapsedTime();
public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele) {
public TurretLock(DcMotorEx turretMotor, Limelight3A cam, SIDE side) {
this.turret = turretMotor;
this.webcam = cam;
this.tele = tele;
switch (side) {
case RED:
goalX = -75;
goalY = -75;
break;
case BLUE:
goalX = 75;
goalY = 75;
break;
}
}
public double getTurrPos() {
@@ -95,6 +109,7 @@ public class TurretLock {
public void resetManualTrim() {
manualTrimDeg = 0;
}
private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
if (result != null) {
if (result.isValid()) {
@@ -167,7 +182,9 @@ public class TurretLock {
}
/** Main update: call every loop. */
/**
* Main update: call every loop.
*/
public double update(Pose2d robotPose, LLResult result, double visionCorrect, double turrPos) {
// 1) ODOM: compute turret angle needed to face goal (in degrees)
double dx = goalX - robotPose.getX();
@@ -241,7 +258,9 @@ public class TurretLock {
return finalDeg;
}
/** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
/**
* Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag.
*/
private static double normalizeDeg(double deg) {
while (deg > 180) deg -= 360;