diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 8b8b5a2..31430ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -20,6 +20,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.FtcDashboard; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java new file mode 100644 index 0000000..283ae86 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -0,0 +1,132 @@ +package org.firstinspires.ftc.teamcode.tests; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +@TeleOp +public class Hardware_Tester extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + + public static boolean subsystemMode = true; + + // Bare Motor Powers + public static double flPow = 0; + public static double frPow = 0; + public static double blPow = 0; + public static double brPow = 0; + public static double intakePow = 0; + public static double transferPow = 0; + public static double shooter1Pow = 0; + public static double shooter2Pow = 0; + + // Subsystem Motor Powers + public static double drivetrainPow = 0; + public static double shooterPow = 0; + + // Bare Servo Positions + public static double spin1Pos = 0.501; + public static double spin2Pos = 0.501; + public static double turr1Pos = 0.501; + public static double turr2Pos = 0.501; + public static double transferServosPos = 0.501; + public static double hoodPos = 0.501; + public static double spindexBlockerPos = 0.501; + public static double rapidFireBlockerPos = 0.501; + public static double tilt1Pos = 0.501; + public static double tilt2Pos = 0.501; + + // Subsystem Servo Positions + public static double spinPos = 0.501; + public static double turrPos = 0.501; + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(); + + if (isStopRequested()) return; + while (opModeIsActive()){ + // Non-subsystem based components + robot.setIntakePower(intakePow); + robot.setTransferPower(transferPow); + + if (transferServosPos != 0.501){ + robot.setTransferServoPos(transferServosPos); + } + if (hoodPos != 0.501){ + robot.setHoodPos(hoodPos); + } + if (rapidFireBlockerPos != 0.501){ + robot.setRapidFireBlockerPos(rapidFireBlockerPos); + } + if (spindexBlockerPos != 0.501){ + robot.setSpindexBlockerPos(spindexBlockerPos); + } + if (tilt1Pos != 0.501){ + robot.setTilt1Pos(tilt1Pos); + } + if (tilt2Pos != 0.501){ + robot.setTilt2Pos(tilt2Pos); + } + + // Subsystem based components + if (subsystemMode){ + robot.setFrontLeftPower(drivetrainPow); + robot.setFrontRightPower(drivetrainPow); + robot.setBackLeftPower(drivetrainPow); + robot.setBackRightPower(drivetrainPow); + robot.shooter1.setPower(shooterPow); + robot.shooter2.setPower(shooterPow); + + if (spinPos != 0.501){ + robot.setSpinPos(spinPos); + } + if (turrPos != 0.501){ + robot.setTurretPos(turrPos); + } + } else { + robot.setFrontLeftPower(flPow); + robot.setFrontRightPower(frPow); + robot.setBackLeftPower(blPow); + robot.setBackRightPower(brPow); + robot.shooter1.setPower(shooter1Pow); + robot.shooter2.setPower(shooter2Pow); + + if (spin1Pos != 0.501){ + robot.spin1.setPosition(spin1Pos); + } + if (spin2Pos != 0.501){ + robot.spin2.setPosition(spin2Pos); + } + if (turr1Pos != 0.501){ + robot.turr1.setPosition(turr1Pos); + } + if (turr2Pos != 0.501){ + robot.turr2.setPosition(turr2Pos); + } + } + + // Sensor Data + + TELE.addData("Beam Break 1?", robot.beam1.isPressed()); + TELE.addData("Beam Break 2?", robot.beam2.isPressed()); + TELE.addData("Beam Break 3?", robot.beam3.isPressed()); + + NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); + TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM)); + TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green)); + + TELE.addData("Voltage Sensor", robot.voltage.getVoltage()); + + TELE.update(); + } + } +} 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) {