staged
This commit is contained in:
84
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java
Executable file → Normal file
84
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java
Executable file → Normal file
@@ -1,84 +1,4 @@
|
|||||||
package org.firstinspires.ftc.teamcode.tuning;
|
package org.firstinspires.ftc.teamcode.tuning;
|
||||||
|
|
||||||
import com.bylazar.configurables.annotations.Configurable;
|
public class PIDFTuning {
|
||||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
}
|
||||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
|
||||||
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;
|
|
||||||
|
|
||||||
// Make sure your import points to your actual Constants file
|
|
||||||
import org.firstinspires.ftc.teamcode.teleOp.Constants;
|
|
||||||
@Configurable
|
|
||||||
@TeleOp
|
|
||||||
public class PIDFTuning extends OpMode {
|
|
||||||
|
|
||||||
|
|
||||||
private Limelight3A limelight;
|
|
||||||
|
|
||||||
public DcMotorEx flywheelMotorR;
|
|
||||||
public DcMotorEx flywheelMotorL;
|
|
||||||
|
|
||||||
public Servo hoodServo;
|
|
||||||
|
|
||||||
public static double curTargetVelocity = 0;
|
|
||||||
public static double targetPOS = 0;
|
|
||||||
public static int pipeline = 1;
|
|
||||||
|
|
||||||
double distance = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void init() {
|
|
||||||
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
|
|
||||||
flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
|
|
||||||
|
|
||||||
flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
|
|
||||||
flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
||||||
flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
|
|
||||||
|
|
||||||
hoodServo = hardwareMap.servo.get("hoodServo");
|
|
||||||
|
|
||||||
hoodServo.setDirection(Servo.Direction.REVERSE);
|
|
||||||
|
|
||||||
limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
|
||||||
limelight.pipelineSwitch(pipeline);
|
|
||||||
limelight.start();
|
|
||||||
|
|
||||||
telemetry.addLine("Init complete");
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void loop() {
|
|
||||||
limelight.pipelineSwitch(pipeline);
|
|
||||||
|
|
||||||
LLResult result = limelight.getLatestResult();
|
|
||||||
|
|
||||||
if (result.isValid()) {
|
|
||||||
double targetY = result.getTy();
|
|
||||||
telemetry.addData("Ty", targetY);
|
|
||||||
distance = getTrigBasedDistance(targetY);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
|
|
||||||
flywheelMotorL.setVelocity(curTargetVelocity);}
|
|
||||||
|
|
||||||
hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
|
|
||||||
telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
|
|
||||||
telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
|
|
||||||
telemetry.addData("Distance", distance);
|
|
||||||
|
|
||||||
telemetry.update();
|
|
||||||
}
|
|
||||||
|
|
||||||
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
|
|
||||||
return (14) / Math.tan(Math.toRadians(2.0 + targetOffsetAngleVertical));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
@@ -1,370 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.tuning;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_HEIGHT_INCHES;
|
|
||||||
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
|
|
||||||
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.GOAL_HEIGHT_INCHES;
|
|
||||||
|
|
||||||
import com.bylazar.configurables.annotations.Configurable;
|
|
||||||
import com.bylazar.telemetry.PanelsTelemetry;
|
|
||||||
import com.bylazar.telemetry.TelemetryManager;
|
|
||||||
import com.pedropathing.follower.Follower;
|
|
||||||
import com.pedropathing.geometry.Pose;
|
|
||||||
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.DcMotorEx;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
|
||||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
|
||||||
import com.qualcomm.robotcore.hardware.Servo;
|
|
||||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
|
||||||
import com.qualcomm.robotcore.util.Range;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
|
|
||||||
import com.qualcomm.hardware.lynx.LynxModule;
|
|
||||||
|
|
||||||
import java.util.List;
|
|
||||||
|
|
||||||
@Configurable
|
|
||||||
@TeleOp(name = "Flywheel Testing", group = "All")
|
|
||||||
public class flywheelTesting extends LinearOpMode {
|
|
||||||
|
|
||||||
// --- PID Constants ---
|
|
||||||
public static double F = 15.6;
|
|
||||||
public static double P = 22.5;
|
|
||||||
public static double D = 0.001;
|
|
||||||
|
|
||||||
// Turret PID
|
|
||||||
public static double kP_LL = 0.02;
|
|
||||||
public static double kP_ODO = 0.035;
|
|
||||||
public static double kI = 0.0;
|
|
||||||
public static double kD_LL = 0.001;
|
|
||||||
public static double kD_ODO = 0.0007;
|
|
||||||
|
|
||||||
public static double MAX_POWER = 0.5;
|
|
||||||
public static double MAX_INTEGRAL = 0.5;
|
|
||||||
|
|
||||||
public static double TICKS_PER_REV_MOTOR = 384.5;
|
|
||||||
public static double EXTERNAL_GEAR_RATIO = 1.315994798439532;
|
|
||||||
|
|
||||||
// FIX: Static Motor Direction & Logic Multiplier
|
|
||||||
public static boolean MOTOR_REVERSED = false;
|
|
||||||
public static double LL_LOGIC_MULTIPLIER = -1.0;
|
|
||||||
|
|
||||||
public static double LL_TARGET_OFFSET_DEGREES = 4;
|
|
||||||
public static double RED_TARGET_OFFSET_DEGREES = 14;
|
|
||||||
public static double BLUE_TARGET_OFFSET_DEGREES = 17;
|
|
||||||
|
|
||||||
private double TARGET_OFFSET_DEGREES = BLUE_TARGET_OFFSET_DEGREES;
|
|
||||||
|
|
||||||
public static double SOFT_LIMIT_NEG = -250.0;
|
|
||||||
public static double SOFT_LIMIT_POS = 250.0;
|
|
||||||
public static double WRAP_THRESHOLD_DEGREES = 180.0;
|
|
||||||
|
|
||||||
// --- Goal Coordinates for Odometry ---
|
|
||||||
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;
|
|
||||||
|
|
||||||
private double targetTicks = 0;
|
|
||||||
private double ticksPerDegree = 0;
|
|
||||||
|
|
||||||
// --- Hardware Constants ---
|
|
||||||
public static double SERVO_MIN = 0.4;
|
|
||||||
public static double SERVO_MAX = 1.0;
|
|
||||||
public static double OFFSET_GAIN = -0.0005;
|
|
||||||
|
|
||||||
private Limelight3A limelight;
|
|
||||||
private DcMotorEx turretRotation;
|
|
||||||
private DcMotorEx leftTurret, rightTurret;
|
|
||||||
private Servo hoodServo;
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private TelemetryManager telemetryM;
|
|
||||||
|
|
||||||
private double integralSum = 0;
|
|
||||||
private double lastError = 0;
|
|
||||||
|
|
||||||
private boolean launch = false;
|
|
||||||
private int pipelineIndex = 1;
|
|
||||||
|
|
||||||
private final ElapsedTime loopTimer = new ElapsedTime();
|
|
||||||
private final ElapsedTime limelightFailureTimer = new ElapsedTime();
|
|
||||||
|
|
||||||
private double loopTimeSum = 0;
|
|
||||||
private int loopCount = 0;
|
|
||||||
|
|
||||||
private enum Alliance { RED, BLUE }
|
|
||||||
private Alliance currentAlliance = Alliance.BLUE;
|
|
||||||
|
|
||||||
private enum TrackingSource { LIMELIGHT, ODOMETRY }
|
|
||||||
private TrackingSource activeSource = TrackingSource.ODOMETRY;
|
|
||||||
private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
|
|
||||||
|
|
||||||
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() {
|
|
||||||
telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
|
|
||||||
allHubs = hardwareMap.getAll(LynxModule.class);
|
|
||||||
|
|
||||||
for (LynxModule hub : allHubs) {
|
|
||||||
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- Hardware Map ---
|
|
||||||
turretRotation = hardwareMap.get(DcMotorEx.class, "turretRotation");
|
|
||||||
|
|
||||||
// STATIC DIRECTION CONFIGURATION
|
|
||||||
turretRotation.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
|
|
||||||
|
|
||||||
turretRotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
||||||
turretRotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
||||||
turretRotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
|
||||||
|
|
||||||
leftTurret = hardwareMap.get(DcMotorEx.class, "lturret");
|
|
||||||
rightTurret = hardwareMap.get(DcMotorEx.class, "rturret");
|
|
||||||
hoodServo = hardwareMap.get(Servo.class, "hoodServo");
|
|
||||||
|
|
||||||
// PedroPathing Follower Setup
|
|
||||||
follower = Constants.createFollower(hardwareMap);
|
|
||||||
follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
|
|
||||||
|
|
||||||
limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
|
||||||
|
|
||||||
leftTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
||||||
rightTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
||||||
rightTurret.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
||||||
|
|
||||||
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, D, F);
|
|
||||||
rightTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
|
|
||||||
leftTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
|
|
||||||
|
|
||||||
recalculateTicksPerDegree();
|
|
||||||
|
|
||||||
limelight.pipelineSwitch(pipelineIndex);
|
|
||||||
limelight.start();
|
|
||||||
|
|
||||||
telemetryM.debug("Status", "Initialized");
|
|
||||||
telemetryM.update(telemetry);
|
|
||||||
|
|
||||||
waitForStart();
|
|
||||||
loopTimer.reset();
|
|
||||||
limelightFailureTimer.reset();
|
|
||||||
|
|
||||||
follower.startTeleopDrive(true);
|
|
||||||
follower.update();
|
|
||||||
|
|
||||||
loopTimeSum = 0;
|
|
||||||
loopCount = 0;
|
|
||||||
|
|
||||||
while (opModeIsActive()) {
|
|
||||||
update();
|
|
||||||
}
|
|
||||||
limelight.stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
private void update() {
|
|
||||||
for (LynxModule hub : allHubs) {
|
|
||||||
hub.clearBulkCache();
|
|
||||||
}
|
|
||||||
|
|
||||||
follower.update();
|
|
||||||
|
|
||||||
double dt = loopTimer.seconds();
|
|
||||||
loopTimer.reset();
|
|
||||||
loopTimeSum += dt;
|
|
||||||
loopCount++;
|
|
||||||
if (dt < 0.001) dt = 0.001;
|
|
||||||
|
|
||||||
// Pass movement to follower for pose updates (Robot Centric)
|
|
||||||
follower.setTeleOpDrive(
|
|
||||||
-gamepad1.left_stick_y,
|
|
||||||
-gamepad1.left_stick_x,
|
|
||||||
-gamepad1.right_stick_x,
|
|
||||||
true
|
|
||||||
);
|
|
||||||
|
|
||||||
if (gamepad1.startWasPressed()) {
|
|
||||||
pipelineIndex = (pipelineIndex + 1) % 3;
|
|
||||||
limelight.pipelineSwitch(pipelineIndex);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gamepad1.psWasPressed()) {
|
|
||||||
currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gamepad1.yWasPressed()) launch = !launch;
|
|
||||||
|
|
||||||
// --- TURRET TRACKING LOGIC ---
|
|
||||||
if (currentAlliance == Alliance.BLUE) limelight.pipelineSwitch(1);
|
|
||||||
else limelight.pipelineSwitch(2);
|
|
||||||
|
|
||||||
LLResult result = limelight.getLatestResult();
|
|
||||||
|
|
||||||
double rawTicks = turretRotation.getCurrentPosition();
|
|
||||||
double currentDegrees = rawTicks / ticksPerDegree;
|
|
||||||
|
|
||||||
double calculatedTargetTicks = Double.NaN;
|
|
||||||
|
|
||||||
// Hybrid Logic
|
|
||||||
calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
|
|
||||||
|
|
||||||
if (Double.isNaN(calculatedTargetTicks)) {
|
|
||||||
if (limelightFailureTimer.seconds() > 0.5) {
|
|
||||||
calculatedTargetTicks = calculateOdometryTargetTicks();
|
|
||||||
activeSource = TrackingSource.ODOMETRY;
|
|
||||||
} else {
|
|
||||||
calculatedTargetTicks = targetTicks;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
limelightFailureTimer.reset();
|
|
||||||
activeSource = TrackingSource.LIMELIGHT;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!Double.isNaN(calculatedTargetTicks)) {
|
|
||||||
targetTicks = calculatedTargetTicks;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (activeSource != lastActiveSource) {
|
|
||||||
integralSum = 0;
|
|
||||||
lastError = 0;
|
|
||||||
}
|
|
||||||
lastActiveSource = activeSource;
|
|
||||||
|
|
||||||
double kP_use = (activeSource == TrackingSource.ODOMETRY) ? kP_ODO : kP_LL;
|
|
||||||
double kD_use = (activeSource == TrackingSource.ODOMETRY) ? kD_ODO : kD_LL;
|
|
||||||
|
|
||||||
double error = targetTicks - rawTicks;
|
|
||||||
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_use * error) + integralTerm + (kD_use * derivative);
|
|
||||||
output = Range.clip(output, -MAX_POWER, MAX_POWER);
|
|
||||||
|
|
||||||
turretRotation.setPower(output);
|
|
||||||
lastError = error;
|
|
||||||
|
|
||||||
// --- SHOOTER CALCULATIONS (Using TY) ---
|
|
||||||
double calculatedDistance = 0;
|
|
||||||
double calculatedVelocity = 0;
|
|
||||||
double calculatedHoodPos = 0;
|
|
||||||
|
|
||||||
if (result != null && result.isValid()) {
|
|
||||||
double ty = result.getTy();
|
|
||||||
calculatedDistance = getDistanceFromTag(ty);
|
|
||||||
calculatedVelocity = getFlywheelVelocity(calculatedDistance);
|
|
||||||
calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
|
|
||||||
|
|
||||||
if (launch) {
|
|
||||||
leftTurret.setVelocity(calculatedVelocity);
|
|
||||||
rightTurret.setVelocity(calculatedVelocity);
|
|
||||||
hoodServo.setPosition(calculatedHoodPos);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (launch) {
|
|
||||||
if (leftTurret.getVelocity() * 0.9 <= calculatedVelocity) {
|
|
||||||
// unlockTurret();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Telemetry
|
|
||||||
telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
|
|
||||||
telemetryM.debug("Source", activeSource);
|
|
||||||
telemetryM.debug("Turret Angle", currentDegrees);
|
|
||||||
telemetryM.debug("Hood Target", calculatedHoodPos);
|
|
||||||
telemetryM.debug("Distance", calculatedDistance);
|
|
||||||
telemetryM.debug("Velocity", calculatedVelocity);
|
|
||||||
|
|
||||||
if (loopCount > 0) {
|
|
||||||
telemetryM.debug("▰▰▰▰▰▰▰ FPS ▰▰▰▰▰▰▰");
|
|
||||||
telemetryM.debug("FPS", "%.2f", 1000 / (dt * 1000));
|
|
||||||
}
|
|
||||||
|
|
||||||
telemetryM.update(telemetry);
|
|
||||||
}
|
|
||||||
|
|
||||||
private double calculateLimelightTargetTicks(LLResult result, double currentDegrees, double currentTicks) {
|
|
||||||
if (result != null && result.isValid()) {
|
|
||||||
double tx = result.getTx();
|
|
||||||
double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
|
|
||||||
|
|
||||||
TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
|
|
||||||
double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
|
|
||||||
double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
|
|
||||||
|
|
||||||
double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
|
|
||||||
if (potentialTargetTicks < -wrapLimitTicks) {
|
|
||||||
potentialTargetTicks = SOFT_LIMIT_POS;
|
|
||||||
} else if (potentialTargetTicks > wrapLimitTicks) {
|
|
||||||
potentialTargetTicks = SOFT_LIMIT_NEG;
|
|
||||||
}
|
|
||||||
|
|
||||||
return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
|
|
||||||
}
|
|
||||||
return Double.NaN;
|
|
||||||
}
|
|
||||||
|
|
||||||
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();
|
|
||||||
|
|
||||||
TARGET_OFFSET_DEGREES = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
|
|
||||||
|
|
||||||
double relativeRad = targetFieldHeading - robotHeading;
|
|
||||||
|
|
||||||
while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
|
|
||||||
while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
|
|
||||||
|
|
||||||
double calculatedTargetAngle = Math.toDegrees(relativeRad);
|
|
||||||
double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
|
|
||||||
double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
|
|
||||||
|
|
||||||
return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
|
|
||||||
}
|
|
||||||
|
|
||||||
private void recalculateTicksPerDegree() {
|
|
||||||
ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
private double getDistanceFromTag(double x) {
|
|
||||||
// Uses Trigonometry (TY) instead of Area (TA)
|
|
||||||
return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + x));
|
|
||||||
}
|
|
||||||
|
|
||||||
private double calculateHoodOffset(double target) {
|
|
||||||
return (target - leftTurret.getVelocity()) * OFFSET_GAIN;
|
|
||||||
}
|
|
||||||
|
|
||||||
private double getFlywheelVelocity(double dist) {
|
|
||||||
return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
|
|
||||||
}
|
|
||||||
|
|
||||||
private double getHoodPOS(double dist, double targetVelocity) {
|
|
||||||
return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,90 @@
|
|||||||
|
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;
|
||||||
|
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;
|
||||||
|
|
||||||
|
// Make sure your import points to your actual Constants file
|
||||||
|
import org.firstinspires.ftc.teamcode.teleOp.Constants;
|
||||||
|
@Configurable
|
||||||
|
@TeleOp
|
||||||
|
public class PIDFTuning extends OpMode {
|
||||||
|
|
||||||
|
|
||||||
|
private Limelight3A limelight;
|
||||||
|
|
||||||
|
public DcMotorEx flywheelMotorR;
|
||||||
|
public DcMotorEx flywheelMotorL;
|
||||||
|
|
||||||
|
public Servo hoodServo;
|
||||||
|
|
||||||
|
public static double curTargetVelocity = 0;
|
||||||
|
public static double targetPOS = 0;
|
||||||
|
public static int pipeline = 1;
|
||||||
|
|
||||||
|
public static boolean reverse = false;
|
||||||
|
|
||||||
|
double distance = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
|
||||||
|
flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
|
||||||
|
|
||||||
|
flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
|
||||||
|
flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
|
||||||
|
flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
|
||||||
|
flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
|
||||||
|
|
||||||
|
hoodServo = hardwareMap.servo.get("hoodservo");
|
||||||
|
|
||||||
|
hoodServo.setDirection(Servo.Direction.REVERSE);
|
||||||
|
|
||||||
|
limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
||||||
|
limelight.pipelineSwitch(pipeline);
|
||||||
|
limelight.start();
|
||||||
|
|
||||||
|
telemetry.addLine("Init complete");
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
limelight.pipelineSwitch(pipeline);
|
||||||
|
|
||||||
|
LLResult result = limelight.getLatestResult();
|
||||||
|
|
||||||
|
if (result.isValid()) {
|
||||||
|
double targetY = result.getTy();
|
||||||
|
telemetry.addData("Ty", targetY);
|
||||||
|
distance = getTrigBasedDistance(targetY);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
|
||||||
|
flywheelMotorL.setVelocity(curTargetVelocity);}
|
||||||
|
|
||||||
|
hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
|
||||||
|
telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
|
||||||
|
telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
|
||||||
|
telemetry.addData("Distance", distance);
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
|
||||||
|
return (17.1) / Math.tan(Math.toRadians(7.9 + targetOffsetAngleVertical));
|
||||||
|
// ^ GOAL - CAMERA HEIGHT ^ Offset Degrees
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user