From c160b3fa6b93d5f26fc578a4d507ff3bdb551d74 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 13 Jan 2026 22:10:16 -0600 Subject: [PATCH] configuration preparation --- .../ftc/teamcode/tests/PIDServoTest.java | 4 +- .../utils/PositionalServoProgrammer.java | 24 ++++++-- .../ftc/teamcode/utils/Robot.java | 55 ++++--------------- .../ftc/teamcode/utils/Servos.java | 2 +- 4 files changed, 32 insertions(+), 53 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java index ccae676..3286912 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java @@ -46,7 +46,7 @@ public class PIDServoTest extends LinearOpMode { controller.setPIDF(p, i, d, f); if (mode == 0) { - pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3); + pos = robot.turr1Pos.getCurrentPosition(); double pid = controller.calculate(pos, target); @@ -62,7 +62,7 @@ public class PIDServoTest extends LinearOpMode { } telemetry.addData("pos", pos); - telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage()); + telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition()); telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage()); telemetry.addData("target", target); telemetry.addData("Mode", mode); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 4f65624..804e535 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -61,13 +61,25 @@ public class PositionalServoProgrammer extends LinearOpMode { if (hoodPos != 0.501){ robot.hood.setPosition(hoodPos); } - TELE.addData("spindexer", servo.getSpinPos()); - TELE.addData("turret", servo.getTurrPos()); - TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); - TELE.addData("hood voltage", robot.hoodPos.getVoltage()); + // To check configuration of spindexer: + // Set "mode" to 1 and spindexPow to 0.1 + // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE + // Do the previous test again to confirm + // Set "mode" to 0 but keep spindexPos at 0.501 + // Manually turn the spindexer until "spindexer pos" is set close to 0 + // Check each spindexer voltage: + // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done! + // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE + //TODO: @KeshavAnandCode do the above please + + TELE.addData("spindexer pos", servo.getSpinPos()); + TELE.addData("turret pos", servo.getTurrPos()); + TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage()); + TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); + TELE.addData("hood pos", robot.hood.getPosition()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); - TELE.addData("turret voltage", robot.turr1Pos.getVoltage()); - TELE.addData("Spin Equal", servo.spinEqual(spindexPos)); + TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition()); + TELE.addData("spindexer pow", robot.spin1.getPower()); 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 38000fd..dee7dac 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 @@ -7,7 +7,6 @@ import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -28,28 +27,16 @@ public class Robot { public DcMotorEx shooter2; public Servo hood; public Servo transferServo; - public Servo rejecter; public CRServo turr1; public CRServo turr2; public CRServo spin1; public CRServo spin2; - public DigitalChannel pin0; - public DigitalChannel pin1; - public DigitalChannel pin2; - public DigitalChannel pin3; - public DigitalChannel pin4; - public DigitalChannel pin5; - public AnalogInput analogInput; - public AnalogInput analogInput2; public AnalogInput spin1Pos; public AnalogInput spin2Pos; - public AnalogInput hoodPos; - public AnalogInput turr1Pos; - public AnalogInput turr2Pos; + public DcMotorEx turr1Pos; public AnalogInput transferServoPos; public AprilTagProcessor aprilTagProcessor; public WebcamName webcam; - public DcMotorEx shooterEncoder; public RevColorSensorV3 color1; public RevColorSensorV3 color2; public RevColorSensorV3 color3; @@ -57,10 +44,12 @@ public class Robot { public static boolean usingLimelight = true; + public static boolean usingCamera = true; + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map - + //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontRight = hardwareMap.get(DcMotorEx.class, "fr"); backLeft = hardwareMap.get(DcMotorEx.class, "bl"); @@ -74,30 +63,24 @@ public class Robot { backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); intake = hardwareMap.get(DcMotorEx.class, "intake"); - rejecter = hardwareMap.get(Servo.class, "rejecter"); shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - + //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - shooterEncoder = shooter1; - hood = hardwareMap.get(Servo.class, "hood"); - hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); - turr1 = hardwareMap.get(CRServo.class, "t1"); - turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); - turr2 = hardwareMap.get(CRServo.class, "t2"); - turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); + turr1Pos = intake; // Encoder of turret plugged in intake port + //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(CRServo.class, "spin1"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); @@ -109,22 +92,6 @@ public class Robot { spin1.setDirection(DcMotorSimple.Direction.REVERSE); spin2.setDirection(DcMotorSimple.Direction.REVERSE); - pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); - - pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); - - pin2 = hardwareMap.get(DigitalChannel.class, "pin2"); - - pin3 = hardwareMap.get(DigitalChannel.class, "pin3"); - - pin4 = hardwareMap.get(DigitalChannel.class, "pin4"); - - pin5 = hardwareMap.get(DigitalChannel.class, "pin5"); - - analogInput = hardwareMap.get(AnalogInput.class, "analog"); - - analogInput2 = hardwareMap.get(AnalogInput.class, "analog2"); - transfer = hardwareMap.get(DcMotorEx.class, "transfer"); transferServo = hardwareMap.get(Servo.class, "transferServo"); @@ -132,10 +99,7 @@ public class Robot { transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); transfer.setDirection(DcMotorSimple.Direction.REVERSE); - - aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); - - webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); @@ -145,6 +109,9 @@ public class Robot { if (usingLimelight){ limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } else if (usingCamera){ + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 058565d..967fa78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -45,7 +45,7 @@ public class Servos { } public double getTurrPos() { - return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3); + return robot.turr1Pos.getCurrentPosition(); } public double setTurrPos(double pos) {