From a470b7dbc4b102cc16acd5dfe1581778fd19a696 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 1 Jun 2026 16:21:31 -0500 Subject: [PATCH] loop times calling --- .../ftc/teamcode/utils/Robot.java | 187 ++++++++++++++++-- .../ftc/teamcode/utilsv2/Drivetrain.java | 8 +- 2 files changed, 178 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 98ca41c..5635b5b 100644 --- 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; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java index eac72e0..ecba638 100644 --- 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) {