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

Merge remote-tracking branch 'origin/danielv5' into update-teleop

Browse Source
This commit is contained in:
Keshav Anand
2026-06-01 16:25:07 -05:00
parent bfb37f13f8 ccc0e2123a
commit f32f31a224
4 changed files with 311 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

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

132
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java Normal file
View File

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

187
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

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

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
View File

@@ -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) {