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

a decent working version

Browse Source
This commit is contained in:
Arkm20
2026-03-12 18:05:18 -05:00
parent c90732ff53
commit d3ec25b8dc
20 changed files with 602 additions and 278 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,12 +35,10 @@ public class hwMap {
}
public static class LedHwMap {
public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1;
public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led;
public LedHwMap(HardwareMap hardwareMap) {
/*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1");
led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/
//led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED);
}
}
@@ -89,7 +87,7 @@ public class hwMap {
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP
)
);
@@ -250,7 +248,7 @@ public class hwMap {
private double lastKnownDistance = 0.0;
private ElapsedTime noTargetTimer = new ElapsedTime();
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(22.5, 0, 0.001, 15.6);
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15);
public TurretHwMap(HardwareMap hardwareMap) {
noTargetTimer.reset();
@@ -283,12 +281,31 @@ public class hwMap {
initLimelight(hardwareMap);
}
public void setPIDF(double p, double i, double d, double f) {
PIDFCoefficients pidf = new PIDFCoefficients(p, i, d, f);
turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
public void resetTurret() {
turretrotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void setPIDF(double p, double i, double d, double f) {
PIDFCoefficients PIDF = new PIDFCoefficients(p, i, d, f);
turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF);
turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF);
}
public void enableTurretHold() {
turretrotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void disableTurretHold() {
turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void runToPos(int target) {
turretrotation.setTargetPosition(target);
}
public double getTurretRotationPosition() {
return turretrotation.getCurrentPosition();

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java
View File

@@ -517,6 +517,8 @@ public class Auton {
hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
turretHw.setPIDF(120, 0, 10, 15);
systems = new pedroStateMachine(intakeHw, transferHw, turretHw);
systems.setAlliance(allianceTarget);
@@ -531,13 +533,16 @@ public class Auton {
telemetry.update();
}
follower.update();
turretHw.resetTurret();
waitForStart();
systems.update(allianceTarget, follower.getPose());
opmodeTimer.resetTimer();
setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
while (opModeIsActive()) {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}

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

@@ -25,7 +25,7 @@ public class Constants {
.lateralZeroPowerAcceleration(-76.57271150137258);
public static MecanumConstants driveConstants = new MecanumConstants()
.maxPower(0.8)
.maxPower(0.7)
.xVelocity(68.10320673181904)
.yVelocity(57.52038399624941)
.rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
@@ -41,15 +41,15 @@ public class Constants {
public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
.forwardTicksToInches(0.001978956)
.strafeTicksToInches(0.001978956)
.forwardTicksToInches(0.0020041908950982315)
.strafeTicksToInches(0.002004094712407555)
.turnTicksToInches(0.0022824233082645137)
.leftPodY(3.75)
.rightPodY(-3.25)
.strafePodX(-6.25)
.leftEncoder_HardwareMapName("intake")
.rightEncoder_HardwareMapName("fl")
.strafeEncoder_HardwareMapName("bl")
.strafeEncoder_HardwareMapName("fr")
.leftEncoderDirection(Encoder.FORWARD)
.rightEncoderDirection(Encoder.FORWARD)
.strafeEncoderDirection(Encoder.FORWARD);

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
View File

@@ -1,76 +1,56 @@
package org.firstinspires.ftc.teamcode.subsystems;
import org.firstinspires.ftc.robotcore.external.Const;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.Prism.Color;
import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
import org.firstinspires.ftc.teamcode.teleOp.Constants;
public class LED {
private final hwMap.LedHwMap hardware;
// Track states to prevent spamming the I2C bus
private int currentLed1State = -1;
private int currentLed2State = -1;
private int currentLedState = -1;
// Reusable animations for efficiency
private final PrismAnimations.Solid solidRed = new PrismAnimations.Solid(Color.RED);
private final PrismAnimations.Solid solidOrange = new PrismAnimations.Solid(Color.ORANGE);
private final PrismAnimations.Solid solidYellow = new PrismAnimations.Solid(Color.YELLOW);
private final PrismAnimations.Solid solidGreen = new PrismAnimations.Solid(Color.GREEN);
private final PrismAnimations.Solid solidBlue = new PrismAnimations.Solid(Color.BLUE);
private final PrismAnimations.Solid solidPurple = new PrismAnimations.Solid(Color.PURPLE); // or MAGENTA if PURPLE is missing
public LED(hwMap.LedHwMap hardware) {
this.hardware = hardware;
solidRed.setBrightness(50);
solidOrange.setBrightness(50);
solidGreen.setBrightness(50);
solidBlue.setBrightness(50);
solidPurple.setBrightness(50);
int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS;
solidRed.setBrightness(LED_BRIGHTNESS);
solidOrange.setBrightness(LED_BRIGHTNESS);
solidYellow.setBrightness(LED_BRIGHTNESS);
solidGreen.setBrightness(LED_BRIGHTNESS);
}
public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) {
/*
int newLed1State = 0;
if (isScoring) {
if (isTurretReady) {
newLed1State = 2;
} else {
newLed1State = 1;
}
public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) {
/*int newLedState = 0;
if (isTurretLocked && isFlywheelUpToSpeed) {
newLedState = 1;
} else if (isTurretLocked) {
newLedState = 2;
} else if (isFlywheelUpToSpeed) {
newLedState = 3;
}
if (newLed1State != currentLed1State) {
if (newLed1State == 0) {
hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
} else if (newLed1State == 1) {
hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
} else if (newLed1State == 2) {
hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
if (newLedState != currentLedState) {
if (newLedState == 0) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
} else if (newLedState == 1) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
} else if (newLedState == 2) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
} else if (newLedState == 3) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow);
}
currentLed1State = newLed1State;
}
int newLed2State = 0;
if (hasTarget && isAnchorActive) {
newLed2State = 3;
} else if (hasTarget) {
newLed2State = 1;
} else if (isAnchorActive) {
newLed2State = 2;
}
if (newLed2State != currentLed2State) {
if (newLed2State == 0) {
hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
} else if (newLed2State == 1) {
hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
} else if (newLed2State == 2) {
hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
} else if (newLed2State == 3) {
hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple);
}
currentLed2State = newLed2State;
currentLedState = newLedState;
}*/
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystems;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.teleOp.Constants;
public class TransferSys {
private final hwMap.TransferHwMap hardware;
@@ -9,7 +10,7 @@ public class TransferSys {
private boolean isTurretReady = false;
private boolean limiterClosed = true;
public double launchDuration = 2; // CHANGEABLE TODO
public double launchDuration = Constants.TransferConstants.launch_duration; // CHANGEABLE TODO
public enum TransferState {
LAUNCHING, // Gate OPEN
@@ -94,6 +95,13 @@ public class TransferSys {
return limiterClosed;
}
public double getLaunchElapsedTime() {
if (currentState == TransferState.LAUNCHING && isLaunchSequenceActive) {
return timer.seconds();
}
return 100.0;
}
public TransferState getTransferState() {
return currentState;
}

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

@@ -1,8 +1,5 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.tuning.allInOne.OFFSET_GAIN;
import static org.firstinspires.ftc.teamcode.tuning.allInOne.SERVO_MIN;
import com.pedropathing.geometry.Pose;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -33,7 +30,14 @@ public class Turret {
private Pose robotPose;
public double targetTick;
private final ElapsedTime loopTimer = new ElapsedTime();
private final ElapsedTime stateTimer = new ElapsedTime();
public boolean turretModeHold = false;
public boolean lastTurretModeHold = false;
public enum TurretState {
LAUNCH,
@@ -45,17 +49,23 @@ public class Turret {
public Turret(hwMap.TurretHwMap hardware) {
this.hardware = hardware;
offsetTicks = 0; // Reset manual offset on every init
targetCorrectionOffsetTicks = 0; // Reset vision correction
loopTimer.reset();
stateTimer.reset();
}
public void setAlliance(int alliance) {
this.alliance = alliance;
if (alliance == 2) {
hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
} else if(alliance == 1) {
hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
} else {
hardware.setPipeline(0);
if (this.alliance != alliance) {
this.alliance = alliance;
targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
if (alliance == 2) {
hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
} else if (alliance == 1) {
hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
} else {
hardware.setPipeline(0);
}
}
}
@@ -68,21 +78,24 @@ public class Turret {
}
public void setTurretState(TurretState state) {
this.currentState = state;
if (this.currentState != state) {
this.currentState = state;
stateTimer.reset();
switch (state) {
case IDLE:
hardware.turretOff();
hardware.stopTurretRotation();
integralSum = 0;
lastError = 0;
break;
case EXTAKE:
hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
break;
case INTAKING:
hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
break;
switch (state) {
case IDLE:
hardware.turretOff();
hardware.stopTurretRotation();
integralSum = 0;
lastError = 0;
break;
case EXTAKE:
hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
break;
case INTAKING:
hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
break;
}
}
}
@@ -95,7 +108,7 @@ public class Turret {
return hardware.hasTarget();
}
public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) {
public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) {
double dt = loopTimer.seconds();
loopTimer.reset();
if (dt < 0.001) dt = 0.001;
@@ -115,8 +128,18 @@ public class Turret {
}
if (currentState == TurretState.LAUNCH) {
handleLaunchLogic();
handleLaunchLogic(elapsedTime);
}
else {
// idleLaunch();
}
}
public void idleLaunch() {
hardware.setTurretVelocity(1000);
double calculatedHoodPos = getHoodPOS(65);
setHoodPos(calculatedHoodPos);
}
public void updateAuton(int alliance, Pose currentPose, double offset) {
@@ -127,6 +150,11 @@ public class Turret {
setAlliance(alliance);
if (currentState == TurretState.IDLE) {
hardware.stopTurretRotation();
return;
}
double rawTicks = hardware.getTurretRotationPosition();
double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
@@ -134,38 +162,63 @@ public class Turret {
calculateHybridTarget(currentPose, currentDegrees, rawTicks);
runPIDControl(rawTicks, dt);
handleLaunchLogic(-offset);
handleLaunchLogicOffset(-offset);
}
private void handleLaunchLogic() {
private void handleLaunchLogic(double elapsedTime) {
if (distance > 0) {
targetVelocity = getFlywheelVelocity(distance);
double baseVelocity = getFlywheelVelocity(distance);
double t = stateTimer.seconds();
/* TUNING GUIDE:
* 1. Adjust ARTIFACT_1_WINDOW and ARTIFACT_2_WINDOW in Constants.java to match your launch sequence timing.
* 2. Adjust ARTIFACT_OFFSET_LOW to lower the velocity for the 1st and 3rd artifacts.
* 3. ARTIFACT_OFFSET_NORMAL (usually 0) is for the 2nd artifact.
*/
double compensation;
if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) {
compensation = Constants.TurretConstants.ARTIFACT_1;
} else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) {
compensation = Constants.TurretConstants.ARTIFACT_2;
} else {
compensation = Constants.TurretConstants.ARTIFACT_3;
}
targetVelocity = baseVelocity + compensation;
hardware.setTurretVelocity(targetVelocity);
double calculatedHoodPos = getHoodPOS(distance);
double currentVel = hardware.getFlywheelVelocities()[0];
double dropThreshold = targetVelocity * 0.95;
double recoilOffset = 0.0;
double currentVel = hardware.getFlywheelVelocities()[1];
recoilOffset = (currentVel - targetVelocity) * 0.000004;
double recoilOffset = (currentVel - targetVelocity) * -0.000185;
setHoodPos(calculatedHoodPos + recoilOffset);
setHoodPos(calculatedHoodPos + recoilOffset - 0.03);
} else {
hardware.setTurretVelocity(0);
}
}
private void handleLaunchLogic(double offset) {
private void handleLaunchLogicOffset(double offset) {
if (distance > 0) {
targetVelocity = getFlywheelVelocity(distance);
double baseVelocity = getFlywheelVelocity(distance);
double t = stateTimer.seconds();
double compensation;
if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) {
compensation = Constants.TurretConstants.ARTIFACT_1;
} else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) {
compensation = Constants.TurretConstants.ARTIFACT_2;
} else {
compensation = Constants.TurretConstants.ARTIFACT_3;
}
targetVelocity = baseVelocity + compensation;
hardware.setTurretVelocity(targetVelocity);
double calculatedHoodPos = getHoodPOS(distance);
double currentVel = hardware.getFlywheelVelocities()[0];
double dropThreshold = targetVelocity * 0.95;
double recoilOffset = 0.0;
double currentVel = hardware.getFlywheelVelocities()[1];
recoilOffset = (currentVel - targetVelocity) * 0.000004;
double recoilOffset = (currentVel - targetVelocity) * -0.000185;
setHoodPos(calculatedHoodPos + recoilOffset);
} else {
@@ -178,13 +231,36 @@ public class Turret {
LLResult result = hardware.getLimelightResult();
if (result != null && result.isValid()) {
double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING))
+ (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING);
// 1. Calculate Vision Truth: Where the goal sits based on the camera right now
double tx = result.getTx();
double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
// 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
double correctionError = visionTargetTicks - currentFusedTarget;
// 4. Normalize the error for wrapping (360 degrees)
double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev;
while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
// 5. Apply the error to the persistent offset (Bias-Shift)
targetCorrectionOffsetTicks += (correctionError * Constants.TurretConstants.OFFSET_SMOOTHING);
}
double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
double fullWrapTicks = Constants.TurretConstants.TURRET_FULL_WRAP_TICKS;
double halfWrapTicks = fullWrapTicks / 2.0;
if (rawTarget > halfWrapTicks) {rawTarget -= fullWrapTicks;}
else if (rawTarget < -halfWrapTicks) {rawTarget += fullWrapTicks;}
fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS);
targetTick = fusedTargetTicks;
}
private double calculateOdometryTargetTicks(Pose robotPose) {
@@ -208,42 +284,53 @@ public class Turret {
return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE;
}
private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
double tx = result.getTx();
double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
double rawErrorTicks = visionTargetTicks - odoTargetTicks;
double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev;
while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
return rawErrorTicks;
}
private void runPIDControl(double currentTicks, double dt) {
fusedTargetTicks = fusedTargetTicks + offsetTicks;
double error = fusedTargetTicks - currentTicks;
double derivative = (error - lastError) / dt;
if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
integralSum += (error * dt);
} else {
integralSum = 0;
// Hysteresis thresholds for mode switching
double entryThreshold = 15; // Enter hold mode if error is within this range
double exitThreshold = 30; // Exit hold mode if error exceeds this range
if (!turretModeHold && Math.abs(error) <= entryThreshold) {
turretModeHold = true;
} else if (turretModeHold && Math.abs(error) > exitThreshold) {
turretModeHold = false;
}
double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
if (turretModeHold) {
if (lastTurretModeHold != turretModeHold) {
hardware.runToPos((int) fusedTargetTicks); // Set target first
hardware.enableTurretHold(); // Then set mode
hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power
}
hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode
}
else {
if (lastTurretModeHold != turretModeHold) {
hardware.disableTurretHold();
}
hardware.setTurretRotationPower(output);
turretPowerRotation = output;
if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
integralSum += (error * dt);
} else {
integralSum = 0;
}
double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
hardware.setTurretRotationPower(output);
turretPowerRotation = output;
}
lastTurretModeHold = turretModeHold;
lastError = error;
}
public boolean hasReachedVelocity() {
double currentVel = hardware.getFlywheelVelocities()[0];
double currentVel = hardware.getFlywheelVelocities()[1];
if (Math.abs(targetVelocity) > 1000) {
double absCurrent = Math.abs(currentVel);
@@ -258,7 +345,7 @@ public class Turret {
}
public boolean isTurretReady() {
return hasReachedVelocity();
return hasReachedVelocity() && isTurretRotationLocked();
}
@@ -288,11 +375,11 @@ public class Turret {
}
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);
return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126);
}
private double getHoodPOS(double dist) {
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);
return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
}
public void setHoodPos(double pos) {
@@ -311,7 +398,20 @@ public class Turret {
return turretPower;
}
public double returnFlywheelSpeed() {
return hardware.getFlywheelVelocities()[1];
}
public double getTargetVelocity() {
return targetVelocity;
}
}
public void resetTurret() {
hardware.resetTurret();
}
public boolean isTurretRotationLocked() {
return turretModeHold && Math.abs(lastError) <= 10;
}
}

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

@@ -5,7 +5,8 @@ import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@Configurable
public class Constants {
public static class FieldConstants {
@@ -37,6 +38,13 @@ public class Constants {
public static final double TURBO_SPEED_MULTIPLIER = 1.0;
}
public static class LEDConstants {
public static final String LED = "prism";
public static final int LAYER_BRIGHTNESS = 50;
}
public static class IntakeConstants {
public static final String FRONT_INTAKE_MOTOR = "intake";
@@ -68,6 +76,9 @@ public class Constants {
public static final double POWER_HOLDING = 0.1;
}
@Configurable
public static class TransferConstants {
public static final String LIMITER_SERVO = "limiter";
@@ -82,34 +93,37 @@ public class Constants {
public static final double UNLOCK_POS = 0.82;
public static double launch_duration = 0.77;
}
@Configurable
public static class TurretConstants {
public static final double TICKS_PER_REV_MOTOR = 384.5;
public static final double EXTERNAL_GEAR_RATIO = 2.68888889;
public static double TICKS_PER_REV_MOTOR = 145.1;
public static double EXTERNAL_GEAR_RATIO = 9.179875;
public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
public static final double KP = 0.02;
public static final double KI = 0.0;
public static final double KD = 0.001;
public static final double MAX_POWER = 0.6;
public static double KP = 0.0048;
public static double KI = 0.00801;
public static double KD = 0.00083;
public static final double MAX_POWER = 0.8;
public static final double MAX_INTEGRAL = 0.5;
public static final double GOAL_RED_X = 138;
public static final double GOAL_RED_Y = 138;
public static final double GOAL_BLUE_X = 6;
public static final double GOAL_BLUE_Y = 138;
public static double GOAL_RED_X = 140;
public static double GOAL_RED_Y = 140;
public static double GOAL_BLUE_X = 4;
public static double GOAL_BLUE_Y = 140;
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 = -2;
public static final double LL_LOGIC_MULTIPLIER = 1.0;
public static final double OFFSET_SMOOTHING = 0.2;
public static double RED_TARGET_OFFSET_DEGREES = 0;
public static double BLUE_TARGET_OFFSET_DEGREES = 0;
public static double LL_TARGET_OFFSET_DEGREES = 0;
public static double LL_LOGIC_MULTIPLIER = -1.0;
public static double OFFSET_SMOOTHING = 0.2;
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 double SOFT_LIMIT_NEG = -324;
public static double SOFT_LIMIT_POS = 195;
public static final double HOOD_POS_CLOSE = 0.3;
public static final double HOOD_POS_FAR = 0.7;
@@ -122,6 +136,7 @@ public class Constants {
public static final double TURRET_POWER_MID = -0.84;
public static final double TURRET_POWER_MAX = -1;
public static final double TURRET_POWER_LOW = -0.7;
public static final double TURRET_HOLD_POWER = 0.75;
public static final double EXTAKE_POWER = 0.3;
public static final double INTAKE_POWER = -0.04;
@@ -140,12 +155,25 @@ public class Constants {
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 = 12.4;
public static final double GOAL_HEIGHT_INCHES = 29.5;
public static final double CAMERA_MOUNT_ANGLE_DEGREES = 7.9;
public static double CAMERA_HEIGHT_INCHES = 12.75;
public static double GOAL_HEIGHT_INCHES = 29.5;
public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8;
public static final double SERVO_MAX = 0.5;
public static final double SERVO_MIN = 0;
public static double LL_POSE_HEALING_SMOOTHING = 0.05;
public static double TURRET_FULL_WRAP_TICKS = 1340.0;
// Artifact Compensation Tuning
// Lower (negative offset) for 1st and 3rd, Normal (0 offset) for 2
public static double ARTIFACT_1 = -95.0;
public static double ARTIFACT_2 = 20.0;
public static double ARTIFACT_3 = -20.0;
// Timing Windows (seconds)
public static double ARTIFACT_1_WINDOW = 0.35;
public static double ARTIFACT_2_WINDOW = 0.55;
}

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

@@ -14,6 +14,8 @@ import org.firstinspires.ftc.teamcode.util.AutoTransfer;
import org.firstinspires.ftc.teamcode.util.FPSCounter;
import com.qualcomm.hardware.lynx.LynxModule;
import java.util.List;
import com.qualcomm.robotcore.hardware.VoltageSensor;
public class SOLOTeleOP {
@TeleOp(name = "Red SOLO", group = "FINAL")
@@ -35,14 +37,17 @@ public class SOLOTeleOP {
private Follower follower;
private int currentAlliance;
private final int initialAlliance;
private boolean isScoringActive = false;
protected List<LynxModule> allHubs;
private final Pose DEFAULT_RED_START = new Pose(129.25, 112, Math.toRadians(90));
private final Pose DEFAULT_BLUE_START = new Pose(14.75, 112, Math.toRadians(90));
protected FPSCounter fpsCounter = new FPSCounter();
private boolean isScoringActive = false;
public BaseSoloTeleOp(int alliance) {
this.initialAlliance = alliance;
this.currentAlliance = 0;
@@ -63,6 +68,7 @@ public class SOLOTeleOP {
currentAlliance = initialAlliance;
if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
else follower.setStartingPose(DEFAULT_BLUE_START);
stateMachine.getTurret().resetTurret();
telemetry.addLine("MANUAL START - NO AUTON DATA");
}
@@ -91,16 +97,12 @@ public class SOLOTeleOP {
follower.update();
Pose currentPose = follower.getPose();
//Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
//if (healingPose != null) {
// follower.setPose(healingPose);
//}
stateMachine.update(false, false, currentPose);
handleGlobalLogic();
handleDriverControls();
stateMachine.update(false, false, currentPose);
updateTelemetry(currentPose);
}
@@ -159,9 +161,8 @@ public class SOLOTeleOP {
}
if (stateMachine.getCurrentGameState() != GameState.SCORING) {
if (!stateMachine.getTurret().hasReachedVelocity()) {
int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
gamepad1.rumble(rumbleDuration);
if (!stateMachine.getTurret().hasReachedVelocity() && (gamepad1.timestamp % 100 < 10)) {
gamepad1.rumble(50);
}
}
@@ -195,14 +196,16 @@ public class SOLOTeleOP {
else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE);
}
if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.06);
if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06);
if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
if (gamepad1.aWasPressed()) {
stateMachine.forceLaunchSequence();
}
}
}
private void updateTelemetry(Pose p) {
telemetry.addData("OpMod ev", "SOLO TeleOp");
private void updateTelemetry(Pose p) {
telemetry.addData("OpMode", "SOLO TeleOp");
telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
@@ -212,13 +215,15 @@ public class SOLOTeleOP {
telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
telemetry.addData("Flywheel RPM", stateMachine.getTurret().returnFlywheelSpeed());
telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰");
telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));
telemetry.addData("Distance", "%.1f in", stateMachine.getTurret().getDistance());
telemetry.addData("Turret Ready", stateMachine.getTurret().isTurretReady());
telemetry.addData("Target Tick", stateMachine.getTurret().targetTick);
telemetry.update();
}
}
}
}
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java
View File

@@ -81,7 +81,7 @@ public class StateMachine {
public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
m_driveTrain.update();
m_turret.update(manualTracking, manualSAM, currentPose);
m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime());
if (currentGameState == GameState.SCORING) {
m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
@@ -94,10 +94,8 @@ public class StateMachine {
m_transfer.update();
m_led.update(
currentGameState == GameState.SCORING,
m_turret.hasReachedVelocity(),
m_turret.hasTarget(),
m_driveTrain.isAnchorActive()
m_turret.isTurretRotationLocked(),
m_turret.hasReachedVelocity()
);
if (currentGameState == GameState.SCORING) {
@@ -108,7 +106,7 @@ public class StateMachine {
}
public void initUpdate() {
m_led.update(false, false, false, false);
m_led.update(false, false);
}
private void handleGameStateEntry(GameState newState) {
@@ -182,6 +180,13 @@ public class StateMachine {
setGameState(GameState.IDLE);
}
public void forceLaunchSequence() {
setGameState(GameState.SCORING);
m_transfer.updateTurretReady(true);
m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
isIntakeLaunching = true;
}
public DriveTrain getDriveTrain() { return m_driveTrain; }
public Intake getIntake() { return m_intake; }
public Turret getTurret() { return m_turret; }

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

@@ -107,11 +107,6 @@ public class finalTeleOp {
handleOperatorInput();
stateMachine.update(manualSAM, manualTracking, currentPose);
//Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
// if (healingPose != null) {
// follower.setPose(healingPose);
// }
if (gamepad2.a) {
stateMachine.getTransfer().openLimiter();
@@ -124,6 +119,7 @@ public class finalTeleOp {
updateTelemetry(currentPose);
sleep(fpsCounter.getSyncTime(100));
}
stateMachine.setRobotState(RobotState.ESTOP);
@@ -168,9 +164,8 @@ public class finalTeleOp {
stateMachine.emergencyStop();
}
if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) {
int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
gamepad2.rumble(rumbleDuration);
if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity() && (gamepad2.timestamp % 100 < 10)) {
gamepad2.rumble(50);
}
if (gamepad1.psWasPressed()) {
@@ -218,8 +213,8 @@ public class finalTeleOp {
if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
}
private void handleOperatorInput() {

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java
View File

@@ -20,15 +20,16 @@ public class GetDistanceTuning extends OpMode {
private Limelight3A limelight;
private double distance;
public static double CAMERA_HEIGHT_INCHES = 12.5;
public static double CAMERA_HEIGHT_INCHES = 12.75;
public static double GOAL_HEIGHT_INCHES = 29.5;
public static int pipeline = 1;
public static double CAMERA_MOUNT_ANGLE_DEGREES = 14.3;
public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8;
public static double TARGET_DISTANCE = 0.0;
private TelemetryManager telemetryM;
@Override
public void init() {
limelight = hardwareMap.get(Limelight3A.class, "limelight");
@@ -49,14 +50,22 @@ public class GetDistanceTuning extends OpMode {
double targetY = result.getTy();
double targetArea = result.getTa();
if (TARGET_DISTANCE != 0) {
double requiredAngleRadians = Math.atan((GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / TARGET_DISTANCE);
CAMERA_MOUNT_ANGLE_DEGREES = Math.toDegrees(requiredAngleRadians) - targetY;
telemetryM.debug("Mode", "Angle Auto-Tuning (Diff Mode)");
} else {
telemetryM.debug("Mode", "Standard");
}
distance = getTrigBasedDistance(targetY);
double areaDistance = getDistanceFromTag(targetArea);
telemetryM.debug("Method", "Trigonometry");
telemetryM.debug("Target Y (ty)", targetY);
telemetryM.debug("Calculated Distance", distance);
telemetryM.debug("Camera Mount Angle", CAMERA_MOUNT_ANGLE_DEGREES);
telemetryM.debug("Area Algo Distance", areaDistance);
@@ -71,13 +80,7 @@ public class GetDistanceTuning extends OpMode {
}
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + targetOffsetAngleVertical));
// Calculate distance
//double distanceFromLimelightToGoalInches = (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
//return distanceFromLimelightToGoalInches;
}
private double getDistanceFromTag(double x) {

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

@@ -30,11 +30,11 @@ public class PIDFTuning extends OpMode {
public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
flywheelMotorR.setDirection(DcMotorSimple.Direction.FORWARD);
flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
flywheelMotorL = hardwareMap.get(DcMotorEx.class, "lturret");
flywheelMotorL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
telemetry.addLine("Init complete");
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java
View File

@@ -27,8 +27,8 @@ public class TurretRotationTuning extends LinearOpMode {
public static double MAX_INTEGRAL = 0.5;
// --- MECHANICAL CONSTANTS ---
public static double TICKS_PER_REV_MOTOR = 384.5;
public static double EXTERNAL_GEAR_RATIO = 2.68888889;
public static double TICKS_PER_REV_MOTOR = 145.1;
public static double EXTERNAL_GEAR_RATIO = 9.179875;
public static boolean MOTOR_REVERSED = true;
// --- CALIBRATION SETTINGS ---
@@ -117,8 +117,6 @@ public class TurretRotationTuning extends LinearOpMode {
break;
}
sleep(fps.getSyncTime(targetFPS));
// --- TELEMETRY ---
telemetryM.debug("▰▰▰▰▰ MODE ▰▰▰▰▰", currentMode.toString());
telemetryM.debug("Gear Ratio", EXTERNAL_GEAR_RATIO);
@@ -128,6 +126,8 @@ public class TurretRotationTuning extends LinearOpMode {
telemetryM.debug("Target Deg", targetTicks / ticksPerDegree);
telemetry.addData("FPS", fps.getAverageFps());
telemetryM.update(telemetry);
sleep(fps.getSyncTime(targetFPS));
}
limelight.stop();
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java
View File

@@ -336,7 +336,7 @@ public class allInOne extends LinearOpMode {
double ta = result.getTa();
calculatedDistance = getDistanceFromTag(ta);
calculatedVelocity = getFlywheelVelocity(calculatedDistance);
calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
calculatedHoodPos = getHoodPOS(calculatedDistance);
if (launch) {
leftTurret.setVelocity(calculatedVelocity);
@@ -459,7 +459,7 @@ public class allInOne extends LinearOpMode {
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);
private double getHoodPOS(double dist) {
return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
}
}

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

@@ -83,8 +83,6 @@ 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());
@@ -94,6 +92,8 @@ public class anchorTuning extends LinearOpMode {
telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError());
telemetryM.update(telemetry);
sleep(fps.getSyncTime(targetFPS));
}
}
}

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

@@ -223,8 +223,8 @@ public class autoCollection extends LinearOpMode {
break;
}
sleep(fpsCounter.getSyncTime(targetFPS));
updateTelemetry(currentTicks, currentDegrees);
sleep(fpsCounter.getSyncTime(targetFPS));
}
limelight.stop();
}

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

@@ -5,6 +5,7 @@ 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.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@@ -12,26 +13,46 @@ 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 flywheelTuning extends OpMode {
public enum CalibrationState {
IDLE,
SPOOLING,
TRACKING,
TUNING
}
private Limelight3A limelight;
public DcMotorEx flywheelMotorR;
public DcMotorEx flywheelMotorL;
public Servo hoodServo;
public DcMotor intake;
public Servo hoodServo;
public Servo limiter;
// Configurable / Dashboard variables
public static boolean autoMode = false;
public static double curTargetVelocity = 0;
public static double targetPOS = 0;
public static int pipeline = 1;
public static boolean reverse = false;
// Standard runtime variables
double distance = 0;
// State machine and tracking variables
CalibrationState calibState = CalibrationState.IDLE;
boolean lastX = false;
double maxError = 0.0;
double snapshotAutoHood = 0.0;
double calculatedGain = 0.0;
@Override
public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
@@ -43,45 +64,168 @@ public class flywheelTuning extends OpMode {
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();
intake = hardwareMap.get(DcMotor.class, "intake");
intake.setDirection(DcMotorSimple.Direction.FORWARD);
intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
limiter = hardwareMap.servo.get("limiter");
limiter.setDirection(Servo.Direction.FORWARD);
limiter.setPosition(1);
telemetry.addLine("Init complete");
}
@Override
public void loop() {
limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult();
if (result.isValid()) {
if (result != null && result.isValid()) {
double targetY = result.getTy();
telemetry.addData("Ty", targetY);
distance = getTrigBasedDistance(targetY);
}
if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
flywheelMotorL.setVelocity(curTargetVelocity);}
boolean currentX = gamepad1.x;
if (currentX && !lastX) {
switch (calibState) {
case IDLE:
maxError = 0.0;
calculatedGain = 0.0;
calibState = CalibrationState.SPOOLING;
break;
case SPOOLING:
calibState = CalibrationState.TRACKING;
break;
case TRACKING:
snapshotAutoHood = getHoodPOS(distance);
calibState = CalibrationState.TUNING;
break;
case TUNING:
calibState = CalibrationState.IDLE;
break;
}
}
lastX = currentX;
hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
double applyVel = 0;
double applyHood = 0;
double currentVel = flywheelMotorR.getVelocity();
double autoTargetVel = getFlywheelVelocity(distance);
double autoHoodPos = getHoodPOS(distance);
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);
if (autoMode) {
switch (calibState) {
case IDLE:
applyVel = autoTargetVel;
applyHood = autoHoodPos;
break;
case SPOOLING:
applyVel = autoTargetVel;
applyHood = autoHoodPos;
if (Math.abs(currentVel - autoTargetVel) <= 30) {
calibState = CalibrationState.TRACKING;
}
break;
case TRACKING:
applyVel = autoTargetVel;
applyHood = autoHoodPos;
double currentError = currentVel - autoTargetVel;
if (currentError < maxError) {
maxError = currentError;
}
break;
case TUNING:
applyVel = autoTargetVel + maxError;
applyHood = targetPOS;
if (maxError != 0) {
calculatedGain = (targetPOS - snapshotAutoHood) / maxError;
} else {
calculatedGain = 0;
}
break;
}
} else {
applyVel = curTargetVelocity;
applyHood = targetPOS;
}
if (!gamepad1.b) {
flywheelMotorR.setVelocity(applyVel);
flywheelMotorL.setVelocity(applyVel);
} else {
flywheelMotorR.setVelocity(0);
flywheelMotorL.setVelocity(0);
}
if (gamepad1.a) {
intake.setPower(1);
limiter.setPosition(0.82);
} else {
limiter.setPosition(1);
intake.setPower(0);
}
hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
// --- TELEMETRY ---
telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL");
telemetry.addData("Calibration State", calibState.toString());
telemetry.addLine();
telemetry.addData("Current Velocity", "%.2f", currentVel);
telemetry.addData("Applied Target Vel", "%.2f", applyVel);
telemetry.addData("Applied Target Hood", "%.4f", applyHood);
telemetry.addData("Distance", "%.2f inches", distance);
telemetry.addLine();
if (autoMode) {
telemetry.addData("Raw Auto Target Vel", "%.2f", autoTargetVel);
telemetry.addData("Raw Auto Target Hood", "%.4f", autoHoodPos);
telemetry.addLine("--- CALIBRATION DATA ---");
telemetry.addData("Max RPM Drop (Error)", "%.2f", maxError);
telemetry.addData("Snapshot Auto Hood", "%.4f", snapshotAutoHood);
telemetry.addData("User Tuning Hood (targetPOS)", "%.4f", targetPOS);
telemetry.addData("CALCULATED GAIN", "%.8f", calculatedGain);
telemetry.addLine("NOTE: When done tuning, copy CALCULATED GAIN into your snippet!");
}
telemetry.update();
}
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
return (17.1) / Math.tan(Math.toRadians(7.9 + targetOffsetAngleVertical));
return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical));
// ^ GOAL - CAMERA HEIGHT ^ Offset Degrees
}
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) {
return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
}
}

105
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java
View File

@@ -23,38 +23,40 @@ import java.util.List;
@TeleOp(name = "Odo Turret Tracking", group = "Turret")
public class odoTracking extends LinearOpMode {
public static double kP = 0.02;
public static double kI = 0.0;
public static double kD = 0.001;
public static double kP = 0.0040;
public static double kI = 0.002;
public static double kD = 0.0004;
public static double MAX_POWER = 0.6;
public static double MAX_INTEGRAL = 0.5;
// ================= Hardware Constants =================
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 TICKS_PER_REV_MOTOR = 145.1;
public static double EXTERNAL_GEAR_RATIO = 9.179875;
public static boolean MOTOR_REVERSED = false;
// ================= Limits & Logic =================
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 LL_LOGIC_MULTIPLIER = -1.0;
public static double SOFT_LIMIT_NEG = -330;
public static double SOFT_LIMIT_POS = 350;
// ================= Targeting Offsets =================
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 RED_TARGET_OFFSET_DEGREES = 0;
public static double BLUE_TARGET_OFFSET_DEGREES = 0;
public static double LL_TARGET_OFFSET_DEGREES = 0;
// ================= Field Coordinates =================
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 GOAL_RED_X = 140;
public static double GOAL_RED_Y = 140;
public static double GOAL_BLUE_X = 4;
public static double GOAL_BLUE_Y = 140;
// ================= Sensor Fusion Settings =================
public static double OFFSET_SMOOTHING = 0.2;
public static int targetFPS = 100;
public static double TURRET_FULL_WRAP_TICKS = 1340.0;
// ================= Hardware Objects =================
private Limelight3A limelight;
private DcMotorEx turretMotor;
@@ -68,7 +70,6 @@ public class odoTracking extends LinearOpMode {
private double targetCorrectionOffsetTicks = 0;
private double fusedTargetTicks = 0;
private enum TuningMode { CALIBRATION, TRACKING }
private TuningMode currentMode = TuningMode.CALIBRATION;
@@ -80,7 +81,7 @@ public class odoTracking extends LinearOpMode {
protected FPSCounter fpsCounter = new FPSCounter();
public static double start_x = 72;
public static double start_y = 8.5;
public static double start_y = 72;
public static double start_heading = 90;
@@ -157,48 +158,65 @@ public class odoTracking extends LinearOpMode {
break;
}
sleep(fpsCounter.getSyncTime(targetFPS));
updateTelemetry(currentTicks, currentDegrees);
sleep(fpsCounter.getSyncTime(targetFPS));
}
limelight.stop();
}
private void calculateHybridTarget(double currentTicks, double currentDegrees) {
double odoTargetTicks = calculateOdometryTargetTicks();
Pose robotPose = follower.getPose();
double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid()) {
double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
// 1. Calculate Vision Truth: Where the goal sits based on the camera right now
double tx = result.getTx();
double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES;
double visionTargetTicks = finalVisionDegrees * ticksPerDegree;
targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING))
+ (rawErrorTicks * OFFSET_SMOOTHING);
// 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
// 3. Calculate Correction Error: The delta between our current belief and camera reality
double correctionError = visionTargetTicks - currentFusedTarget;
// 4. Normalize the error for wrapping (360 degrees)
double ticksPerRev = 360.0 * ticksPerDegree;
while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev;
while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
// 5. Apply the error to the persistent offset (Bias-Shift)
// We use OFFSET_SMOOTHING as a gain to prevent single-frame noise from causing jitter
targetCorrectionOffsetTicks += (correctionError * OFFSET_SMOOTHING);
}
double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
// 1. Apply initial soft limits
double preWrapTarget = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
// 2. Implement wrapping logic: if target exceeds 180 degrees (670 ticks), normalize to the other side.
// This implies a full wrap cycle is 2 * 670 = 1340 ticks.
double fullWrapTicks = TURRET_FULL_WRAP_TICKS;
double halfWrapTicks = fullWrapTicks / 2.0;
// If target is beyond positive half-wrap threshold, wrap to negative equivalent
if (preWrapTarget > halfWrapTicks) {
preWrapTarget -= fullWrapTicks;
}
// If target is beyond negative half-wrap threshold, wrap to positive equivalent
else if (preWrapTarget < -halfWrapTicks) {
preWrapTarget += fullWrapTicks;
}
// 3. Apply final soft limits after wrapping
fusedTargetTicks = Range.clip(preWrapTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
}
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 double calculateOdometryTargetTicks() {
Pose robotPose = follower.getPose();
private double calculateOdometryTargetTicks(Pose robotPose) {
double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
@@ -237,6 +255,7 @@ public class odoTracking extends LinearOpMode {
double output = (kP * error) + integralTerm + (kD * derivative);
output = Range.clip(output, -MAX_POWER, MAX_POWER);
telemetry.addData("Power", output);
turretMotor.setPower(output);
lastError = error;

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java
View File

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.util.ElapsedTime;
public class FPSCounter {
private ElapsedTime loopTimer = new ElapsedTime();
private final ElapsedTime loopTimer = new ElapsedTime();
private double loopTimeSum = 0;
private int loopCount = 0;
private double currentLoopTime = 0;
@@ -13,6 +13,9 @@ public class FPSCounter {
reset();
}
/**
* Call this at the very beginning of your while(opModeIsActive) loop.
*/
public void startLoop() {
double now = loopTimer.milliseconds();
if (loopCount > 0) {
@@ -23,10 +26,17 @@ public class FPSCounter {
loopCount++;
}
/**
* Calculates the time remaining to hit the target FPS.
* To be accurate, call this at the very end of your loop,
* AFTER telemetry.update().
*/
public long getSyncTime(int targetFps) {
if (targetFps <= 0) return 0;
double targetMs = 1000.0 / targetFps;
double elapsedWorkMs = loopTimer.milliseconds() - lastTime;
double now = loopTimer.milliseconds();
double elapsedWorkMs = now - lastTime;
double remainingMs = targetMs - elapsedWorkMs;
return (remainingMs > 0) ? (long) remainingMs : 0;
@@ -57,4 +67,4 @@ public class FPSCounter {
loopCount = 0;
currentLoopTime = 0;
}
}
}

9
gradle.properties
View File

@@ -7,7 +7,12 @@ android.useAndroidX=true
android.enableJetifier=false
# Allow Gradle to use up to 1 GB of RAM
org.gradle.jvmargs=-Xmx1024M
org.gradle.jvmargs=-Xmx2048M
android.nonTransitiveRClass=false
org.gradle.configuration-cache=true
org.gradle.configuration-cache=true
org.gradle.parallel=true
org.gradle.caching=true
org.gradle.daemon=true