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

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

@@ -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++;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java

@@ -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);
+    }
+
+}

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

@@ -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;
     }
 

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

@@ -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;
+                NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
-                double red = robot.color1.getNormalizedColors().red;
-                double blue = robot.color1.getNormalizedColors().blue;
 
-                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;
+                NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
-                double red = robot.color2.getNormalizedColors().red;
-                double blue = robot.color2.getNormalizedColors().blue;
 
-                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;
+                NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
-                double red = robot.color3.getNormalizedColors().red;
-                double blue = robot.color3.getNormalizedColors().blue;
 
-                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);
+        servos.setSpinPos(pos);
-        robot.spin2.setPosition(1-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;
 

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

@@ -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;