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

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

@@ -19,7 +19,7 @@ import java.util.List;
public class TurretLock {
public enum MODE{
public enum MODE {
LIME,
ODOMETRY
}
@@ -40,7 +40,7 @@ public class TurretLock {
// Mechanical limits (ticks). Keep your existing numbers.
public int minTicks = (int) (ticksPerDegree * (-90));
public int maxTicks = (int) (ticksPerDegree * ( 90 ));
public int maxTicks = (int) (ticksPerDegree * (90));
public double finalDeg = 0;
@@ -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;
@@ -76,25 +81,35 @@ public class TurretLock {
Telemetry tele;
// private final ElapsedTime tagTimer = new ElapsedTime();
// 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() {
return turret.getCurrentPosition();
}
public void addManualTrimDeg(double deltaDeg){
public void addManualTrimDeg(double deltaDeg) {
manualTrimDeg += deltaDeg;
}
public void resetManualTrim(){
public void resetManualTrim() {
manualTrimDeg = 0;
}
private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
if (result != null) {
if (result.isValid()) {
@@ -112,7 +127,7 @@ public class TurretLock {
}
public double getBearing() {
// tx = 1000;
// tx = 1000;
LLResult result = webcam.getLatestResult();
return result.getBotpose().getOrientation().getYaw();
}
@@ -151,23 +166,25 @@ public class TurretLock {
private double bearingAlign(LLResult llResult) {
double targetTx = 0;
if(llResult != null && llResult.isValid()){
if (llResult != null && llResult.isValid()) {
targetTx = llResult.getTx(); // How far left or right the target is (degrees)
}
return llResult.getBotpose().getOrientation().getYaw();
}
public void Init(){
public void Init() {
pid = new PersonalPID(p, i, d, f);
}
public String getStatus(){
public String getStatus() {
return String.valueOf(webcam.getStatus());
}
/** 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();
@@ -191,7 +208,7 @@ public class TurretLock {
finalDeg = manualTrimDeg + turretCmdDeg;
limelightRead(result);
filteredVisionTrimDeg = -visionCorrect;
switch (TYPE){
switch (TYPE) {
case LIME:
finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg;
if (result != null && (finalDeg > 20 || finalDeg < -20)) {
@@ -221,13 +238,13 @@ public class TurretLock {
// turret.setTargetPosition(targetTicks);
// turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
// turret.setPower(1.0);
}
}
public double updateHeading(Pose2d robotPose, LLResult result, double turrPos) {
double robotHeadingDeg = Math.toDegrees(robotPose.getHeading());
if(result.getTx() < 1 || result.getTy() > -1){
robotHeadingDeg = - (turrPos / ticksPerDegree - 45);
if (result.getTx() < 1 || result.getTy() > -1) {
robotHeadingDeg = -(turrPos / ticksPerDegree - 45);
}
return robotHeadingDeg;
@@ -237,11 +254,13 @@ public class TurretLock {
return TYPE;
}
public double getFinalDeg(){
public double getFinalDeg() {
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;