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

2/11 changes

Browse Source
This commit is contained in:
Plano East Robotics
2026-02-13 13:18:32 -06:00
parent 510a0495e3
commit bf43291fc2
4 changed files with 29 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
View File

@@ -116,8 +116,8 @@ public class blueDepot extends LinearOpMode {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-13,65, Math.toRadians(55)))
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-16,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
@@ -143,7 +143,7 @@ public class blueDepot extends LinearOpMode {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
@@ -155,7 +155,7 @@ public class blueDepot extends LinearOpMode {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();

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

@@ -61,6 +61,9 @@ public class DriverControlsV1 extends LinearOpMode {
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
@@ -74,16 +77,19 @@ public class DriverControlsV1 extends LinearOpMode {
}
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);
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);
@@ -159,8 +165,11 @@ public class DriverControlsV1 extends LinearOpMode {
break;
case LOWER_HOOD:
targetHood = Math.max(0.45, targetHood - 0.2);
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;

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

@@ -127,7 +127,7 @@ public class TeleOpTurretLock extends LinearOpMode {
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
// telemetry.update();\
//
double targetTicks = turretLock.update(pose, result, vision);
double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
robot.turret.setTargetPosition((int) targetTicks);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1);

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

@@ -168,7 +168,7 @@ public class TurretLock {
/** Main update: call every loop. */
public double update(Pose2d robotPose, LLResult result, double visionCorrect) {
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();
double dy = goalY - robotPose.getY();
@@ -193,7 +193,7 @@ public class TurretLock {
filteredVisionTrimDeg = -visionCorrect;
switch (TYPE){
case LIME:
finalDeg = manualTrimDeg + filteredVisionTrimDeg + turretCmdDeg;
finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg;
if (result != null && (finalDeg > 20 || finalDeg < -20)) {
TYPE = MODE.ODOMETRY;
}
@@ -223,6 +223,16 @@ public class TurretLock {
// 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);
}
return robotHeadingDeg;
}
public MODE getTYPE() {
return TYPE;
}