Arkm/Ftc-12926
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto Collection + others

Browse Source
This commit is contained in:
Arkm20
2026-03-08 11:03:36 -05:00
parent 2f04fc0aea
commit c90732ff53
13 changed files with 572 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
FtcRobotController/build.gradle
View File

@@ -3,6 +3,7 @@ import java.text.SimpleDateFormat
//
// build.gradle in FtcRobotController
//
apply plugin: 'com.android.library'
android {

26
TeamCode/build.gradle
View File

@@ -10,34 +10,10 @@
// Custom definitions may go here
repositories {
// Dairy releases repository
maven {
url = "https://repo.dairy.foundation/releases"
}
// Dairy snapshots repository
maven {
url = "https://repo.dairy.foundation/snapshots"
}
}
buildscript {
repositories {
mavenCentral()
maven {
url "https://repo.dairy.foundation/releases"
}
}
dependencies {
classpath "dev.frozenmilk:Load:0.2.4"
}
}
// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
apply plugin: 'dev.frozenmilk.sinister.sloth.load'
android {
namespace = 'org.firstinspires.ftc.teamcode'
@@ -49,6 +25,4 @@ android {
dependencies {
implementation project(':FtcRobotController')
implementation("dev.frozenmilk.sinister:Sloth:0.2.4")
implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12")
}

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

@@ -172,7 +172,7 @@ public class hwMap {
public NormalizedColorSensor[] sensors;
public TransferHwMap(HardwareMap hardwareMap) {
Limiter = hardwareMap.servo.get("limiter");
Limiter = hardwareMap.servo.get(Constants.TransferConstants.LIMITER_SERVO);
indexA = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_A);
@@ -259,8 +259,8 @@ public class hwMap {
turretLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
turretRightMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
turretLeftMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_L_DIRECTION);
turretRightMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_R_DIRECTION);
turretLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD);
turretRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
turretRightMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
turretLeftMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
@@ -271,11 +271,12 @@ public class hwMap {
// --- Turret Rotation Motor ---
turretrotation = hardwareMap.dcMotor.get(Constants.TurretConstants.TURRET_ROTATION_MOTOR);
turretrotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
turretrotation.setDirection(Constants.TurretConstants.TURRET_ROTATION_DIR);
turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// --- Hood Servo ---
hoodservo = hardwareMap.servo.get(Constants.TurretConstants.HOOD_TURRET_SERVO);
hoodservo.setDirection(Servo.Direction.FORWARD);
hoodservo.setDirection(Servo.Direction.REVERSE);
// --- External Encoder (if used separately) ---

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
View File

@@ -18,10 +18,16 @@ import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
public class Constants {
public static FollowerConstants followerConstants = new FollowerConstants()
.mass(12);
.mass(12)
.translationalPIDFCoefficients(new PIDFCoefficients(0.26, 0, 0.01, 0.03))
.headingPIDFCoefficients(new PIDFCoefficients(1.8, 0, 0.01, 0.03))
.forwardZeroPowerAcceleration(-39.64668514000648)
.lateralZeroPowerAcceleration(-76.57271150137258);
public static MecanumConstants driveConstants = new MecanumConstants()
.maxPower(0.8)
.xVelocity(68.10320673181904)
.yVelocity(57.52038399624941)
.rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
.rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
.leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
@@ -37,13 +43,13 @@ public class Constants {
public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
.forwardTicksToInches(0.001978956)
.strafeTicksToInches(0.001978956)
.turnTicksToInches(0.001978956)
.turnTicksToInches(0.0022824233082645137)
.leftPodY(3.75)
.rightPodY(-3.25)
.strafePodX(-6.25)
.leftEncoder_HardwareMapName("intake")
.rightEncoder_HardwareMapName("fl")
.strafeEncoder_HardwareMapName("fr")
.strafeEncoder_HardwareMapName("bl")
.leftEncoderDirection(Encoder.FORWARD)
.rightEncoderDirection(Encoder.FORWARD)
.strafeEncoderDirection(Encoder.FORWARD);

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

@@ -292,7 +292,7 @@ public class Turret {
}
private double getHoodPOS(double dist) {
return ((-(3.62429 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000638975 * Math.pow(dist, 3) - (0.000153252 * Math.pow(dist, 2)) + (-0.0197027 * dist) + 1.40511);
return (((1.2113 * Math.pow(10, -8)) * Math.pow(dist, 4)) - (0.00000498646 * Math.pow(dist, 3)) + (0.00068726 * Math.pow(dist, 2)) + (-0.0405783 * dist) + 1.07118);
}
public void setHoodPos(double pos) {

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
View File

@@ -31,11 +31,6 @@ public class Constants {
public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
public static final double TICKS_PER_REVOLUTION = 537.6;
// Drive characteristics
public static final double MAX_VELOCITY = 30.0; // inches per second
public static final double MAX_ANGULAR_VELOCITY = Math.toRadians(180.0); // radians per second
public static final double MAX_ACCELERATION = 30.0; // inches per second squared
public static final double STOP_SPEED_MULTIPLIER = 0.0;
public static final double PRECISION_SPEED_MULTIPLIER = 0.3;
public static final double NORMAL_SPEED_MULTIPLIER = 0.6;
@@ -45,7 +40,7 @@ public class Constants {
public static class IntakeConstants {
public static final String FRONT_INTAKE_MOTOR = "intake";
public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.REVERSE;
public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD;
public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE;
public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER;
@@ -74,7 +69,7 @@ public class Constants {
}
public static class TransferConstants {
public static final String LIMITER_SERVO = "activeTransfer";
public static final String LIMITER_SERVO = "limiter";
public static final Servo.Direction LIMITER_SERVO_DIR = Servo.Direction.FORWARD;
@@ -83,20 +78,20 @@ public class Constants {
public static final String INDEX_SENSOR_B = "colorB";
public static final String INDEX_SENSOR_C = "colorC";
public static final double LIMIT_POS = 0.99;
public static final double LIMIT_POS = 1;
public static final double UNLOCK_POS = 0.81;
public static final double UNLOCK_POS = 0.82;
}
public static class TurretConstants {
public static final double TICKS_PER_REV_MOTOR = 384.5;
public static final double EXTERNAL_GEAR_RATIO = 1.315994798439532;
public static final double EXTERNAL_GEAR_RATIO = 2.68888889;
public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
public static final double KP = 0.028;
public static final double KP = 0.02;
public static final double KI = 0.0;
public static final double KD = 0.00055;
public static final double KD = 0.001;
public static final double MAX_POWER = 0.6;
public static final double MAX_INTEGRAL = 0.5;
@@ -107,12 +102,14 @@ public class Constants {
public static final double RED_TARGET_OFFSET_DEGREES = 14;
public static final double BLUE_TARGET_OFFSET_DEGREES = 17;
public static final double LL_TARGET_OFFSET_DEGREES = -6;
public static final double LL_LOGIC_MULTIPLIER = -1.0;
public static final double OFFSET_SMOOTHING = 0.3;
public static final double LL_TARGET_OFFSET_DEGREES = -2;
public static final double LL_LOGIC_MULTIPLIER = 1.0;
public static final double OFFSET_SMOOTHING = 0.2;
public static final double SOFT_LIMIT_NEG = -100.0;
public static final double SOFT_LIMIT_POS = 250.0;
public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD;
public static final double SOFT_LIMIT_NEG = -230;
public static final double SOFT_LIMIT_POS = 230;
public static final double HOOD_POS_CLOSE = 0.3;
public static final double HOOD_POS_FAR = 0.7;
@@ -130,25 +127,25 @@ public class Constants {
public static final double INTAKE_POWER = -0.04;
public static final String TURRET_ROTATION_MOTOR = "turretRotation";
public static final String HOOD_TURRET_SERVO = "hoodServo";
public static final String HOOD_TURRET_SERVO = "hoodservo";
public static final Servo.Direction HOOD_SERVO_DIR = Servo.Direction.FORWARD;
public static final String TURRET_RIGHT_MOTOR = "rturret";
public static final String TURRET_LEFT_MOTOR = "lturret";
public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.REVERSE;
public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.FORWARD;
public static final DcMotorSimple.Direction TURRET_MOTOR_L_DIRECTION = DcMotorSimple.Direction.REVERSE;
public static final int LIMELIGHT_PIPELINE_MOTIF = 0;
public static final int LIMELIGHT_PIPELINE_BLUE_TARGET = 1;
public static final int LIMELIGHT_PIPELINE_RED_TARGET = 2;
public static final double CAMERA_HEIGHT_INCHES = 15.5;
public static final double CAMERA_HEIGHT_INCHES = 12.4;
public static final double GOAL_HEIGHT_INCHES = 29.5;
public static final double CAMERA_MOUNT_ANGLE_DEGREES = 2.0;
public static final double CAMERA_MOUNT_ANGLE_DEGREES = 7.9;
public static final double SERVO_MAX = 0.7;
public static final double SERVO_MIN = 0.26;
public static final double SERVO_MAX = 0.5;
public static final double SERVO_MIN = 0;
}

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

@@ -7,6 +7,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
@Disabled
@TeleOp(name="Color Sensor Tuning", group="Diagnostics")
public class ColorSensorTuning extends LinearOpMode {

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java
View File

@@ -1,4 +1,70 @@
package org.firstinspires.ftc.teamcode.tuning;
public class PIDFTuning {
import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@Configurable
@TeleOp
public class PIDFTuning extends OpMode {
public DcMotorEx flywheelMotorR;
public DcMotorEx flywheelMotorL;
public static double targetVelocity = 0;
public static double targetVelocity1 = 1500;
public static double targetVelocity2 = 900;
public static double f = 0;
public static double p = 0;
public static double d = 0;
public static boolean reverse = false;
@Override
public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
telemetry.addLine("Init complete");
}
@Override
public void loop() {
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(p, 0, d, f);
flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients);
flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients);
flywheelMotorR.setVelocity(targetVelocity);
flywheelMotorL.setVelocity(targetVelocity);
if (gamepad1.aWasPressed()) {
targetVelocity = targetVelocity1;
} else if (gamepad1.bWasPressed()) {
targetVelocity = targetVelocity2;
}
double curVelocity = flywheelMotorR.getVelocity();
double error = targetVelocity - curVelocity;
telemetry.addData("Target Velocity", targetVelocity);
telemetry.addData("Current Velocity", curVelocity);
telemetry.addData("Error", error);
telemetry.addData("Tuning P", p);
telemetry.addData("Tuning F", f);
telemetry.update();
}
}

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

@@ -9,30 +9,36 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
import org.firstinspires.ftc.teamcode.util.FPSCounter;
@Configurable
@TeleOp(name = "Anchor Mode Tuner", group = "Tuning")
public class anchorTuning extends LinearOpMode {
public static double driveP = -0.001;
public static double driveP = 0.0;
public static double driveI = 0.0;
public static double driveD = -0.00015;
public static double driveD = 0.0;
public static double strafeP = -0.002;
public static double strafeP = 0.26;
public static double strafeI = 0.0;
public static double strafeD = -0.00008;
public static double strafeD = 0.01;
public static double turnP = -0.08;
public static double turnP = 1.8;
public static double turnI = 0.0;
public static double turnD = -0.0001; // Tuned for Degrees
public static double turnD = 0.01; // Tuned for Degrees
public static double MAX_POWER = 0.2;
public static double MAX_POWER = 0.4;
public static int targetFPS = 110;
private DriveTrain driveTrain;
private TelemetryManager telemetryM;
private boolean lastPS = false;
FPSCounter fps = new FPSCounter();
@Override
public void runOpMode() throws InterruptedException {
// Initialize Telemetry
@@ -44,11 +50,14 @@ public class anchorTuning extends LinearOpMode {
telemetryM.debug("Status", "Initialized. Press PS/Guide to Toggle Anchor.");
telemetryM.update(telemetry);
fps.reset();
waitForStart();
while (opModeIsActive()) {
// Update PID values from Dashboard to Subsystem in real-time
fps.startLoop();
driveTrain.updateAnchorPID(
driveP, driveI, driveD,
strafeP, strafeI, strafeD,
@@ -74,8 +83,12 @@ public class anchorTuning extends LinearOpMode {
driveTrain.teleopDrive(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x);
}
sleep(fps.getSyncTime(targetFPS));
telemetryM.debug("Anchor Active", driveTrain.isAnchorActive());
telemetry.addData("FPS", fps.getAverageFps());
telemetryM.debug("Error FWD (Ticks)", driveTrain.getLastForwardError());
telemetryM.debug("Error STR (Ticks)", driveTrain.getLastStrafeError());
telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError());

443
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java Normal file
View File

@@ -0,0 +1,443 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.bylazar.configurables.annotations.Configurable;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
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.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.util.FPSCounter;
import com.qualcomm.hardware.lynx.LynxModule;
import java.util.List;
@Configurable
@TeleOp(name = "Auto-Collect Testing", group = "Turret")
public class autoCollection extends LinearOpMode {
public static double kP = 0.02;
public static double kI = 0.0;
public static double kD = 0.001;
public static double MAX_POWER = 0.6;
public static double MAX_INTEGRAL = 0.5;
public static double TICKS_PER_REV_MOTOR = 384.5;
public static double EXTERNAL_GEAR_RATIO = 2.68888889;
public static boolean MOTOR_REVERSED = true;
public static double LL_LOGIC_MULTIPLIER = 1.0;
public static double SOFT_LIMIT_NEG = -230;
public static double SOFT_LIMIT_POS = 230;
public static double RED_TARGET_OFFSET_DEGREES = 14;
public static double BLUE_TARGET_OFFSET_DEGREES = 17;
public static double LL_TARGET_OFFSET_DEGREES = -2;
public static double GOAL_RED_X = 138;
public static double GOAL_RED_Y = 138;
public static double GOAL_BLUE_X = 6;
public static double GOAL_BLUE_Y = 138;
public static double BLUE_OBSERVATION_ANGLE = 160;
public static double RED_OBSERVATION_ANGLE = -160;
public static double BLUE_RAY_TARGET_X = 4.0;
public static double RED_RAY_TARGET_X = 140.0;
public static double BLUE_DRIVE_TARGET_X = 8.5;
public static double RED_DRIVE_TARGET_X = 135.5;
public static double CLUSTER_RADIUS_DEG = 5.0;
public static double MIN_SNAPSHOT_WAIT_MS = 300;
public static double TURRET_STABLE_ERROR_DEG = 2.0;
public static int PIPELINE_GOAL_BLUE = 1;
public static int PIPELINE_GOAL_RED = 2;
public static int PIPELINE_ARTIFACTS = 3;
public static double OFFSET_SMOOTHING = 0.2;
public static int targetFPS = 110;
private Limelight3A limelight;
private DcMotorEx turretMotor;
private DcMotor intake;
private Follower follower;
private double integralSum = 0;
private double lastError = 0;
private double zeroOffsetTicks = 0;
private double ticksPerDegree = 0;
private double targetCorrectionOffsetTicks = 0;
private double fusedTargetTicks = 0;
private enum AutoCollectState {
IDLE,
INIT_COLLECT,
WAIT_FOR_TURRET_AND_VISION,
CALCULATE_SNAPSHOT,
DRIVING_TO_INTAKE,
PATH_RETURN,
DRIVING_RETURN
}
private AutoCollectState autoCollectState = AutoCollectState.IDLE;
private enum Alliance { RED, BLUE }
private Alliance currentAlliance = Alliance.BLUE;
private Alliance lastAlliance = null;
private Pose returnPose;
private final ElapsedTime sequenceTimer = new ElapsedTime();
private final ElapsedTime timer = new ElapsedTime();
protected FPSCounter fpsCounter = new FPSCounter();
protected List<LynxModule> allHubs;
public static double start_x = 72;
public static double start_y = 8.5;
public static double start_heading = 90;
@Override
public void runOpMode() {
turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
limelight = hardwareMap.get(Limelight3A.class, "limelight");
intake = hardwareMap.get(DcMotor.class, "intake");
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
applyMotorDirection();
recalculateTicksPerDegree();
limelight.start();
telemetry.addData("Status", "Initialized. Auto-Track is Armed.");
telemetry.update();
waitForStart();
timer.reset();
follower.startTeleopDrive();
fpsCounter.reset();
while (opModeIsActive()) {
for (LynxModule hub : allHubs) { hub.clearBulkCache(); }
fpsCounter.startLoop();
follower.update();
double dt = timer.seconds();
timer.reset();
if (dt < 0.001) dt = 0.001;
double rawTicks = turretMotor.getCurrentPosition();
double currentTicks = rawTicks - zeroOffsetTicks;
double currentDegrees = currentTicks / ticksPerDegree;
handleInput();
if (autoCollectState == AutoCollectState.IDLE) {
updateAlliancePipeline();
}
updateTurretTarget(currentTicks, currentDegrees);
runPidControl(currentTicks, dt);
switch (autoCollectState) {
case IDLE:
follower.setTeleOpDrive(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x,
true
);
break;
case INIT_COLLECT:
returnPose = follower.getPose();
limelight.pipelineSwitch(PIPELINE_ARTIFACTS);
sequenceTimer.reset();
autoCollectState = AutoCollectState.WAIT_FOR_TURRET_AND_VISION;
break;
case WAIT_FOR_TURRET_AND_VISION:
double errorDeg = Math.abs(fusedTargetTicks - currentTicks) / ticksPerDegree;
if (errorDeg <= TURRET_STABLE_ERROR_DEG && sequenceTimer.milliseconds() > MIN_SNAPSHOT_WAIT_MS) {
autoCollectState = AutoCollectState.CALCULATE_SNAPSHOT;
}
break;
case CALCULATE_SNAPSHOT:
boolean snapshotSuccess = calculateSnapshotAndPath(currentDegrees);
if (snapshotSuccess) {
intake.setPower(1.0);
autoCollectState = AutoCollectState.DRIVING_TO_INTAKE;
} else {
abortAutoCollectSequence();
}
break;
case DRIVING_TO_INTAKE:
if (!follower.isBusy()) {
autoCollectState = AutoCollectState.PATH_RETURN;
}
break;
case PATH_RETURN:
PathChain returnChain = follower.pathBuilder()
.addPath(new BezierLine(follower.getPose(), returnPose))
.setLinearHeadingInterpolation(follower.getPose().getHeading(), returnPose.getHeading())
.build();
follower.followPath(returnChain, true);
autoCollectState = AutoCollectState.DRIVING_RETURN;
break;
case DRIVING_RETURN:
if (!follower.isBusy()) {
abortAutoCollectSequence();
}
break;
}
sleep(fpsCounter.getSyncTime(targetFPS));
updateTelemetry(currentTicks, currentDegrees);
}
limelight.stop();
}
private boolean calculateSnapshotAndPath(double turretCurrentDegrees) {
LLResult result = limelight.getLatestResult();
if (result == null || !result.isValid()) return false;
List<LLResultTypes.ColorResult> targets = result.getColorResults();
if (targets == null || targets.isEmpty()) return false;
if (targets.size() > 8) {
targets = targets.subList(0, 8);
}
LLResultTypes.ColorResult bestTarget = null;
int bestCount = 0;
for (int i = 0; i < targets.size(); i++) {
LLResultTypes.ColorResult a = targets.get(i);
int count = 0;
for (int j = 0; j < targets.size(); j++) {
if (i == j) continue;
LLResultTypes.ColorResult b = targets.get(j);
double dx = a.getTargetXDegrees() - b.getTargetXDegrees();
double dy = a.getTargetYDegrees() - b.getTargetYDegrees();
double dist = Math.sqrt(dx * dx + dy * dy);
if (dist < CLUSTER_RADIUS_DEG) {
count++;
}
}
if (count >= bestCount) {
bestCount = count;
bestTarget = a;
}
}
double sumTx = 0;
int clusterSize = 0;
for (LLResultTypes.ColorResult t : targets) {
double dx = bestTarget.getTargetXDegrees() - t.getTargetXDegrees();
double dy = bestTarget.getTargetYDegrees() - t.getTargetYDegrees();
double dist = Math.sqrt(dx * dx + dy * dy);
if (dist < CLUSTER_RADIUS_DEG) {
sumTx += t.getTargetXDegrees();
clusterSize++;
}
}
double avgTx = sumTx / clusterSize;
Pose robotPose = follower.getPose();
double rayHeadingRad = robotPose.getHeading() + Math.toRadians(turretCurrentDegrees) + Math.toRadians(avgTx * LL_LOGIC_MULTIPLIER);
while (rayHeadingRad > Math.PI) rayHeadingRad -= (2 * Math.PI);
while (rayHeadingRad < -Math.PI) rayHeadingRad += (2 * Math.PI);
double dx = (currentAlliance == Alliance.BLUE) ?
(BLUE_RAY_TARGET_X - robotPose.getX()) :
(RED_RAY_TARGET_X - robotPose.getX());
double dy = dx * Math.tan(rayHeadingRad);
double targetY = robotPose.getY() + dy;
if (targetY < 0 || targetY > 144) {
return false;
}
Pose targetPose;
if (currentAlliance == Alliance.BLUE) {
targetPose = new Pose(BLUE_DRIVE_TARGET_X, targetY, Math.toRadians(180));
} else {
targetPose = new Pose(RED_DRIVE_TARGET_X, targetY, Math.toRadians(0));
}
PathChain collectPath = follower.pathBuilder()
.addPath(new BezierLine(robotPose, targetPose))
.setLinearHeadingInterpolation(robotPose.getHeading(), targetPose.getHeading())
.build();
follower.followPath(collectPath, true);
return true;
}
private void updateTurretTarget(double currentTicks, double currentDegrees) {
if (autoCollectState == AutoCollectState.IDLE) {
double odoTargetTicks = calculateOdometryTargetTicks();
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid()) {
double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING)) + (rawErrorTicks * OFFSET_SMOOTHING);
}
double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
} else if (autoCollectState == AutoCollectState.INIT_COLLECT || autoCollectState == AutoCollectState.WAIT_FOR_TURRET_AND_VISION) {
double obsAngle = (currentAlliance == Alliance.BLUE) ? BLUE_OBSERVATION_ANGLE : RED_OBSERVATION_ANGLE;
fusedTargetTicks = obsAngle * ticksPerDegree;
} else {
fusedTargetTicks = 0;
targetCorrectionOffsetTicks = 0;
}
}
private double calculateOdometryTargetTicks() {
Pose robotPose = follower.getPose();
double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
double dx = targetX - robotPose.getX();
double dy = targetY - robotPose.getY();
double targetFieldHeading = Math.atan2(dy, dx);
double robotHeading = robotPose.getHeading();
double relativeRad = targetFieldHeading - robotHeading;
while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
double relativeDegrees = Math.toDegrees(relativeRad);
double staticOffset = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
return (relativeDegrees + staticOffset) * ticksPerDegree;
}
private void runPidControl(double currentTicks, double dt) {
double error = fusedTargetTicks - currentTicks;
double derivative = (error - lastError) / dt;
if (Math.abs(error) < (15 * ticksPerDegree)) {
integralSum += (error * dt);
} else {
integralSum = 0;
}
double integralTerm = kI * integralSum;
if (Math.abs(integralTerm) > MAX_INTEGRAL) {
integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
}
double output = (kP * error) + integralTerm + (kD * derivative);
output = Range.clip(output, -MAX_POWER, MAX_POWER);
turretMotor.setPower(output);
lastError = error;
}
private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
double tx = result.getTx();
double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES;
double visionTargetTicks = finalVisionDegrees * ticksPerDegree;
double rawErrorTicks = visionTargetTicks - odoTargetTicks;
double ticksPerRev = 360.0 * ticksPerDegree;
while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev;
while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
return rawErrorTicks;
}
private void handleInput() {
if (gamepad1.psWasPressed()) {
currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
}
if (gamepad1.x && autoCollectState == AutoCollectState.IDLE) {
autoCollectState = AutoCollectState.INIT_COLLECT;
}
if (gamepad1.b && autoCollectState != AutoCollectState.IDLE) {
abortAutoCollectSequence();
}
}
private void abortAutoCollectSequence() {
autoCollectState = AutoCollectState.IDLE;
intake.setPower(0);
follower.startTeleopDrive();
lastAlliance = null;
targetCorrectionOffsetTicks = 0;
}
private void updateAlliancePipeline() {
if (lastAlliance != currentAlliance) {
if (currentAlliance == Alliance.RED) {
limelight.pipelineSwitch(PIPELINE_GOAL_RED);
} else {
limelight.pipelineSwitch(PIPELINE_GOAL_BLUE);
}
lastAlliance = currentAlliance;
}
}
private void updateTelemetry(double currentTicks, double currentDegrees) {
telemetry.addData("State", autoCollectState);
telemetry.addData("Alliance", currentAlliance);
telemetry.addLine("------------------");
telemetry.addData("Target Ticks", fusedTargetTicks);
telemetry.addData("Current Ticks", currentTicks);
telemetry.addData("Error Ticks", fusedTargetTicks - currentTicks);
telemetry.addLine("------------------");
telemetry.addData("FPS", "%.2f", fpsCounter.getAverageFps());
telemetry.update();
}
private void applyMotorDirection() {
turretMotor.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
}
private void recalculateTicksPerDegree() {
ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
}
}

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.bylazar.telemetry.PanelsTelemetry;
@Disabled
@TeleOp(name = "Encoders Test", group = "Test")
public class encoderTest extends OpMode {

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java
View File

@@ -1,7 +1,5 @@
package org.firstinspires.ftc.teamcode.tuning;
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -9,7 +7,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
@@ -17,7 +14,7 @@ import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.teleOp.Constants;
@Configurable
@TeleOp
public class PIDFTuning extends OpMode {
public class flywheelTuning extends OpMode {
private Limelight3A limelight;

2
build.dependencies.gradle
View File

@@ -17,6 +17,6 @@ dependencies {
implementation 'com.pedropathing:ftc:2.0.6'
implementation 'com.pedropathing:telemetry:1.0.0'
//implementation 'com.bylazar:fullpanels:1.0.9'
implementation 'com.bylazar:fullpanels:1.0.9'
}