loop times calling

2026-06-01 16:21:31 -05:00
parent dd2890ea4a
commit a470b7dbc4
2 changed files with 178 additions and 17 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
@@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.TouchSensor;
 import com.qualcomm.robotcore.hardware.VoltageSensor;

 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -38,12 +39,22 @@ public class Robot {
    public DcMotorEx shooter2;
    public Servo hood;
    public Servo transferServo;
+    public Servo spindexBlocker;
+    public Servo rapidFireBlocker;
+    public Servo tilt1;
+    public Servo tilt2;
    public Servo turr1;
    public Servo turr2;
-
    public Servo spin1;
-
    public Servo spin2;
+    public TouchSensor beam1;
+    public TouchSensor beam2;
+    public TouchSensor beam3;
+    public RevColorSensorV3 revSensor;
+
+    public VoltageSensor voltage;
+
+    // Below is disregarded
    public AnalogInput spin1Pos;
    public AnalogInput spin2Pos;
    public AnalogInput turr1Pos;
@@ -55,7 +66,6 @@ public class Robot {
    public RevColorSensorV3 color3;
    public Limelight3A limelight;
    public Servo light;
-    public VoltageSensor voltage;

    public Robot(HardwareMap hardwareMap) {

@@ -89,29 +99,44 @@ public class Robot {

        hood = hardwareMap.get(Servo.class, "hood");

-        turr1 = hardwareMap.get(Servo.class, "t1");
+        turr1 = hardwareMap.get(Servo.class, "turr1");

-        turr2 = hardwareMap.get(Servo.class, "t2");
-
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+        turr2 = hardwareMap.get(Servo.class, "turr2");

        spin1 = hardwareMap.get(Servo.class, "spin2");

-        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
-
        spin2 = hardwareMap.get(Servo.class, "spin1");

-        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
-
        transfer = hardwareMap.get(DcMotorEx.class, "transfer");

        transferServo = hardwareMap.get(Servo.class, "transferServo");

-        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
-
        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

+        spindexBlocker = hardwareMap.get(Servo.class, "spinB");
+
+        rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB");
+
+        tilt1 = hardwareMap.get(Servo.class, "tilt1");
+        tilt2 = hardwareMap.get(Servo.class, "tilt2");
+
+        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
+        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
+        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
+
+        revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
+
+        // Below is disregarded
+
+        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+
+        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+
+        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+
+        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+
        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");

        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
@@ -128,4 +153,140 @@ public class Robot {
        light = hardwareMap.get(Servo.class, "light");
        voltage = hardwareMap.voltageSensor.iterator().next();
    }
+
+    // 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 roundingFactor = (int) Math.pow(10, maxDigits);
+
+    private double prevFrontLeftPower = -10.501;
+    public void setFrontLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontLeftPower){
+            frontLeft.setPower(pow);
+        }
+        prevFrontLeftPower = pow;
+    }
+
+    private double prevFrontRightPower = -10.501;
+    public void setFrontRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontRightPower){
+            frontRight.setPower(pow);
+        }
+        prevFrontRightPower = pow;
+    }
+
+    private double prevBackLeftPower = -10.501;
+    public void setBackLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackLeftPower){
+            backLeft.setPower(pow);
+        }
+        prevBackLeftPower = pow;
+    }
+
+    private double prevBackRightPower = -10.501;
+    public void setBackRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackRightPower){
+            backRight.setPower(pow);
+        }
+        prevBackRightPower = pow;
+    }
+
+    private double prevIntakePower = -10.501;
+    public void setIntakePower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevIntakePower){
+            intake.setPower(pow);
+        }
+        prevIntakePower = pow;
+    }
+
+    private double prevTransferPower = -10.501;
+    public void setTransferPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevTransferPower){
+            transfer.setPower(pow);
+        }
+        prevTransferPower = pow;
+    }
+
+    // shooter motors are done in separate class
+
+    private double prevHoodPos = -10.501;
+    public void setHoodPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevHoodPos){
+            hood.setPosition(pos);
+        }
+        prevHoodPos = pos;
+    }
+
+    private double prevTransferServoPos = -10.501;
+    public void setTransferServoPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTransferServoPos){
+            transferServo.setPosition(pos);
+        }
+        prevTransferServoPos = pos;
+    }
+
+    private double prevSpinPos = -10.501;
+    public void setSpinPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpinPos){
+            spin1.setPosition(pos);
+            spin2.setPosition(pos);
+        }
+        prevSpinPos = pos;
+    }
+
+    private double prevTurretPos = -10.501;
+    public void setTurretPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTurretPos){
+            turr1.setPosition(pos);
+            turr2.setPosition(pos);
+        }
+        prevTurretPos = pos;
+    }
+
+    private double prevTilt1Pos = -10.501;
+    public void setTilt1Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt1Pos){
+            tilt1.setPosition(pos);
+        }
+        prevTilt1Pos = pos;
+    }
+
+    private double prevTilt2Pos = -10.501;
+    public void setTilt2Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt2Pos){
+            tilt2.setPosition(pos);
+        }
+        prevTilt2Pos = pos;
+    }
+
+    private double prevSpindexBlockerPos = -10.501;
+    public void setSpindexBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpindexBlockerPos){
+            spindexBlocker.setPosition(pos);
+        }
+        prevSpindexBlockerPos = pos;
+    }
+
+    private double prevRapidFireBlockerPos = -10.501;
+    public void setRapidFireBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevRapidFireBlockerPos){
+            rapidFireBlocker.setPosition(pos);
+        }
+        prevRapidFireBlockerPos = pos;
+    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
@@ -76,10 +76,10 @@ public class Drivetrain {
        double frontRightPower = (y - x - rx) / denominator;
        double backRightPower = (y + x - rx) / denominator;

-        robot.frontLeft.setPower(frontLeftPower);
-        robot.backLeft.setPower(backLeftPower);
-        robot.frontRight.setPower(frontRightPower);
-        robot.backRight.setPower(backRightPower);
+        robot.setFrontLeftPower(frontLeftPower);
+        robot.setBackLeftPower(backLeftPower);
+        robot.setFrontRightPower(frontRightPower);
+        robot.setBackRightPower(backRightPower);

        if (tele) {