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

a lot of stuff happened

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-07 21:19:00 -05:00
parent 104058bbce
commit 9d03e1125a
9 changed files with 236 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -33,6 +33,8 @@ public class TeleopV4 extends LinearOpMode {
public static Pose relocalizePose = new Pose(56, 11, 0);
public static Pose teleStart = new Pose(0,0,0);
private boolean firstTickFull = true;
@Override
public void runOpMode() throws InterruptedException {
@@ -77,6 +79,25 @@ public class TeleopV4 extends LinearOpMode {
TELE.addData("TELE START", teleStart);
TELE.update();
while (opModeInInit()){
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
}
TELE.addLine("Initialization is done");
TELE.addData("Starting Position", follower.getPose());
TELE.addData("TELE START", teleStart);
TELE.addData("Front?:", VelocityCommander.lockFront);
TELE.addData("Back?:", VelocityCommander.lockBack);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
@@ -86,7 +107,8 @@ public class TeleopV4 extends LinearOpMode {
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x
gamepad1.left_stick_x,
gamepad1.left_trigger
);
if (gamepad1.crossWasPressed()){
@@ -116,6 +138,23 @@ public class TeleopV4 extends LinearOpMode {
robot.setHoodPos(0.6);
}
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
TELE.addData("Front?:", VelocityCommander.lockFront);
TELE.addData("Back?:", VelocityCommander.lockBack);
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
TELE.addData("Front?:", VelocityCommander.lockFront);
TELE.addData("Back?:", VelocityCommander.lockBack);
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
TELE.addData("Front?:", VelocityCommander.lockFront);
TELE.addData("Back?:", VelocityCommander.lockBack);
}
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
@@ -130,6 +169,13 @@ public class TeleopV4 extends LinearOpMode {
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
SpindexerTransferIntake.intakeFull = false;
firstTickFull = true;
}
if (SpindexerTransferIntake.intakeFull && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
if (gamepad1.right_trigger > 0.5 &&
@@ -159,20 +205,20 @@ public class TeleopV4 extends LinearOpMode {
}
loopTimes.loop();
TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
// TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
// TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
// TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
//
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", robot.transfer.getPower());
// TELE.addData("Hood Position", commander.getHoodPredicted());
// TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("Current Position", currentPose);
TELE.addData("Current LL Pipeline", turret.pipeline());
//
// TELE.addData("Current Position", currentPose);
//
// TELE.addData("Current LL Pipeline", turret.pipeline());
//
TELE.update();
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
View File

@@ -81,9 +81,11 @@ public class NewShooterTest extends LinearOpMode {
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x
gamepad1.left_stick_x,
gamepad1.left_trigger
);
if (gamepad1.crossWasPressed()){
if (Color.redAlliance){
TeleopV4.relocalizePose = new Pose(57.5, 5, 0);
} else {
@@ -92,6 +94,7 @@ public class NewShooterTest extends LinearOpMode {
follower.setPose(TeleopV4.relocalizePose);
sleep(500);
gamepad1.rumble(100);
}
follower.update();
@@ -154,6 +157,7 @@ public class NewShooterTest extends LinearOpMode {
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", commander.getTransferPow());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Manuel Velocity RPM", flywheelVelo);
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("TX:", turret.getTX());

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java
View File

@@ -91,7 +91,8 @@ public class SortedSpindexerTest extends LinearOpMode {
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x
gamepad1.left_stick_x,
gamepad1.left_trigger
);
follower.update();

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
View File

@@ -37,7 +37,7 @@ public class Drivetrain {
tele = input;
}
public void drive(double y, double x, double rx) {
public void drive(double y, double x, double rx, double stop) {
boolean snappedForward = false;
boolean snappedStrafe = false;
@@ -79,18 +79,30 @@ public class Drivetrain {
robot.setFrontRightPower(frontRightPower);
robot.setBackRightPower(backRightPower);
if (tele) {
if (stop > 0.5){
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
telemetry.addData("Forward Snap", snappedForward);
telemetry.addData("Strafe Snap", snappedStrafe);
telemetry.addData("Correction RX", correctionRX);
telemetry.addData("FL", frontLeftPower);
telemetry.addData("BL", backLeftPower);
telemetry.addData("FR", frontRightPower);
telemetry.addData("BR", backRightPower);
robot.setFrontLeftPower(0);
robot.setBackLeftPower(0);
robot.setFrontRightPower(0);
robot.setBackRightPower(0);
}
}
// if (tele) {
//
// telemetry.addData("Forward Snap", snappedForward);
// telemetry.addData("Strafe Snap", snappedStrafe);
//
// telemetry.addData("Correction RX", correctionRX);
//
// telemetry.addData("FL", frontLeftPower);
// telemetry.addData("BL", backLeftPower);
// telemetry.addData("FR", frontRightPower);
// telemetry.addData("BR", backRightPower);
//
// }
}
}

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
View File

@@ -1,6 +1,8 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -16,10 +18,20 @@ public class Flywheel {
// public PIDFCoefficients shooterPIDF1, shooterPIDF2;
public static PIDFCoefficients shooterPIDF;
PIDFController pidf;
SimpleMotorFeedforward feedforward;
public static double kS = 0.01; // Static feedforward
public static double kV = 0.0001935; // Velocity feedforward
public static double shooterPIDF_P = 500;
public static double shooterPIDF_I = 1;
public static double shooterPIDF_D = 0.0;
public static double shooterPIDF_F = 93;
// public static double shooterPIDF_P = 0.0001;
// public static double shooterPIDF_I = 0;
// public static double shooterPIDF_D = 0.00001;
// public static double shooterPIDF_F = 0;
private double velo = 0.0;
private double velo1 = 0.0;
@@ -36,6 +48,8 @@ public class Flywheel {
public Flywheel(Robot rob) {
robot = rob;
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
// pidf = new PIDFController(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
// feedforward = new SimpleMotorFeedforward(kS, kV);
}
public double getVelo() {
@@ -67,19 +81,23 @@ public class Flywheel {
shooterPIDF.d = d;
shooterPIDF.f = f;
// pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
private double prevF = 0;
public static double voltagePIDFDifference = 1;
double averageVoltage = 0;
public void setF(double voltage){
averageVoltage = ALPHA * voltage + (1 - ALPHA) * averageVoltage;
double f = shooterPIDF_F / voltage;
if (Math.abs(prevF - f) > voltagePIDFDifference) {
if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
shooterPIDF.f = f;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
prevF = f;
}
prevF = f;
}
// Convert from RPM to Ticks per Second
@@ -92,25 +110,30 @@ public class Flywheel {
return (TPS * 60.0) / 28.0;
}
double ALPHA = 0.3;
private void updateVelocityAverage(double newVelocity) {
velocityHistory.add(newVelocity);
// velocityHistory.add(newVelocity);
//
// int velocityHistorySize = 5;
// if (velocityHistory.size() > velocityHistorySize) {
// velocityHistory.removeFirst();
// }
//
// double sum = 0.0;
//
// for (double v : velocityHistory) {
// sum += v;
// }
//
// averageVelocity = sum / velocityHistory.size();
int velocityHistorySize = 5;
if (velocityHistory.size() > velocityHistorySize) {
velocityHistory.removeFirst();
}
double sum = 0.0;
for (double v : velocityHistory) {
sum += v;
}
averageVelocity = sum / velocityHistory.size();
averageVelocity = ALPHA * newVelocity + (1 - ALPHA) * averageVelocity;
}
double power;
double prevTargetTime = 0;
double prevTargetVelocity = 0;
public void manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -125,13 +148,12 @@ public class Flywheel {
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = (velo1 + velo2) / 2.0;
velo = velo1;
updateVelocityAverage(velo);
steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
}
public double getShooterPower(){return power;}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
View File

@@ -104,7 +104,8 @@ public class Shooter {
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
voltage,
fly.getAverageVelocity()
);
fly.manageFlywheel(flywheelVelocity);
@@ -131,7 +132,8 @@ public class Shooter {
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
voltage,
fly.getAverageVelocity()
);
flywheelVelocity = commander.getPredictedRPM();
@@ -153,7 +155,8 @@ public class Shooter {
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
voltage,
fly.getAverageVelocity()
);
flywheelVelocity = commander.getPredictedRPM();
@@ -172,7 +175,8 @@ public class Shooter {
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
voltage,
fly.getAverageVelocity()
);
flywheelVelocity = commander.getPredictedRPM();
@@ -200,7 +204,8 @@ public class Shooter {
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
voltage,
fly.getAverageVelocity()
);
fly.manageFlywheel(flywheelVelocity);
fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -274,22 +274,23 @@ public class SpindexerTransferIntake {
private int greenTicks = 0;
private int ballTicks = 0;
private int holdBallsTicker = 0;
public static boolean intakeFull = true;
public void update() {
TELE.addData("Sorted State", sortedIntakeStates);
TELE.addData("Ball0", ballColors[0]);
TELE.addData("Ball1", ballColors[1]);
TELE.addData("Ball2", ballColors[2]);
TELE.addData("Shoot0", shootOrder[0]);
TELE.addData("Shoot1", shootOrder[1]);
TELE.addData("Shoot2", shootOrder[2]);
TELE.addData("Color0", ballColors[0]);
TELE.addData("Color1", ballColors[1]);
TELE.addData("Color2", ballColors[2]);
TELE.addData("Shoot State", shootState);
// TELE.addData("Sorted State", sortedIntakeStates);
// TELE.addData("Ball0", ballColors[0]);
// TELE.addData("Ball1", ballColors[1]);
// TELE.addData("Ball2", ballColors[2]);
//
// TELE.addData("Shoot0", shootOrder[0]);
// TELE.addData("Shoot1", shootOrder[1]);
// TELE.addData("Shoot2", shootOrder[2]);
//
// TELE.addData("Color0", ballColors[0]);
// TELE.addData("Color1", ballColors[1]);
// TELE.addData("Color2", ballColors[2]);
//
// TELE.addData("Shoot State", shootState);
switch (mode) {
@@ -317,9 +318,13 @@ public class SpindexerTransferIntake {
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
setRapidMode(RapidMode.TRANSFER_OFF);
holdBallsTicker++;
}
if (holdBallsTicker > 10){
setRapidMode(RapidMode.TRANSFER_OFF);
holdBallsTicker = 0;
}
break;
@@ -366,10 +371,12 @@ public class SpindexerTransferIntake {
case HOLD_BALLS:
if (robot.insideBeam.isPressed()
&& robot.outsideBeam.isPressed() && holdBallsTicker > 10) {
&& robot.outsideBeam.isPressed()) {
robot.setIntakePower(0.1);
robot.setTransferPower(0);
robot.setIntakePower(0.1);
robot.setTransferServoPos(ServoPositions.transferServo_in);
intakeFull = true;
} else {
holdBallsTicker++;
@@ -379,37 +386,42 @@ public class SpindexerTransferIntake {
case OPEN_GATE:
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
if (stateTime() >= 100) {
setRapidMode(RapidMode.SHOOT);
}
robot.setTransferServoPos(ServoPositions.transferServo_in);
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower);
robot.setIntakePower(-NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
robot.setIntakePower(-commander.getTransferPow());
}
break;
case SHOOT:
robot.setTransferServoPos(
ServoPositions.transferServo_in
);
if (stateTime() >= 400) {
if (stateTime() >= 500) {
setRapidMode(RapidMode.INTAKE);
}
if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower);
robot.setIntakePower(-NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
robot.setIntakePower(-commander.getTransferPow());
}
holdBallsTicker = 0;
break;
}
break;

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

@@ -20,7 +20,7 @@ public class Turret {
private final double turretMin = 0.05;
private final double turretMax = 0.95;
public static boolean limelightUsed = true;
public static double B_PID_P = 0.00015, B_PID_I = 0.0, B_PID_D = 0.00001;
public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
LLResult result;
PIDController bearingPID;
boolean bearingAligned = false;

112
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
View File

@@ -8,88 +8,71 @@ public class VelocityCommander {
public static double xAccK = 0.025; // TODO: Tune
public static double yVelK = 0.05; // TODO: Tune
public static double yAccK = 0.025; // TODO: Tune
public static boolean lockFront = false;
public static boolean lockBack = false;
public static int farBound = 140;
public static int closeBound = 110;
public static double errorHoodAdjustment = 0.0005;
private double hoodPos = 0.88;
private double transferPow = -1;
private int velo = 0;
public VelocityCommander() {}
private final double veloA = -2.703087757*Math.pow(10, -14);
private final double veloB = 2.904756341*Math.pow(10, -11);
private final double veloC = -1.381814293*Math.pow(10, -8);
private final double veloD = 0.000003829224585;
private final double veloE = -0.000684090204;
private final double veloF = 0.0822754689;
private final double veloG = -6.743119277;
private final double veloH = 371.7359504;
private final double veloI = -13189.70958;
private final double veloJ = 272005.7124;
private final double veloK = -2474581.713;
final double veloA = -0.00000133612;
final double veloB = 0.000542733;
final double veloC = -0.0739531;
final double veloD = 5.16759;
final double veloE = 62.45781;
private double distToRPM (double dist){
double currentVelo = 0;
if (dist < 49) {
currentVelo = 2000;
} else if (dist > 165){
velo = 3760;
} else {
currentVelo = veloA*Math.pow(dist, 10) +
veloB*Math.pow(dist, 9) +
veloC*Math.pow(dist, 8) +
veloD*Math.pow(dist, 7) +
veloE*Math.pow(dist, 6) +
veloF*Math.pow(dist, 5) +
veloG*Math.pow(dist, 4) +
veloH*Math.pow(dist, 3) +
veloI*Math.pow(dist, 2) +
veloJ*Math.pow(dist, 1) +
veloK;
double currentVelo;
if (lockFront && dist > closeBound){
dist = closeBound;
} else if (lockBack && dist < farBound){
dist = farBound;
}
if (dist < 54) {
velo = 2000;
} else if (dist > 181){
velo = 3600;
} else {
currentVelo = veloA*Math.pow(dist, 4) +
veloB*Math.pow(dist, 3) +
veloC*Math.pow(dist, 2) +
veloD*Math.pow(dist, 1) +
veloE;
velo = 10 * Math.round((float) Math.max(200, Math.min(360, currentVelo)));
}
velo = Math.round((float) Math.max(2000, Math.min(3760, currentVelo)));
return velo;
}
private final double hoodA = -4.3276177*Math.pow(10, -13);
private final double hoodB = 2.68062979*Math.pow(10, -10);
private final double hoodC = -7.12859632*Math.pow(10, -8);
private final double hoodD = 0.0000106010785;
private final double hoodE = -0.000960693973;
private final double hoodF = 0.0540375808;
private final double hoodG = -1.82724027;
private final double hoodH = 33.4797545;
private final double hoodI = -246.888632;
final double hoodA = 9.04203*Math.pow(10, -8);
final double hoodB = -0.0000204165;
final double hoodC = -0.00252089;
final double hoodD = 1.06154;
private void distToHood (double dist){
if (dist > 112){
hoodPos = 0.35;
} else if (dist < 49){
if (dist > 174){
hoodPos = 0.48;
} else if (dist < 54){
hoodPos = 0.88;
} else {
hoodPos = hoodA*Math.pow(dist, 8) +
hoodB*Math.pow(dist, 7) +
hoodC*Math.pow(dist, 6) +
hoodD*Math.pow(dist, 5) +
hoodE*Math.pow(dist, 4) +
hoodF*Math.pow(dist, 3) +
hoodG*Math.pow(dist, 2) +
hoodH*Math.pow(dist, 1) +
hoodI;
hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
hoodPos = hoodA*Math.pow(dist, 3) +
hoodB*Math.pow(dist, 2) +
hoodC*Math.pow(dist, 1) +
hoodD;
}
hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
}
public double getHoodPredicted(){
return hoodPos;
}
private void distToTransferPow(double dist, double voltage){
if (dist < 118){
transferPow = -1;
} else if (dist < 125){
transferPow = -0.7;
if (dist < 140){
transferPow = -0.8;
} else {
transferPow = -0.5;
}
// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
}
public double getTransferPow(){return transferPow;}
@@ -99,7 +82,7 @@ public class VelocityCommander {
}
double predictedDist = 0;
public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) {
public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage, double velocity) {
double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot
@@ -110,6 +93,17 @@ public class VelocityCommander {
distToHood(predictedDist);
distToTransferPow(predictedDist, voltage);
distToRPM(predictedDist);
hoodPos += adjustHood(predictedDist, velocity, velo);
}
public double adjustHood(double dist, double currentVelocity, double targetVelocity){
double error = targetVelocity - currentVelocity;
if (dist < farBound || error < 0){
error = 0;
}
System.out.println("Error "+ error);
return error * errorHoodAdjustment;
}
public double getPredictedRPM(){return velo;}