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

Reduce calls to the spindexer, transfer and color sensors. Add new MeasuringLoopTimes class to measure min, max and avg loop times.

Browse Source
This commit is contained in:
Matt9150
2026-02-07 16:06:03 -06:00
parent 127995c5fe
commit 9f5dcb4343
5 changed files with 142 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -74,6 +75,8 @@ public class TeleopV3 extends LinearOpMode {
boolean turretInterpolate = false;
private boolean shootAll = false;
public MeasuringLoopTimes measuringLoopTimes;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
@@ -94,6 +97,9 @@ public class TeleopV3 extends LinearOpMode {
drivetrain = new Drivetrain(robot, drive);
measuringLoopTimes = new MeasuringLoopTimes();
measuringLoopTimes.init();
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
@@ -115,7 +121,7 @@ public class TeleopV3 extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
while (opModeIsActive()) {
@@ -135,7 +141,7 @@ public class TeleopV3 extends LinearOpMode {
if (gamepad1.right_bumper) {
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
}
@@ -263,14 +269,14 @@ public class TeleopV3 extends LinearOpMode {
spindexer.prepareShootAllContinous();
//TELE.addLine("preparing to shoot");
// } else if (shooterTicker == 2) {
// //robot.transferServo.setPosition(transferServo_in);
// //servo.setTransferPos(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
servo.setTransferPos(transferServo_in);
//TELE.addLine("shoot");
} else {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
@@ -282,7 +288,7 @@ public class TeleopV3 extends LinearOpMode {
}
if (gamepad1.left_stick_button) {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
@@ -304,6 +310,7 @@ public class TeleopV3 extends LinearOpMode {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
measuringLoopTimes.loop();
//
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -340,6 +347,11 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Avg Loop Time", measuringLoopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", measuringLoopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", measuringLoopTimes.getMaxLoopTimeOneMin());
TELE.update();
ticker++;

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java Normal file
View File

@@ -0,0 +1,66 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.robotcore.util.ElapsedTime;
public class MeasuringLoopTimes {
private ElapsedTime elapsedtime;
private double minLoopTime = 999999999999.0;
private double maxLoopTime = 0.0;
private double mainLoopTime = 0.0;
private double MeasurementStart = 0.0;
private double currentTime = 0.0;
private double avgLoopTime = 0.0;
private int avgLoopTimeTicker = 0;
private double avgLoopTimeSum = 0;
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public void init() {
elapsedtime = new ElapsedTime();
elapsedtime.reset();
MeasurementStart = getTimeSeconds();
}
public double getAvgLoopTime() {
return avgLoopTime;
}
public double getMaxLoopTimeOneMin() {
return maxLoopTime;
}
public double getMinLoopTimeOneMin() {
return minLoopTime;
}
public void loop() {
currentTime = getTimeSeconds();
if ((MeasurementStart + 15.0) < currentTime)
{
minLoopTime = 9999999.0;
maxLoopTime = 0.0;
MeasurementStart = currentTime;
avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
avgLoopTimeSum = 0.0;
avgLoopTimeTicker = 0;
}
mainLoopTime = elapsedtime.milliseconds();
elapsedtime.reset();
avgLoopTimeSum += mainLoopTime;
avgLoopTimeTicker++;
minLoopTime = Math.min(minLoopTime,mainLoopTime);
maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
}
}

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

@@ -18,6 +18,14 @@ public class Servos {
PIDFController spinPID;
PIDFController turretPID;
private double prevSpinPos = 0.0;
private boolean firstSpinPos = true;
private double prevTransferPos = 0.0;
private boolean firstTransferPos = true;
private double spinPos = 0.0;
public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF);
@@ -32,8 +40,32 @@ public class Servos {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
}
public double setSpinPos(double pos) {
public double getSpinCmdPos() {
return prevSpinPos;
}
public boolean servoPosEqual(double pos1, double pos2) {
return (Math.abs(pos1 - pos2) < 0.005);
}
public double setTransferPos(double pos) {
if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
robot.transferServo.setPosition(pos);
firstTransferPos = false;
}
prevTransferPos = pos;
return pos;
}
public double setSpinPos(double pos) {
if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
firstSpinPos = false;
}
prevSpinPos = pos;
return pos;
}

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import static org.firstinspires.ftc.teamcode.constants.Color.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
@@ -184,11 +186,9 @@ public class Spindexer {
if (detectRearColor) {
// Detect which color
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) {
@@ -205,11 +205,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP >= 0.4) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -234,11 +232,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -269,8 +265,7 @@ public class Spindexer {
}
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
servos.setSpinPos(pos);
// double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
@@ -281,7 +276,7 @@ public class Spindexer {
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
// }
// prevPos = currentPos;
// prevPos = pos;
}
public void stopSpindexer() {
@@ -325,7 +320,7 @@ public class Spindexer {
case UNKNOWN_START:
// For now just set position ONE if UNKNOWN
commandedIntakePosition = 0;
moveSpindexerToPos(intakePositions[0]);
servos.setSpinPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break;
case UNKNOWN_MOVE:
@@ -336,7 +331,7 @@ public class Spindexer {
unknownColorDetect = 0;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
case UNKNOWN_DETECT:
@@ -355,7 +350,7 @@ public class Spindexer {
} else {
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
}
break;
case FINDNEXT:
@@ -387,7 +382,7 @@ public class Spindexer {
//commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
break;
case MOVING:
@@ -403,7 +398,7 @@ public class Spindexer {
//detectBalls(false, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
@@ -416,7 +411,7 @@ public class Spindexer {
}
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break;
case SHOOT_ALL_PREP:
@@ -425,7 +420,7 @@ public class Spindexer {
commandedIntakePosition = 0;
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
}
break;
@@ -437,7 +432,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
// Maintain Position
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTNEXT:
@@ -458,7 +453,7 @@ public class Spindexer {
// Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT;
}
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTMOVING:
@@ -467,7 +462,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
}
break;
@@ -492,14 +487,14 @@ public class Spindexer {
}
// Keep moving the spindexer
spindexerOuttakeWiggle *= -1.0;
moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break;
case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else {
moveSpindexerToPos(spinStartPos);
servos.setSpinPos(spinStartPos);
}
break;
@@ -510,11 +505,11 @@ public class Spindexer {
if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03;
}
moveSpindexerToPos(spinPos);
servos.setSpinPos(spinPos);
}
break;

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

@@ -11,6 +11,7 @@ import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;