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

pre-spring break

Browse Source
This commit is contained in:
Arkm20
2026-03-14 16:16:49 -05:00
parent d3ec25b8dc
commit 22dd346d26
13 changed files with 292 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,6 +15,10 @@ import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.SwitchableLight;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import com.qualcomm.robotcore.hardware.PwmControl;
import org.firstinspires.ftc.teamcode.teleOp.Constants;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@@ -35,10 +39,24 @@ public class hwMap {
}
public static class LedHwMap {
public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led;
private ServoImplEx led;
public LedHwMap(HardwareMap hardwareMap) {
//led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED);
led = hardwareMap.get(ServoImplEx.class, Constants.LEDConstants.LED);
led.setPwmRange(new PwmControl.PwmRange(500, 2500));
}
public void setLedPos(double pos) {
led.setPosition(pos);
}
public void setLedOn(boolean yes) {
if (yes) {
led.setPwmEnable();
} else {
led.setPwmDisable();
}
}
}
@@ -248,7 +266,7 @@ public class hwMap {
private double lastKnownDistance = 0.0;
private ElapsedTime noTargetTimer = new ElapsedTime();
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15);
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(10, 0, 11, 14.3);
public TurretHwMap(HardwareMap hardwareMap) {
noTargetTimer.reset();

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

@@ -5,6 +5,7 @@ import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.math.Vector;
import com.pedropathing.paths.PathChain;
import com.pedropathing.util.Timer;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -116,7 +117,7 @@ public class Auton {
@Override
public void buildPaths() {
startPose = new Pose(31.5, 137, Math.toRadians(180));
startPose = new Pose(21, 124, Math.toRadians(143.5));
follower.setStartingPose(startPose);
// Waypoints
@@ -124,9 +125,9 @@ public class Auton {
Pose set2DrivePose = new Pose(39, 60, Math.toRadians(180));
Pose set2PickPose = new Pose(14, 60, Math.toRadians(180)); // tangential
Pose set2ScorePose = new Pose(54, 90, Math.toRadians(144));
Pose gate1PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
Pose gate1PickPose = new Pose(9, 60, Math.toRadians(143.5));
Pose gate1ScorePose = new Pose(54, 90, Math.toRadians(144));
Pose gate2PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
Pose gate2PickPose = new Pose(9, 60, Math.toRadians(143.5));
Pose gate2ScorePose = new Pose(54, 90, Math.toRadians(144));
Pose set3DrivePose = new Pose(38.515, 47.156, Math.toRadians(220));
Pose set3PickPose = new Pose(16.419, 35.945, Math.toRadians(185));
@@ -137,7 +138,6 @@ public class Auton {
Pose parkTargetPose = new Pose(36.219, 93.170, Math.toRadians(148));
// Controls
Pose preloadControl = new Pose(27.400, 115.541);
Pose set2ScoreControl = new Pose(50.386, 78.923);
Pose gate1PickControl = new Pose(40.119, 55.595);
Pose gate1ScoreControl = new Pose(41.918, 62.187);
@@ -148,7 +148,7 @@ public class Auton {
Pose set1PickControl = new Pose(14.863, 77.692);
scorePreloadPath = follower.pathBuilder()
.addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
.addPath(new BezierLine(startPose, preloadScorePose))
.setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
.build();
@@ -319,7 +319,7 @@ public class Auton {
@Override
public void buildPaths() {
startPose = new Pose(112.5, 137, Math.toRadians(0));
startPose = new Pose(122, 125.5, Math.toRadians(36));
follower.setStartingPose(startPose);
// Waypoints
@@ -327,9 +327,9 @@ public class Auton {
Pose set2DrivePose = new Pose(105, 60, Math.toRadians(0));
Pose set2PickPose = new Pose(130, 60, Math.toRadians(0)); // tangential
Pose set2ScorePose = new Pose(90, 90, Math.toRadians(36));
Pose gate1PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
Pose gate1PickPose = new Pose(132.7, 56, Math.toRadians(36));
Pose gate1ScorePose = new Pose(90, 90, Math.toRadians(36));
Pose gate2PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
Pose gate2PickPose = new Pose(132.7, 56, Math.toRadians(36));
Pose gate2ScorePose = new Pose(90, 90, Math.toRadians(36));
Pose set3DrivePose = new Pose(105.485, 47.156, Math.toRadians(-40));
Pose set3PickPose = new Pose(127.581, 35.945, Math.toRadians(-5));
@@ -442,8 +442,8 @@ public class Auton {
protected final boolean isNet;
protected final boolean useTimer;
public static double timerTimeout = 4;
public static double gateWaitTime = 3.0;
public static double timerTimeout = 4.5;
public static double gateWaitTime = 4.0;
protected Pose startPose;
@@ -517,8 +517,6 @@ 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);
@@ -538,7 +536,7 @@ public class Auton {
turretHw.resetTurret();
waitForStart();
systems.update(allianceTarget, follower.getPose());
systems.update(allianceTarget, follower.getPose(), new Vector(0, 0));
opmodeTimer.resetTimer();
setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
@@ -548,7 +546,7 @@ public class Auton {
}
follower.update();
statePathUpdate();
systems.update(allianceTarget, follower.getPose());
systems.update(allianceTarget, follower.getPose(), follower.getVelocity());
telemetry.addData("Path State", pathState);
telemetry.addData("System State", systems.getCurrentState());

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java
View File

@@ -137,7 +137,7 @@ public abstract class TimeAuton extends LinearOpMode {
timer.reset();
while(opModeIsActive() && timer.seconds() < 2.0) {
updateSystems();
if(turret.isTurretReady()) break;
if(turret.isTurretReady(true)) break;
}
@@ -167,13 +167,13 @@ public abstract class TimeAuton extends LinearOpMode {
AutoTransfer.updatePose(currentPose);
// Update Turret with current Pose
turret.updateAuton(alliance, currentPose, 0.2);
turret.updateAuton(alliance, currentPose, follower.getVelocity(), 0.2);
transfer.updateTurretReady(turret.isTurretReady());
transfer.updateTurretReady(turret.isTurretReady(true));
transfer.update();
// Telemetry for debugging
telemetry.addData("Turret Ready", turret.isTurretReady());
telemetry.addData("Turret Ready", turret.isTurretReady(false));
telemetry.addData("Transfer State", transfer.getTransferState());
telemetry.update();
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java
View File

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.auton;
import com.pedropathing.geometry.Pose;
import com.pedropathing.math.Vector;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; // Unused but kept for reference
@@ -88,8 +89,8 @@ public class pedroStateMachine {
}
}
public void update(int allianceTarget, Pose currentPose) {
m_turret.updateAuton(allianceTarget, currentPose, 0.2);
public void update(int allianceTarget, Pose currentPose, Vector currentVelocity) {
m_turret.updateAuton(allianceTarget, currentPose, currentVelocity, 0.2);
if (currentState == AutonState.SCORING) {
m_transfer.updateTurretReady(m_turret.hasReachedVelocity());

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java
View File

@@ -40,7 +40,7 @@ public class DriveTrain {
public enum DriveState {
NORMAL, TURBO, PRECISION, STOP
}
private DriveState currentState = DriveState.NORMAL;
private DriveState currentState = DriveState.TURBO;
private double speedMultiplier = 1.0;
public DriveTrain(hwMap.DriveHwMap hardware) {
@@ -59,8 +59,8 @@ public class DriveTrain {
if(dt == 0) dt = 0.001;
lastLoopTime = currentTime;
double currentFwd = h_driveTrain.backRightMotor.getCurrentPosition();
double currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition();
double currentFwd = h_driveTrain.frontLeftMotor.getCurrentPosition();
double currentStrafe = h_driveTrain.frontRightMotor.getCurrentPosition();
double currentHeading = getHeadingDegrees();
// 2. Calculate Errors
@@ -72,8 +72,6 @@ public class DriveTrain {
while (errorTurn > 180) errorTurn -= 360;
while (errorTurn < -180) errorTurn += 360;
// 3. PID Calculations
// Forward
sumFwd += errorForward * dt;
double dFwd = (errorForward - lastFwdErr) / dt;

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

@@ -1,36 +1,20 @@
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 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);
public LED(hwMap.LedHwMap hardware) {
this.hardware = hardware;
int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS;
solidRed.setBrightness(LED_BRIGHTNESS);
solidOrange.setBrightness(LED_BRIGHTNESS);
solidYellow.setBrightness(LED_BRIGHTNESS);
solidGreen.setBrightness(LED_BRIGHTNESS);
turnOn();
}
public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) {
/*int newLedState = 0;
int newLedState = 0;
if (isTurretLocked && isFlywheelUpToSpeed) {
newLedState = 1;
@@ -42,15 +26,25 @@ public class LED {
if (newLedState != currentLedState) {
if (newLedState == 0) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
hardware.setLedPos(Constants.LEDConstants.COLOR_RED);
} else if (newLedState == 1) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
hardware.setLedPos(Constants.LEDConstants.COLOR_GREEN);
} else if (newLedState == 2) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
hardware.setLedPos(Constants.LEDConstants.COLOR_ORANGE);
} else if (newLedState == 3) {
hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow);
hardware.setLedPos(Constants.LEDConstants.COLOR_YELLOW);
}
currentLedState = newLedState;
}*/
}
}
public void turnOff() {
hardware.setLedOn(false);
currentLedState = -1;
}
public void turnOn() {
hardware.setLedOn(true);
}
}

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

@@ -7,6 +7,8 @@ import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.teleOp.Constants;
import com.pedropathing.math.Vector;
public class Turret {
private final hwMap.TurretHwMap hardware;
@@ -24,11 +26,14 @@ public class Turret {
private double targetCorrectionOffsetTicks = 0;
private double fusedTargetTicks = 0;
public double turretPowerRotation;
public static double offsetTicks = 0.0;
private double offsetTicks = 0.0;
public double offsetHood = 0;
private Pose robotPose;
private Vector robotVelocity = new Vector(0,0); // NEW
private double kinematicDistanceOffset = 0.0; // NEW
private double kinematicAngularOffsetTicks = 0.0; // NEW
public double targetTick;
@@ -59,6 +64,7 @@ public class Turret {
if (this.alliance != alliance) {
this.alliance = alliance;
targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
offsetTicks = 0; // Reset manual offset when alliance changes
if (alliance == 2) {
hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
} else if (alliance == 1) {
@@ -108,17 +114,17 @@ public class Turret {
return hardware.hasTarget();
}
public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) {
public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, Vector currentVelocity, double elapsedTime) {
double dt = loopTimer.seconds();
loopTimer.reset();
if (dt < 0.001) dt = 0.001;
updatePose(currentPose);
updatePose(currentPose, currentVelocity);
double rawTicks = hardware.getTurretRotationPosition();
double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
distance = getDistance();
distance = getDistance() + kinematicDistanceOffset;
if (!manualTracking) {
calculateHybridTarget(currentPose, currentDegrees, rawTicks);
@@ -131,19 +137,21 @@ public class Turret {
handleLaunchLogic(elapsedTime);
}
else {
// idleLaunch();
idleLaunch();
}
}
public void idleLaunch() {
hardware.setTurretVelocity(1000);
hardware.setTurretVelocity(1300);
double calculatedHoodPos = getHoodPOS(65);
setHoodPos(calculatedHoodPos);
}
public void updateAuton(int alliance, Pose currentPose, double offset) {
updatePose(currentPose);
public void updateAuton(int alliance, Pose currentPose, Vector currentVelocity, double offset) {
updatePose(currentPose, currentVelocity); // Updated
double dt = loopTimer.seconds();
loopTimer.reset();
if (dt < 0.001) dt = 0.001;
@@ -158,11 +166,11 @@ public class Turret {
double rawTicks = hardware.getTurretRotationPosition();
double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
distance = getDistance();
distance = getDistance() + kinematicDistanceOffset;
calculateHybridTarget(currentPose, currentDegrees, rawTicks);
runPIDControl(rawTicks, dt);
handleLaunchLogicOffset(-offset);
handleLaunchLogicOffset(0);
}
private void handleLaunchLogic(double elapsedTime) {
@@ -170,11 +178,6 @@ public class Turret {
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;
@@ -190,7 +193,7 @@ public class Turret {
double calculatedHoodPos = getHoodPOS(distance);
double currentVel = hardware.getFlywheelVelocities()[1];
double recoilOffset = (currentVel - targetVelocity) * -0.000185;
double recoilOffset = (currentVel - targetVelocity) * -0.000385;
setHoodPos(calculatedHoodPos + recoilOffset - 0.03);
} else {
@@ -218,7 +221,7 @@ public class Turret {
double calculatedHoodPos = getHoodPOS(distance);
double currentVel = hardware.getFlywheelVelocities()[1];
double recoilOffset = (currentVel - targetVelocity) * -0.000185;
double recoilOffset = (currentVel - targetVelocity) * -0.000385;
setHoodPos(calculatedHoodPos + recoilOffset);
} else {
@@ -226,21 +229,27 @@ public class Turret {
}
}
private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
LLResult result = hardware.getLimelightResult();
if (result != null && result.isValid()) {
// 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)
visionTargetTicks += kinematicAngularOffsetTicks;
// Shift the vision target by our manual offset so LL doesn't fight it
double visionTargetWithManualOffset = visionTargetTicks + offsetTicks;
double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
double correctionError = visionTargetTicks - currentFusedTarget;
double correctionError = visionTargetWithManualOffset - currentFusedTarget;
// 4. Normalize the error for wrapping (360 degrees)
double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
@@ -277,21 +286,19 @@ public class Turret {
while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
double relativeDegrees = Math.toDegrees(relativeRad);
double staticOffset = (alliance == 2) ? Constants.TurretConstants.RED_TARGET_OFFSET_DEGREES : Constants.TurretConstants.BLUE_TARGET_OFFSET_DEGREES;
return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE;
// APPLY THE KINEMATIC SHIFT
return ((relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE) + kinematicAngularOffsetTicks;
}
private void runPIDControl(double currentTicks, double dt) {
fusedTargetTicks = fusedTargetTicks + offsetTicks;
double error = fusedTargetTicks - currentTicks;
double derivative = (error - lastError) / dt;
// 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
double entryThreshold = 10;
double exitThreshold = 30;
if (!turretModeHold && Math.abs(error) <= entryThreshold) {
turretModeHold = true;
@@ -302,7 +309,7 @@ public class Turret {
if (turretModeHold) {
if (lastTurretModeHold != turretModeHold) {
hardware.runToPos((int) fusedTargetTicks); // Set target first
hardware.enableTurretHold(); // Then set mode
hardware.enableTurretHold();
hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power
}
hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode
@@ -337,20 +344,60 @@ public class Turret {
double absTarget = Math.abs(targetVelocity);
double error = Math.abs(absCurrent - absTarget);
double tolerance = absTarget * 0.03;
double tolerance = absTarget * 0.05;
return error <= tolerance;
}
return false;
}
public boolean isTurretReady() {
return hasReachedVelocity() && isTurretRotationLocked();
public boolean isTurretReady(boolean isAuton) {
if (!isAuton) return hasReachedVelocity() && isTurretRotationLocked();
else return hasReachedVelocity();
}
public void updatePose(Pose robotPose) {
public void updatePose(Pose robotPose) { // Fallback if called somewhere else without velocity
updatePose(robotPose, new Vector(0,0));
}
public void updatePose(Pose robotPose, Vector robotVelocity) {
this.robotPose = robotPose;
this.robotVelocity = robotVelocity != null ? robotVelocity : new Vector(0,0);
calculateKinematicOffsets();
}
private void calculateKinematicOffsets() {
if (robotPose == null || robotVelocity == null) {
kinematicDistanceOffset = 0;
kinematicAngularOffsetTicks = 0;
return;
}
double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X;
double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y;
double virtualX = targetX - (robotVelocity.getXComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT);
double virtualY = targetY - (robotVelocity.getYComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT);
double realDx = targetX - robotPose.getX();
double realDy = targetY - robotPose.getY();
double virtualDx = virtualX - robotPose.getX();
double virtualDy = virtualY - robotPose.getY();
double realDistance = Math.hypot(realDx, realDy);
double virtualDistance = Math.hypot(virtualDx, virtualDy);
kinematicDistanceOffset = virtualDistance - realDistance;
double realAngle = Math.atan2(realDy, realDx);
double virtualAngle = Math.atan2(virtualDy, virtualDx);
double angleDiff = virtualAngle - realAngle;
while (angleDiff > Math.PI) angleDiff -= (2 * Math.PI);
while (angleDiff < -Math.PI) angleDiff += (2 * Math.PI);
kinematicAngularOffsetTicks = Math.toDegrees(angleDiff) * Constants.TurretConstants.TICKS_PER_DEGREE;
}
public double getDistance() {
@@ -375,7 +422,11 @@ public class Turret {
}
private double getFlywheelVelocity(double dist) {
return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126);
return (-0.0000144121 * Math.pow(dist, 4)
+ 0.00398607 * Math.pow(dist, 3)
- 0.342198 * Math.pow(dist, 2)
+ (16.43014 * dist)
+ 1198.24434);
}
private double getHoodPOS(double dist) {
@@ -392,6 +443,7 @@ public class Turret {
public void updateOffsetTicks(double change) {
offsetTicks += change;
targetCorrectionOffsetTicks += change;
}
public double getTurretPower() {
@@ -414,4 +466,4 @@ public class Turret {
public boolean isTurretRotationLocked() {
return turretModeHold && Math.abs(lastError) <= 10;
}
}
}

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

@@ -40,8 +40,12 @@ public class Constants {
public static class LEDConstants {
public static final String LED = "prism";
public static final int LAYER_BRIGHTNESS = 50;
// TODO: Tune these!
public static final double COLOR_RED = 0.28;
public static final double COLOR_GREEN = 0.45;
public static final double COLOR_ORANGE = 0.32;
public static final double COLOR_YELLOW = 0.38;
}
@@ -93,7 +97,7 @@ public class Constants {
public static final double UNLOCK_POS = 0.82;
public static double launch_duration = 0.77;
public static double launch_duration = 0.9;
}
@@ -122,8 +126,8 @@ public class Constants {
public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD;
public static double SOFT_LIMIT_NEG = -324;
public static double SOFT_LIMIT_POS = 195;
public static double SOFT_LIMIT_NEG = -370;
public static double SOFT_LIMIT_POS = 370;
public static final double HOOD_POS_CLOSE = 0.3;
public static final double HOOD_POS_FAR = 0.7;
@@ -167,13 +171,15 @@ public class Constants {
// 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_2 = 45.0;
public static double ARTIFACT_3 = -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;
public static double SHOT_TIME_OF_FLIGHT = 0.83;
}

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

@@ -12,10 +12,15 @@ import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
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.pedropathing.math.Vector;
import com.qualcomm.robotcore.hardware.VoltageSensor;
public class SOLOTeleOP {
@TeleOp(name = "Red SOLO", group = "FINAL")
@@ -24,6 +29,7 @@ public class SOLOTeleOP {
super(2);
}
}
@TeleOp(name = "Blue SOLO", group = "FINAL")
public static class BlueSOLO extends BaseSoloTeleOp {
public BlueSOLO() {
@@ -96,8 +102,9 @@ public class SOLOTeleOP {
follower.update();
Pose currentPose = follower.getPose();
Vector currentVelocity = follower.getVelocity();
stateMachine.update(false, false, currentPose);
stateMachine.update(false, false, currentPose, currentVelocity);
handleGlobalLogic();
@@ -173,9 +180,12 @@ public class SOLOTeleOP {
private void handleDriverControls() {
if (gamepad1.dpad_up) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
else if (gamepad1.dpad_left) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
else if (gamepad1.dpad_down) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
if (gamepad1.dpad_up)
stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
else if (gamepad1.dpad_left)
stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
else if (gamepad1.dpad_down)
stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
if (stateMachine.getCurrentGameState() != GameState.SCORING) {
if (gamepad1.right_trigger > 0.1) {
@@ -193,11 +203,12 @@ public class SOLOTeleOP {
if (gamepad1.yWasPressed()) {
isScoringActive = !isScoringActive;
if (isScoringActive) stateMachine.setGameState(GameState.SCORING);
else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE);
else if (stateMachine.getCurrentGameState() == GameState.SCORING)
stateMachine.setGameState(GameState.IDLE);
}
if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(3);
if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-3);
if (gamepad1.aWasPressed()) {
stateMachine.forceLaunchSequence();
@@ -220,10 +231,10 @@ public class SOLOTeleOP {
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("Turret Ready", stateMachine.getTurret().isTurretReady(false));
telemetry.addData("Target Tick", stateMachine.getTurret().targetTick);
telemetry.update();
}
}
}
}

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

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.teleOp;
import com.pedropathing.geometry.Pose; // ADDED
import com.pedropathing.math.Vector;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.Hware.hwMap;
import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
@@ -29,11 +31,13 @@ public class StateMachine {
private final Lift m_lift;
private final LED m_led;
private final ElapsedTime ledTimer = new ElapsedTime();
boolean isMotifEdited = false;
private long lastIndexAllArtifactsTime = 0;
public boolean isIntakeLaunching = false;
private boolean isForceLaunch = false;
public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) {
this.m_driveTrain = new DriveTrain(h_driveTrain);
@@ -69,6 +73,7 @@ public class StateMachine {
m_turret.setTurretState(Turret.TurretState.IDLE);
m_intake.setIntakeState(Intake.IntakeState.IDLE);
isIntakeLaunching = false;
isForceLaunch = false;
//m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
m_driveTrain.stopAnchor();
break;
@@ -78,14 +83,17 @@ public class StateMachine {
}
}
public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
public void update(boolean manualSAM, boolean manualTracking, Pose currentPose, Vector currentVelocity) {
m_driveTrain.update();
m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime());
m_turret.update(manualTracking, manualSAM, currentPose, currentVelocity, m_transfer.getLaunchElapsedTime());
if (currentGameState == GameState.SCORING) {
if (!isForceLaunch) {
m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
if (m_turret.hasReachedVelocity()) {
}
if (m_turret.hasReachedVelocity() || isForceLaunch) {
isIntakeLaunching = true;
m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
}
@@ -182,6 +190,7 @@ public class StateMachine {
public void forceLaunchSequence() {
setGameState(GameState.SCORING);
isForceLaunch = true;
m_transfer.updateTurretReady(true);
m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
isIntakeLaunching = true;

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

@@ -15,6 +15,8 @@ import org.firstinspires.ftc.teamcode.util.FPSCounter;
import com.qualcomm.hardware.lynx.LynxModule;
import java.util.List;
import com.pedropathing.math.Vector;
public class finalTeleOp {
@TeleOp(name = "Red Final", group = "FINAL")
@@ -91,7 +93,7 @@ public class finalTeleOp {
if (stateMachine.getCurrentGameState() != GameState.SCORING) {
stateMachine.getTransfer().closeLimiter();
} else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady()) {
} else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady(false)) {
stateMachine.getTransfer().openLimiter();
}
@@ -101,12 +103,14 @@ public class finalTeleOp {
follower.update();
Pose currentPose = follower.getPose();
Vector currentVelocity = follower.getVelocity();
handleGlobalLogic();
handleDriverInput();
handleOperatorInput();
stateMachine.update(manualSAM, manualTracking, currentPose);
stateMachine.update(manualSAM, manualTracking, currentPose, currentVelocity);
if (gamepad2.a) {
stateMachine.getTransfer().openLimiter();
@@ -273,7 +277,7 @@ public class finalTeleOp {
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("Turret Ready", stateMachine.getTurret().isTurretReady(false));
telemetry.addData("Hood Offset", stateMachine.getTurret().offsetHood);

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

@@ -8,6 +8,7 @@ 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.Range;
@@ -35,17 +36,14 @@ public class flywheelTuning extends OpMode {
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;
@@ -63,6 +61,8 @@ public class flywheelTuning extends OpMode {
flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3));
flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3));
hoodServo = hardwareMap.servo.get("hoodservo");
hoodServo.setDirection(Servo.Direction.REVERSE);
@@ -184,7 +184,9 @@ public class flywheelTuning extends OpMode {
intake.setPower(0);
}
hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
double recoilOffset = (currentVel - applyVel) * -0.000185;
hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
// --- TELEMETRY ---
@@ -213,19 +215,19 @@ public class flywheelTuning extends OpMode {
}
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical));
return (29.5-12.75) / Math.tan(Math.toRadians(19.8 + 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);
return (-0.0000144121 * Math.pow(dist, 4)
+ 0.00398607 * Math.pow(dist, 3)
- 0.342198 * Math.pow(dist, 2)
+ (16.43014 * dist)
+ 1198.24434);
}
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);
return (((3.55794 * Math.pow(10, -8)) * Math.pow(dist, 4)) + (-0.00000995887 * Math.pow(dist, 3)) - (0.000924672 * Math.pow(dist, 2)) + (-0.037262 * dist) + 0.943067);
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ledTest.java Normal file
View File

@@ -0,0 +1,71 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import com.qualcomm.robotcore.hardware.PwmControl;
import com.bylazar.configurables.annotations.Configurable;
@Configurable
@TeleOp(name = "GoBilda RGB Indicator Test", group = "Tests")
public class ledTest extends OpMode {
// Declare the light as a ServoImplEx
private ServoImplEx rgbLight;
public static double LED1 = 0.33;
public static double LED2 = 0.66;
public static double LED3 = 0.99;
private boolean lightOn = false;
private double lightPosition = 0;
@Override
public void init() {
rgbLight = hardwareMap.get(ServoImplEx.class, "prism");
rgbLight.setPwmRange(new PwmControl.PwmRange(500, 2500));
telemetry.addData("Status", "Initialized. Ready to test colors.");
}
@Override
public void loop() {
if (gamepad1.psWasPressed()) {
lightOn = !lightOn;
}
if (gamepad1.a) {
lightPosition = LED1;
}
else if (gamepad1.b) {
lightPosition = 0;
}
else if (gamepad1.x) {
lightPosition = LED2;
}
else if (gamepad1.y) {
lightPosition = LED3;
}
if (gamepad1.right_trigger > 0.1) {
lightPosition = gamepad1.right_trigger;
}
if (lightOn) {
rgbLight.setPwmEnable();
} else {
rgbLight.setPwmDisable();
}
rgbLight.setPosition(lightPosition);
telemetry.addData("Current Light Position", rgbLight.getPosition());
telemetry.update();
}
}