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

added regression

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-04 15:18:08 -05:00
parent 3ae976c16d
commit deefa19be4
7 changed files with 102 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,6 +27,7 @@ public class TeleopV4 extends LinearOpMode {
SpindexerTransferIntake spindexerTransferIntake;
Turret turret;
Flywheel flywheel;
VelocityCommander commander;
ParkTilter parkTilter;
@@ -41,6 +42,7 @@ public class TeleopV4 extends LinearOpMode {
FtcDashboard.getInstance().getTelemetry(), telemetry
);
commander = new VelocityCommander();
drivetrain = new Drivetrain(robot, TELE);
follower = Constants.createFollower(hardwareMap);
Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
@@ -51,10 +53,9 @@ public class TeleopV4 extends LinearOpMode {
parkTilter = new ParkTilter(robot);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);

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

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
public static double hoodPos = 0.51;
public static double flywheel_velo = 0;
public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93;
private enum ShootState {
IDLE,

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

@@ -14,10 +14,10 @@ public class Flywheel {
// public PIDFCoefficients shooterPIDF1, shooterPIDF2;
public static PIDFCoefficients shooterPIDF;
public static double shooterPIDF_P = 255;
public static double shooterPIDF_I = 0.0;
public static double shooterPIDF_P = 500;
public static double shooterPIDF_I = 1;
public static double shooterPIDF_D = 0.0;
public static double shooterPIDF_F = 75;
public static double shooterPIDF_F = 93;
private double velo = 0.0;
private double velo1 = 0.0;

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

@@ -169,7 +169,7 @@ public class Robot {
// Voids below are used to minimize hardware calls to minimize loop times
// Used to cut off digits that are negligible
private final int maxDigits = 5;
private final int maxDigits = 3;
private final int roundingFactor = (int) Math.pow(10, maxDigits);
private double prevFrontLeftPower = -10.501;

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

@@ -81,7 +81,6 @@ public class Shooter {
case NOTHING:
break;
case MANUAL:
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
@@ -115,6 +114,7 @@ public class Shooter {
follow.getAcceleration().getYComponent()
);
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
break;
case READ_OBELISK:
@@ -146,6 +146,7 @@ public class Shooter {
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
break;

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

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import android.health.connect.datatypes.units.Velocity;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -9,8 +11,11 @@ public class SpindexerTransferIntake {
private final Robot robot;
public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
VelocityCommander commander;
public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE, VelocityCommander com) {
this.robot = rob;
this.commander = com;
}
private final double sensorDistanceThreshold = 6.0;
@@ -61,6 +66,10 @@ public class SpindexerTransferIntake {
public void update() {
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
robot.setTransferPower(-0.7);
}
switch (mode) {
case RAPID:
@@ -74,7 +83,6 @@ public class SpindexerTransferIntake {
case INTAKE:
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Closed
);
@@ -95,8 +103,6 @@ public class SpindexerTransferIntake {
case TRANSFER_OFF:
robot.setTransferPower(-0.7);
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
setRapidMode(RapidMode.BEFORE_PULSE_OUT);
}

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

@@ -6,15 +6,90 @@ public class VelocityCommander {
private final double xAccK = 0; // TODO: Tune
private final double yVelK = 0; // TODO: Tune
private final double yAccK = 0; // TODO: Tune
private double hoodPos = 0.88;
private double transferPow = -1;
public VelocityCommander() {
}
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;
private double distToRPM (double dist){
return Math.sqrt(dist*dist + goalH*goalH) * 20;
//TODO: Add regression here using goalH
double velo = 0;
if (dist < 49) {
velo = 2000;
} else if (dist > 165){
velo = 3760;
} else {
velo = 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;
velo = Math.max(2000, Math.min(3760, velo));
}
return velo;
}
private final double hoodA = -4.3276177*Math.pow(10, -13);
private final double hoodB = 2.68062979*Math.pow(10, -11);
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;
private void distToHood (double dist){
if (dist > 112){
hoodPos = 0.35;
} else if (dist < 49){
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));
}
}
private void distToTransferPow(double dist){
if (dist < 118){
transferPow = -1;
} else if (dist < 125){
transferPow = -0.7;
} else {
transferPow = -0.5;
}
}
public double getTransferPow(){return transferPow;}
public double getHoodPredicted(){
return hoodPos;
}
// 27
public double getVeloStationary (double distance){
return distToRPM(distance);
@@ -29,6 +104,8 @@ public class VelocityCommander {
predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
distToHood(predictedDist);
return distToRPM(predictedDist);
}