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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-06 19:52:26 -06:00
parent c368552724
commit 8b07ed5579
3 changed files with 50 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -18,28 +18,6 @@ public class ShooterTest extends LinearOpMode {
public static double parameter = 0.0;
// --- CONSTANTS YOU TUNE ---
public static double hoodPos = 0.501;
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double currentPos = 0.0;
double velo = 0.0;
double velo1 = 0.0;
double velo2 = 0.0;
double velo3 = 0.0;
double velo4 = 0.0;
double velo5 = 0.0;
double initPosq = 0.0;
double stampq = 0.0;
double stamp1q = 0.0;
double tickerq = 0.0;
double currentPosq = 0.0;
double veloq = 0.0;
double velo1q = 0.0;
double velo2q = 0.0;
double velo3q = 0.0;
double velo4q = 0.0;
double velo5q = 0.0;
Robot robot;
Flywheel flywheel;
@@ -64,40 +42,8 @@ public class ShooterTest extends LinearOpMode {
if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
leftShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
if (ticker % 2 == 0) {
velo5 = velo4;
velo4 = velo3;
velo3 = velo2;
velo2 = velo1;
currentPos = (double) leftShooter.getCurrentPosition() / 28;
stamp = getRuntime();
velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
}
if (tickerq % 2 == 0) {
velo5q = velo4q;
velo4q = velo3q;
velo3q = velo2q;
velo2q = velo1q;
currentPosq = (double) rightShooter.getCurrentPosition() / 28;
stampq = getRuntime();
velo1q = 60 * ((currentPosq - initPosq) / (stampq - stamp1q));
initPosq = currentPosq;
stamp1q = stampq;
veloq = (velo1q + velo2q + velo3q + velo4q + velo5q) / 5;
}
} else if (mode == 1) {
leftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
double powPID = flywheel.manageFlywheel1((int) parameter, leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition());
rightShooter.setPower(powPID);
leftShooter.setPower(powPID);
TELE.addData("PIDPower", powPID);
@@ -107,9 +53,9 @@ public class ShooterTest extends LinearOpMode {
robot.hood.setPosition(hoodPos);
}
TELE.addData("Velocity1", velo);
TELE.addData("Velocity2", veloq);
TELE.addData("Encoder Velocity", flywheel.getVelo());
TELE.addData("Used Velocity", flywheel.getVelo(leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition()));
TELE.addData("Velocity1", flywheel.getVelo1());
TELE.addData("Velocity2", flywheel.getVelo2());
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28);

73
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -6,13 +6,17 @@ import com.acmerobotics.dashboard.config.Config;
public class Flywheel {
public static double kP = 0.001; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
double initPos = 0.0;
double initPos1 = 0.0;
double initPos2 = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double currentPos = 0.0;
double currentPos1 = 0.0;
double currentPos2 = 0.0;
double velo = 0.0;
double velo1 = 0.0;
double velo1a = 0.0;
double velo1b = 0.0;
double velo2 = 0.0;
double velo3 = 0.0;
double velo4 = 0.0;
@@ -20,27 +24,12 @@ public class Flywheel {
double targetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel () {
public Flywheel() {
//robot = new Robot(hardwareMap);
}
public double getVelo () {
return velo;
}
public boolean getSteady() {
return steady;
}
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
targetVelocity = commandedVelocity;
public double getVelo(double shooter1CurPos, double shooter2CurPos) {
ticker++;
if (ticker % 2 == 0) {
velo5 = velo4;
@@ -48,18 +37,46 @@ public class Flywheel {
velo3 = velo2;
velo2 = velo1;
currentPos = shooter1CurPos / 2048;
currentPos1 = shooter1CurPos / 28;
currentPos2 = shooter2CurPos / 28;
stamp = getTimeSeconds(); //getRuntime();
velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1));
velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1));
initPos1 = currentPos1;
stamp1 = stamp;
velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
if (Math.abs(velo1a - velo1b) > 200) {
if (velo1a < 200) {
velo1 = velo1b;
} else {
velo1 = velo1a;
}
} else {
velo1 = (velo1a + velo1b) / 2;
}
}
// Flywheel control code here
return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5);
}
public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; }
public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; }
public boolean getSteady() {
return steady;
}
private double getTimeSeconds() {
return (double) System.currentTimeMillis() / 1000.0;
}
public double manageFlywheel1(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) {
targetVelocity = commandedVelocity;
velo = getVelo(shooter1CurPos, shooter2CurPos);
// Flywheel PID code here
if (targetVelocity - velo > 500) {
powPID = 1.0;
} else if (velo - targetVelocity > 500){
} else if (velo - targetVelocity > 500) {
powPID = 0.0;
} else {
double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
@@ -78,13 +95,11 @@ public class Flywheel {
powPID = Math.max(0, Math.min(1, powPID));
}
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 100.0);
return powPID;
}
public void update()
{
public void update() {
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -35,8 +35,8 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "s2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
hood = hardwareMap.get(Servo.class, "hood");