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

Further speed up loop times. We are now running 50% faster but need to retune the turret PID and update the shoot all speed everywhere.

Browse Source
This commit is contained in:
Matt9150
2026-02-10 00:17:26 -06:00
parent 9f5dcb4343
commit 830c2b1481
4 changed files with 35 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,6 +12,7 @@ public class Flywheel {
public double velo1 = 0.0;
public double velo2 = 0.0;
double targetVelocity = 0.0;
double previousTargetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel (HardwareMap hardwareMap) {
@@ -55,12 +56,12 @@ public class Flywheel {
private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
public double manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
targetVelocity = commandedVelocity;
// Add code here to set PIDF based on desired RPM
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
@@ -69,6 +70,7 @@ public class Flywheel {
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
}
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 200.0);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -44,7 +44,7 @@ public class Servos {
return prevSpinPos;
}
public boolean servoPosEqual(double pos1, double pos2) {
public static boolean servoPosEqual(double pos1, double pos2) {
return (Math.abs(pos1 - pos2) < 0.005);
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -49,6 +49,7 @@ public class Spindexer {
public double spindexerOuttakeWiggle = 0.01;
private double prevPos = 0.0;
public double spindexerPosOffset = 0.00;
double shootWaitMax = 4;
public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
@@ -467,7 +468,6 @@ public class Spindexer {
break;
case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition==2) {
@@ -505,8 +505,8 @@ public class Spindexer {
if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
double spinPos = servos.getSpinCmdPos() + 0.005; //shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03) { // 0.03 @ 48ms loops times
spinPos = spinEndPos + 0.03;
}
servos.setSpinPos(spinPos);

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -38,7 +38,8 @@ public class Turret {
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
//public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
@@ -56,6 +57,9 @@ public class Turret {
private double permanentOffset = 0.0;
private PIDController bearingPID;
private double prevTurretPos = 0.0;
private boolean firstTurretPos = true;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
@@ -74,8 +78,18 @@ public class Turret {
}
public void manualSetTurret(double pos) {
setTurretPos(pos);
}
public double setTurretPos(double pos) {
if (firstTurretPos || !Servos.servoPosEqual(pos, prevTurretPos)) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
firstTurretPos = false;
}
prevTurretPos = pos;
return pos;
}
public boolean turretEqual(double pos) {
@@ -273,8 +287,7 @@ public class Turret {
}
// Set servo positions
robot.turr1.setPosition(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
setTurretPos(turretPos + manualOffset);
/* ---------------- TELEMETRY ---------------- */