From 4b998766a1471ab966d80bee08f77f23d0657627 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 3 Jan 2026 15:55:35 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/autonomous/Blue_V2.java | 28 +-- .../ftc/teamcode/autonomous/Red_V2.java | 24 +-- .../ftc/teamcode/teleop/TeleopV2.java | 46 ++--- .../ftc/teamcode/tests/IntakeTest.java | 167 ++++++++++++++++++ .../ftc/teamcode/tests/PIDServoTest.java | 22 ++- .../ftc/teamcode/tests/ShooterTest.java | 15 +- .../ftc/teamcode/utils/Flywheel.java | 2 +- .../utils/PositionalServoProgrammer.java | 28 ++- .../ftc/teamcode/utils/Robot.java | 19 +- .../ftc/teamcode/utils/Servos.java | 71 ++++---- 10 files changed, 307 insertions(+), 115 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java index 7cb04a1..518c249 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java @@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - robot.turr1.setPosition(turret_blue); - robot.turr2.setPosition(1 - turret_blue); + robot.turr1.setPower(turret_blue); + robot.turr2.setPower(1 - turret_blue); return false; } else { return true; @@ -203,8 +203,8 @@ public class Blue_V2 extends LinearOpMode { position = spindexer_intakePos1 - 0.02; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); if (ticker == 0) { stamp = getRuntime(); @@ -264,8 +264,8 @@ public class Blue_V2 extends LinearOpMode { velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - robot.spin1.setPosition(spindexer); - robot.spin2.setPosition(1-spindexer); + robot.spin1.setPower(spindexer); + robot.spin2.setPower(1-spindexer); TELE.addData("Velocity", velo); TELE.addLine("spindex"); TELE.update(); @@ -380,8 +380,8 @@ public class Blue_V2 extends LinearOpMode { } else { position = spindexer_intakePos1 - 0.02; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); TELE.addData("Velocity", velo); TELE.addLine("Intaking"); @@ -418,8 +418,8 @@ public class Blue_V2 extends LinearOpMode { } else { position = spindexer_intakePos1 - 0.02; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); double s1D = robot.color1.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM); @@ -541,13 +541,13 @@ public class Blue_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - robot.turr1.setPosition(turret_detectBlue); - robot.turr2.setPosition(1 - turret_detectBlue); + robot.turr1.setPower(turret_detectBlue); + robot.turr2.setPower(1 - turret_detectBlue); robot.transferServo.setPosition(transferServo_out); - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1 - spindexer_intakePos1); + robot.spin1.setPower(spindexer_intakePos1); + robot.spin2.setPower(1 - spindexer_intakePos1); aprilTag.update(); TELE.addData("Velocity", velo); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index 71a742a..1f518d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -171,8 +171,8 @@ public class Red_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - robot.turr1.setPosition(turret_red); - robot.turr2.setPosition(1 - turret_red); + robot.turr1.setPower(turret_red); + robot.turr2.setPower(1 - turret_red); return false; } else { return true; @@ -190,8 +190,8 @@ public class Red_V2 extends LinearOpMode { velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - robot.spin1.setPosition(spindexer); - robot.spin2.setPosition(1-spindexer); + robot.spin1.setPower(spindexer); + robot.spin2.setPower(1-spindexer); TELE.addData("Velocity", velo); TELE.addLine("spindex"); TELE.update(); @@ -272,8 +272,8 @@ public class Red_V2 extends LinearOpMode { } else { position = spindexer_intakePos1 - 0.02; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); TELE.addData("Velocity", velo); TELE.addLine("Intaking"); @@ -331,8 +331,8 @@ public class Red_V2 extends LinearOpMode { } else { position = spindexer_intakePos1 - 0.02; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); double s1D = robot.color1.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM); @@ -454,13 +454,13 @@ public class Red_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - robot.turr1.setPosition(turret_detectRed); - robot.turr2.setPosition(1 - turret_detectRed); + robot.turr1.setPower(turret_detectRed); + robot.turr2.setPower(1 - turret_detectRed); robot.transferServo.setPosition(transferServo_out); - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1 - spindexer_intakePos1); + robot.spin1.setPower(spindexer_intakePos1); + robot.spin2.setPower(1 - spindexer_intakePos1); aprilTag.update(); TELE.addData("Velocity", velo); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 83f9805..7a9b262 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.teleop; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*; @@ -126,8 +126,8 @@ public class TeleopV2 extends LinearOpMode { aprilTagWebcam.init(new Robot(hardwareMap), TELE); - robot.turr1.setPosition(0.4); - robot.turr2.setPosition(1 - 0.4); + robot.turr1.setPower(0.4); + robot.turr2.setPower(1 - 0.4); waitForStart(); if (isStopRequested()) return; @@ -181,14 +181,14 @@ public class TeleopV2 extends LinearOpMode { position = spindexer_intakePos1 - 0.015; } - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); } else if (reject) { robot.intake.setPower(-1); double position = spindexer_intakePos1; - robot.spin1.setPosition(position); - robot.spin2.setPosition(1 - position); + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); } else { robot.intake.setPower(0); } @@ -363,8 +363,8 @@ public class TeleopV2 extends LinearOpMode { if (!overrideTurr) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + robot.turr1.setPower(pos); + robot.turr2.setPower(1 - pos); } if (gamepad2.dpad_right) { @@ -445,12 +445,12 @@ public class TeleopV2 extends LinearOpMode { reject = true; if (getRuntime() % 3 > 1.5) { - robot.spin1.setPosition(0); - robot.spin2.setPosition(1); + robot.spin1.setPower(0); + robot.spin2.setPower(1); } else { - robot.spin1.setPosition(1); - robot.spin2.setPosition(0); + robot.spin1.setPower(1); + robot.spin2.setPower(0); } robot.transferServo.setPosition(transferServo_out); @@ -475,9 +475,9 @@ public class TeleopV2 extends LinearOpMode { overrideTurr = true; double bearing = d20.ftcPose.bearing; - double finalPos = robot.turr1.getPosition() - (bearing / 1300); - robot.turr1.setPosition(finalPos); - robot.turr2.setPosition(1 - finalPos); + double finalPos = robot.turr1.getPower() - (bearing / 1300); + robot.turr1.setPower(finalPos); + robot.turr2.setPower(1 - finalPos); TELE.addData("Bear", bearing); @@ -487,9 +487,9 @@ public class TeleopV2 extends LinearOpMode { overrideTurr = true; double bearing = d24.ftcPose.bearing; - double finalPos = robot.turr1.getPosition() - (bearing / 1300); - robot.turr1.setPosition(finalPos); - robot.turr2.setPosition(1 - finalPos); + double finalPos = robot.turr1.getPower() - (bearing / 1300); + robot.turr1.setPower(finalPos); + robot.turr2.setPower(1 - finalPos); } @@ -543,8 +543,8 @@ public class TeleopV2 extends LinearOpMode { } else { // Finished shooting all balls - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1 - spindexer_intakePos1); + robot.spin1.setPower(spindexer_intakePos1); + robot.spin2.setPower(1 - spindexer_intakePos1); shootA = true; shootB = true; shootC = true; @@ -764,8 +764,8 @@ public class TeleopV2 extends LinearOpMode { public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { // Set spin positions - robot.spin1.setPosition(spindexer); - robot.spin2.setPosition(1 - spindexer); + robot.spin1.setPower(spindexer); + robot.spin2.setPower(1 - spindexer); // Check if spindexer has reached the target position if (spinOk || getRuntime() - stamp > 1.5) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java new file mode 100644 index 0000000..0dce6e4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -0,0 +1,167 @@ +package org.firstinspires.ftc.teamcode.tests; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + +import com.acmerobotics.dashboard.FtcDashboard; +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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; + +@Config +@TeleOp +public class IntakeTest extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Servos servo; + + public static int mode = 0; // 0 for teleop, 1 for auto + public static double manualPow = 0.5; + double stamp = 0; + int ticker = 0; + boolean b1 = false; + boolean b2 = false; + boolean b3 = false; + boolean steadySpin = false; + double powPID = 0.0; + double spindexerPos = spindexer_intakePos1; + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + servo = new Servos(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + if (mode == 0) { + if (gamepad1.cross) { + ticker = 0; + robot.spin1.setPower(manualPow); + robot.spin2.setPower(-manualPow); + robot.intake.setPower(1); + } else { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + if (getRuntime() - stamp < 0.5) { + robot.intake.setPower(-1); + } else { + robot.intake.setPower(0); + } + } + } else if (mode == 1) { + + if (gamepad1.cross){ + robot.intake.setPower(1); + } else if (gamepad1.circle){ + robot.intake.setPower(-1); + } else { + robot.intake.setPower(0); + } + + colorDetect(); + spindexer(); + + if (b1 && steadySpin && getRuntime() - stamp > 0.5){ + if (!b2){ + if (servo.spinEqual(spindexer_intakePos1)){ + spindexerPos = spindexer_intakePos2; + } else if (servo.spinEqual(spindexer_intakePos2)){ + spindexerPos = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos3)){ + spindexerPos = spindexer_intakePos1; + } + } else if (!b3){ + if (servo.spinEqual(spindexer_intakePos1)){ + spindexerPos = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos2)){ + spindexerPos = spindexer_intakePos1; + } else if (servo.spinEqual(spindexer_intakePos3)){ + spindexerPos = spindexer_intakePos2; + } + } else { + spindexerPos = spindexer_outtakeBall1; + } + } + + powPID = servo.setSpinPos(spindexerPos); + + } else if (mode == 2){ // switch to this mode before switching modes + powPID = 0; + spindexerPos = spindexer_intakePos1; + stamp = getRuntime(); + ticker = 0; + } + TELE.addData("Manual Power", manualPow); + TELE.addData("PID Power", powPID); + TELE.addData("B1", b1); + TELE.addData("B2", b2); + TELE.addData("B3", b3); + TELE.addData("Spindex Pos", servo.getSpinPos()); + } + } + + public void colorDetect() { + // ----- COLOR 1 ----- + double green1 = robot.color1.getNormalizedColors().green; + double blue1 = robot.color1.getNormalizedColors().blue; + double red1 = robot.color1.getNormalizedColors().red; + + b1 = robot.color1.getDistance(DistanceUnit.MM) < 40; + + TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); + TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); + TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); + +// ----- COLOR 2 ----- + double green2 = robot.color2.getNormalizedColors().green; + double blue2 = robot.color2.getNormalizedColors().blue; + double red2 = robot.color2.getNormalizedColors().red; + + b2 = robot.color2.getDistance(DistanceUnit.MM) < 40; + + TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor()); + TELE.addData("Color2 green", green2 / (green2 + blue2 + red2)); + TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM)); + +// ----- COLOR 3 ----- + double green3 = robot.color3.getNormalizedColors().green; + double blue3 = robot.color3.getNormalizedColors().blue; + double red3 = robot.color3.getNormalizedColors().red; + + b3 = robot.color3.getDistance(DistanceUnit.MM) < 30; + + TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor()); + TELE.addData("Color3 green", green3 / (green3 + blue3 + red3)); + TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM)); + + TELE.update(); + } + + public void spindexer(){ + if (!servo.spinEqual(spindexerPos)){ + robot.spin1.setPower(powPID); + robot.spin2.setPower(-powPID); + steadySpin = false; + ticker = 0; + } else{ + robot.spin1.setPower(0); + robot.spin2.setPower(0); + steadySpin = true; + if (ticker == 0){ + stamp = getRuntime(); + } + ticker++; + } + } +} 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 028290f..ccae676 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 @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.tests; -import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.CRServo; @@ -17,14 +15,14 @@ import org.firstinspires.ftc.teamcode.utils.Robot; @Config public class PIDServoTest extends LinearOpMode { - public static double p = 0.0003, i = 0, d = 0.00001; + public static double p = 2, i = 0, d = 0, f = 0; public static double target = 0.5; public static int mode = 0; //0 is for turret, 1 is for spindexer - public static double scalar = 1.112; - public static double restPos = 0.15; + public static double scalar = 1.01; + public static double restPos = 0.0; Robot robot; @@ -33,7 +31,7 @@ public class PIDServoTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - PIDController controller = new PIDController(p, i, d); + PIDFController controller = new PIDFController(p, i, d, f); controller.setTolerance(0); robot = new Robot(hardwareMap); @@ -45,22 +43,22 @@ public class PIDServoTest extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { - controller.setPID(p, i, d); + controller.setPIDF(p, i, d, f); if (mode == 0) { pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3); double pid = controller.calculate(pos, target); - robot.turr1.setPosition(pid); - robot.turr2.setPosition(-pid); + robot.turr1.setPower(pid); + robot.turr2.setPower(-pid); } else if (mode == 1) { pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3); double pid = controller.calculate(pos, target); - robot.spin1.setPosition(pid); - robot.spin2.setPosition(-pid); + robot.spin1.setPower(pid); + robot.spin2.setPower(-pid); } telemetry.addData("pos", pos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 62bdf58..84dfb72 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -34,6 +34,7 @@ public class ShooterTest extends LinearOpMode { robot = new Robot(hardwareMap); DcMotorEx leftShooter = robot.shooter1; DcMotorEx rightShooter = robot.shooter2; + flywheel = new Flywheel(); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() @@ -50,11 +51,9 @@ public class ShooterTest extends LinearOpMode { leftShooter.setPower(parameter); } else if (mode == 1) { double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition()); - - TELE.addData("Velocity", flywheel.getVelo()); + rightShooter.setPower(powPID); + leftShooter.setPower(powPID); TELE.addData("PIDPower", powPID); - TELE.addData("Power", robot.shooter1.getPower()); - TELE.addData("Steady?", flywheel.getSteady()); } if (hoodPos != 0.501) { @@ -62,8 +61,8 @@ public class ShooterTest extends LinearOpMode { } if (turretPos != 0.501) { - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(turretPos); + robot.turr1.setPower(turretPos); + robot.turr2.setPower(turretPos); } robot.transfer.setPower(transferPower); @@ -72,6 +71,10 @@ public class ShooterTest extends LinearOpMode { } else { robot.transferServo.setPosition(transferServo_out); } + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Power", robot.shooter1.getPower()); + TELE.addData("Steady?", flywheel.getSteady()); + TELE.addData("Position", robot.shooter1.getCurrentPosition()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 6f67a59..2848835 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -51,7 +51,7 @@ public class Flywheel { velo3 = velo2; velo2 = velo1; - currentPos = shooter1CurPos / 2048; + currentPos = shooter1CurPos / 3072; stamp = getTimeSeconds(); //getRuntime(); velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1)); initPos = currentPos; 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 125b5e3..098e0a8 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 @@ -13,6 +13,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; public class PositionalServoProgrammer extends LinearOpMode { Robot robot; MultipleTelemetry TELE; + Servos servo; + public static double spindexPos = 0.501; public static double turretPos = 0.501; public static double transferPos = 0.501; @@ -22,16 +24,25 @@ public class PositionalServoProgrammer extends LinearOpMode { public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + servo = new Servos(hardwareMap); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501){ - robot.spin1.setPosition(spindexPos); - robot.spin2.setPosition(1-spindexPos); + if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ + double pos = servo.setSpinPos(spindexPos); + robot.spin1.setPower(pos); + robot.spin2.setPower(-pos); + } else{ + robot.spin1.setPower(0); + robot.spin2.setPower(0); } - if (turretPos != 0.501){ - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(1-turretPos); + if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ + double pos = servo.setTurrPos(turretPos); + robot.turr1.setPower(pos); + robot.turr2.setPower(-pos); + } else { + robot.turr1.setPower(0); + robot.turr2.setPower(0); } if (transferPos != 0.501){ robot.transferServo.setPosition(transferPos); @@ -39,14 +50,15 @@ public class PositionalServoProgrammer extends LinearOpMode { if (hoodPos != 0.501){ robot.hood.setPosition(hoodPos); } - TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3)); + TELE.addData("spindexer", servo.getSpinPos()); TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3)); TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3)); - TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3)); + TELE.addData("turret", servo.getTurrPos()); TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); TELE.addData("hood voltage", robot.hoodPos.getVoltage()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); TELE.addData("turret voltage", robot.turr1Pos.getVoltage()); + TELE.addData("Spin Equal", servo.spinEqual(spindexPos)); 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 12b9a89..0d2fcb0 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.utils; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -35,13 +36,13 @@ public class Robot { public Servo rejecter; - public Servo turr1; + public CRServo turr1; - public Servo turr2; + public CRServo turr2; - public Servo spin1; + public CRServo spin1; - public Servo spin2; + public CRServo spin2; public DigitalChannel pin0; @@ -104,6 +105,8 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter1.setDirection(DcMotorSimple.Direction.REVERSE); + shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); shooterEncoder = shooter1; @@ -111,19 +114,19 @@ public class Robot { hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); - turr1 = hardwareMap.get(Servo.class, "t1"); + turr1 = hardwareMap.get(CRServo.class, "t1"); turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); - turr2 = hardwareMap.get(Servo.class, "t2"); + turr2 = hardwareMap.get(CRServo.class, "t2"); turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); - spin1 = hardwareMap.get(Servo.class, "spin1"); + spin1 = hardwareMap.get(CRServo.class, "spin1"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); - spin2 = hardwareMap.get(Servo.class, "spin2"); + spin2 = hardwareMap.get(CRServo.class, "spin2"); spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); 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 3c5c987..a2d82e6 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 @@ -1,52 +1,61 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.utils.Robot; @Config public class Servos { - PIDController spinPID; + Robot robot; - PIDController turretPID; + PIDFController spinPID; + + PIDFController turretPID; //PID constants - public static double spinP = 0.0, spinI = 0.0, spinD = 0.0; - public static double turrP = 0.0, turrI = 0.0, turrD = 0.0; + public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03; + public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF; - public static double spin_scalar = 0.15; - public static double spin_restPos = 1.112; - public static double turret_scalar = 0.15; - public static double turret_restPos = 1.112; + public static double spin_scalar = 1.011; + public static double spin_restPos = 0.0; + public static double turret_scalar = 1.009; + public static double turret_restPos = 0.0; - public void initServos() { - spinPID = new PIDController(spinP, spinI, spinD); - turretPID = new PIDController(turrP, turrI, turrD); + public Servos(HardwareMap hardwareMap) { + robot = new Robot(hardwareMap); + spinPID = new PIDFController(spinP, spinI, spinD, spinF); + turretPID = new PIDFController(turrP, turrI, turrD, turrF); } // In the code below, encoder = robot.servo.getVoltage() - public double getSpinPos(double encoder){ - return spin_scalar * ((encoder - spin_restPos) / 3.3); - } - public double setSpinPos(double pos, double encoder){ - spinPID.setPID(spinP, spinI, spinD); - - return spinPID.calculate(this.getSpinPos(encoder), pos); - } - public boolean spinEqual(double pos, double encoder){ - return Math.abs(pos - this.getSpinPos(encoder)) < 0.01; + public double getSpinPos() { + return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - public double getTurrPos(double encoder){ - return turret_scalar * ((encoder - turret_restPos) / 3.3); - } - public double setTurrPos(double pos, double encoder){ - turretPID.setPID(turrP, turrI, turrD); + public double setSpinPos(double pos) { + spinPID.setPIDF(spinP, spinI, spinD, spinF); - return spinPID.calculate(this.getTurrPos(encoder), pos); + return spinPID.calculate(this.getSpinPos(), pos); } - public boolean turretEqual(double pos, double encoder){ - return Math.abs(pos - this.getTurrPos(encoder)) < 0.01; + + public boolean spinEqual(double pos) { + return Math.abs(pos - this.getSpinPos()) < 0.01; + } + + public double getTurrPos() { + return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3); + } + + public double setTurrPos(double pos) { + turretPID.setPIDF(turrP, turrI, turrD, turrF); + + return spinPID.calculate(this.getTurrPos(), pos); + } + + public boolean turretEqual(double pos) { + return Math.abs(pos - this.getTurrPos()) < 0.01; } }