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

teleop is back up and running

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-18 20:36:48 -05:00
parent 6b092bdaeb
commit 2a29e8181b
8 changed files with 149 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -65,4 +65,7 @@ public class Front_Poses {
public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
//For PedroPathing TODO: figure out how to change start poses in auto
public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0;
}

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

@@ -5,21 +5,21 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos1 = 0.26; //0.13;
public static double spindexer_intakePos1 = 0.18; //0.13;
public static double spindexer_intakePos2 = 0.45; //0.33;//0.5;
public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.63; //0.53;//0.66;
public static double spindexer_intakePos3 = 0.55; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.91; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.35; //0.65; //0.24;
public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.73; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.54; //0.27; //0.4;
public static double spinStartPos = 0.05;
public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4;
public static double spinStartPos = 0;
public static double spinEndPos = 0.6;
public static double shootAllSpindexerSpeedIncrease = 0.0095;
public static double shootAllSpindexerSpeedIncrease = 0.01;
public static double transferServo_out = 0.15;

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

@@ -148,7 +148,7 @@ class LocalizationTest extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(0,0));
follower.setStartingPose(new Pose(72,72, 0));
}
/** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */

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

@@ -2,6 +2,9 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
@@ -12,6 +15,7 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -21,6 +25,7 @@ import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Light;
@@ -38,10 +43,11 @@ import java.util.List;
public class TeleopV3 extends LinearOpMode {
public static double manualVel = 3000;
public static double hoodDefaultPos = 0.5;
private double predictedResetX, predictedResetY, predictedResetH;
public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0;
public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180;
public static double spinPow = 0.09;
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 0;
public static double hoodSpeedOffset = 0.01;
public static double turretSpeedOffset = 0.01;
@@ -56,12 +62,13 @@ public class TeleopV3 extends LinearOpMode {
Light light;
Servos servo;
Flywheel flywheel;
MecanumDrive drive;
// MecanumDrive drive;
Spindexer spindexer;
Targeting targeting;
Targeting.Settings targetingSettings;
Drivetrain drivetrain;
MeasuringLoopTimes loopTimes;
Follower follower;
double autoHoodOffset = 0.0;
int shooterTicker = 0;
boolean intake = false;
@@ -84,7 +91,6 @@ public class TeleopV3 extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
robot.light.setPosition(0);
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
@@ -94,12 +100,15 @@ public class TeleopV3 extends LinearOpMode {
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
flywheel = new Flywheel(hardwareMap);
drive = new MecanumDrive(hardwareMap, teleStart);
// drive = new MecanumDrive(hardwareMap, teleStart);
follower = Constants.createFollower(hardwareMap);
Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
follower.setStartingPose(start);
spindexer = new Spindexer(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
drivetrain = new Drivetrain(robot, drive);
drivetrain = new Drivetrain(robot, follower);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
@@ -118,21 +127,33 @@ public class TeleopV3 extends LinearOpMode {
Spindexer.teleop = true;
while (opModeInInit()) {
//ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS
if (gamepad1.crossWasPressed()){
redAlliance = !redAlliance;
}
robot.limelight.start();
if (redAlliance) {
turret.pipelineSwitch(4);
light.setManualLightColor(Color.LightRed);
predictedResetX = redPredictedResetX;
predictedResetY = redPredictedResetY;
predictedResetH = Math.toRadians(redPredictedResetH);
} else {
turret.pipelineSwitch(2);
light.setManualLightColor(Color.LightBlue);
predictedResetX = bluePredictedResetX;
predictedResetY = bluePredictedResetY;
predictedResetH = Math.toRadians(bluePredictedResetH);
}
robot.light.setPosition(1);
limelightUsed = true;
TELE.addData("Red Alliance?", redAlliance);
TELE.update();
light.update();
}
limelightUsed = true;
waitForStart();
if (isStopRequested()) return;
@@ -143,8 +164,8 @@ public class TeleopV3 extends LinearOpMode {
while (opModeIsActive()) {
//TELE.addData("Is limelight on?", robot.limelight.getStatus());
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
follower.update();
Pose currentPose = follower.getPose();
if (enableSpindexerManager) {
//if (!shootAll) {
@@ -226,20 +247,21 @@ public class TeleopV3 extends LinearOpMode {
//TURRET TRACKING
double robX = currentPose.position.x;
double robY = currentPose.position.y;
double robH = currentPose.heading.toDouble();
double robX = currentPose.getX();
double robY = currentPose.getY();
double robH = currentPose.getHeading();
double robotX = robX + xOffset;
double robotY = robY + yOffset;
double robotHeading = robH + hOffset;
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose deltaPose = new Pose(dx, dy, robotHeading);
// double goalX = -15;
// double goalY = 0;
//
// double dx = robotX - goalX; // delta x from robot to goal
// double dy = robotY - goalY; // delta y from robot to goal
// Pose deltaPose = new Pose(dx, dy, robotHeading);
Pose deltaPose = new Pose(robotX, robotY, robotHeading);
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
@@ -253,14 +275,15 @@ public class TeleopV3 extends LinearOpMode {
gamepad2.rumble(500);
}
if (relocalize){
turret.relocalize();
xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
yOffset = (turret.getLimelightX() * 39.3701) - robY;
hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
} else {
//TODO: relocalize using limelight
// if (relocalize){
// turret.relocalize();
// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
// yOffset = (turret.getLimelightX() * 39.3701) - robY;
// hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
// } else {
turret.trackGoal(deltaPose);
}
//}
//VELOCITY AUTOMATIC
if (autoVel) {
@@ -324,7 +347,8 @@ public class TeleopV3 extends LinearOpMode {
}
if (gamepad2.crossWasPressed()) {
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH));
gamepad2.rumble(200);
sleep(500);
}
@@ -381,9 +405,9 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
TELE.update();

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import com.acmerobotics.dashboard.FtcDashboard;

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java
View File

@@ -1,33 +1,31 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
public class Drivetrain {
Robot robot;
boolean autoDrive = false;
Pose2d brakePos = new Pose2d(0, 0, 0);
Pose brakePos = new Pose(0, 0, 0);
MecanumDrive drive;
// MecanumDrive drive;
Follower follower;
private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
public Drivetrain (Robot rob, MecanumDrive mecanumDrive){
public Drivetrain (Robot rob, Follower follower){
this.robot = rob;
this.drive = mecanumDrive;
this.follower = follower;
}
@@ -71,7 +69,7 @@ public class Drivetrain {
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
}
Pose2d currentPos = drive.localizer.getPose();
Pose currentPos = follower.getPose();
brakePos = currentPos;
if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
@@ -84,13 +82,13 @@ public class Drivetrain {
} else if (brake > 0.4) {
TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
.strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);
PathChain traj2 = follower.pathBuilder()
.addPath(new BezierLine(currentPos, brakePos))
.setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading())
.build();
if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) {
Actions.runBlocking(
traj2.build()
);
if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) {
follower.followPath(traj2);
}
} else {
autoDrive = false;

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -3,7 +3,9 @@ package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import java.lang.Math;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
public class Targeting {
@@ -71,7 +73,7 @@ public class Targeting {
public final int TILE_UPPER_QUARTILE = 18;
public final int TILE_LOWER_QUARTILE = 6;
public double robotInchesX, robotInchesY = 0.0;
public int robotGridX, robotGridY = 0;
public int robotGridX = 0, robotGridY = 0;
MultipleTelemetry TELE;
double cancelOffsetX = 0.0; // was -40.0
double cancelOffsetY = 0.0; // was 7.0
@@ -89,45 +91,59 @@ public class Targeting {
Settings recommendedSettings = new Settings(0.0, 0.0);
int gridX;
int gridY;
if (!redAlliance){
sin54 = Math.sin(Math.toRadians(54));
double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
// Convert robot coordinates to inches
robotInchesX = rotatedX * unitConversionFactor + 20;
robotInchesY = rotatedY * unitConversionFactor + 20;
// Find approximate location in the grid
gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
} else {
sin54 = Math.sin(Math.toRadians(-54));
double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
// Convert robot coordinates to inches
robotInchesX = rotatedX * unitConversionFactor;
robotInchesY = rotatedY * unitConversionFactor;
// Find approximate location in the grid
gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
}
int remX = Math.floorMod((int) robotInchesX, tileSize);
int remY = Math.floorMod((int) robotInchesY, tileSize);
//clamp
int remX = 0;
int remY = 0;
// Old code
// if (!redAlliance){
// sin54 = Math.sin(Math.toRadians(54));
// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
//
// // Convert robot coordinates to inches
// robotInchesX = rotatedX * unitConversionFactor + 20;
// robotInchesY = rotatedY * unitConversionFactor + 20;
//
// // Find approximate location in the grid
// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
// } else {
// sin54 = Math.sin(Math.toRadians(-54));
// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
//
// // Convert robot coordinates to inches
// robotInchesX = rotatedX * unitConversionFactor;
// robotInchesY = rotatedY * unitConversionFactor;
//
// // Find approximate location in the grid
// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
// }
//
//
remX = Math.floorMod((int) robotX, tileSize);
remY = Math.floorMod((int) robotY, tileSize);
//
// //clamp
//
// if (redAlliance) {
// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// } else {
// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// }
// New code
if (redAlliance){
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
gridY = Math.round((float) (((144-robotX)-12) / 24));
} else {
gridY = Math.round((float) ((robotX-12) / 24));
}
gridX = Math.round((float) (((144-robotY)-12) / 24));
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
}
// Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile.

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -25,7 +25,7 @@ public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.54;
public static double turret180Range = 0.55;
public static double turrDefault = 0.35;
public static double turrMin = 0;
public static double turrMax = 0.69;
@@ -262,7 +262,12 @@ public class Turret {
double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45;
// Robot heading (field → robot)
double robotHeadingDeg = posH + 135;
double robotHeadingDeg;
if (Color.redAlliance){
robotHeadingDeg = posH + 135;
} else {
robotHeadingDeg = posH + 45;
}
// Turret angle needed relative to robot
double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
@@ -362,16 +367,16 @@ public class Turret {
/* ---------------- TELEMETRY ---------------- */
TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
TELE.addData("Target Pos", "%.3f", targetTurretPos);
TELE.addData("Current Localization Pos", "%.3f", deltaPos);
TELE.addData("Commanded Pos", "%.3f", turretPos);
// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
// TELE.addData("Target Pos", "%.3f", targetTurretPos);
// TELE.addData("Current Localization Pos", deltaPos);
// TELE.addData("Commanded Pos", "%.3f", turretPos);
// TELE.addData("LL Valid", result.isValid());
// TELE.addData("LL getTx", result.getTx());
// TELE.addData("LL Offset", offset);
// TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
// TELE.addData("Learned Offset", "%.2f", offset);
TELE.update();
// TELE.update();
}
}