From 6dee08830084c621062aa1d10733d711e05dcd14 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 16 Jan 2026 21:08:59 -0600 Subject: [PATCH] prototype auto --- .../ftc/teamcode/autonomous/AutoClose_V3.java | 42 +++++++------- .../ftc/teamcode/autonomous/AutoFar_V1.java | 42 +++++++------- .../autonomous/ProtoAutoClose_V3.java | 57 ++++++++----------- .../ftc/teamcode/autonomous/Red_V2.java | 12 ++-- .../ftc/teamcode/constants/Poses.java | 4 +- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV2.java | 16 +++--- .../ftc/teamcode/teleop/TeleopV3.java | 18 +++--- .../ftc/teamcode/tests/IntakeTest.java | 22 +++---- .../utils/PositionalServoProgrammer.java | 14 ++--- .../ftc/teamcode/utils/Servos.java | 27 +++++---- 11 files changed, 122 insertions(+), 134 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java index d91a508..c36040f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java @@ -103,17 +103,17 @@ public class AutoClose_V3 extends LinearOpMode { if (gpp || pgp || ppg) { if (redAlliance){ robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redClose); + double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); + return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition()); } else { robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueClose); + double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueClose); + return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition()); } } else { return true; @@ -132,7 +132,7 @@ public class AutoClose_V3 extends LinearOpMode { robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - spinPID = servo.setSpinPos(spindexer); + spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); TELE.addData("Velocity", velo); @@ -142,7 +142,7 @@ public class AutoClose_V3 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - if (servo.spinEqual(spindexer)){ + if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){ robot.spin1.setPower(0); robot.spin2.setPower(0); return false; @@ -222,27 +222,27 @@ public class AutoClose_V3 extends LinearOpMode { double s2D = robot.color2.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM); - if (!servo.spinEqual(position)){ - double spinPID = servo.setSpinPos(position); + if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){ + double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); } - if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ + if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){ if (s2D > 60){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; } } else if (s3D > 33){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; } } @@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode { double turrPID; if (redAlliance){ - turrPID = servo.setTurrPos(turret_detectRedClose); + turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition()); shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -430,7 +430,7 @@ public class AutoClose_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); } else { - turrPID = servo.setTurrPos(turret_detectBlueClose); + turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition()); shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -458,8 +458,8 @@ public class AutoClose_V3 extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.addData("Spin Pos", servo.getSpinPos()); + TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition())); + TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage())); TELE.update(); } @@ -565,8 +565,8 @@ public class AutoClose_V3 extends LinearOpMode { bearing = result.getTx(); } } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); + double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java index a156c11..25d78c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java @@ -103,17 +103,17 @@ public class AutoFar_V1 extends LinearOpMode { if (gpp || pgp || ppg) { if (redAlliance){ robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redFar); + double turretPID = servo.setTurrPos(turret_redFar, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redFar); + return !servo.turretEqual(turret_redFar, robot.turr1Pos.getCurrentPosition()); } else { robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueFar); + double turretPID = servo.setTurrPos(turret_blueFar, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueFar); + return !servo.turretEqual(turret_blueFar, robot.turr1Pos.getCurrentPosition()); } } else { return true; @@ -132,7 +132,7 @@ public class AutoFar_V1 extends LinearOpMode { robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - spinPID = servo.setSpinPos(spindexer); + spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); TELE.addData("Velocity", velo); @@ -142,7 +142,7 @@ public class AutoFar_V1 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - if (servo.spinEqual(spindexer)){ + if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){ robot.spin1.setPower(0); robot.spin2.setPower(0); return false; @@ -222,27 +222,27 @@ public class AutoFar_V1 extends LinearOpMode { double s2D = robot.color2.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM); - if (!servo.spinEqual(position)){ - double spinPID = servo.setSpinPos(position); + if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){ + double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); } - if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ + if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){ if (s2D > 60){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; } } else if (s3D > 33){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; } } @@ -399,9 +399,9 @@ public class AutoFar_V1 extends LinearOpMode { double turrPID; if (redAlliance){ - turrPID = servo.setTurrPos(turret_detectRedClose); + turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition()); } else { - turrPID = servo.setTurrPos(turret_detectBlueClose); + turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition()); } robot.turr1.setPower(turrPID); @@ -412,8 +412,8 @@ public class AutoFar_V1 extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.addData("Spin Pos", servo.getSpinPos()); + TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition())); + TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage())); TELE.update(); } @@ -466,8 +466,8 @@ public class AutoFar_V1 extends LinearOpMode { bearing = result.getTx(); } } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); + double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java index 10e497b..5438d0b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java @@ -103,17 +103,17 @@ public class ProtoAutoClose_V3 extends LinearOpMode { if (gpp || pgp || ppg) { if (redAlliance){ robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redClose); + double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); + return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition()); } else { robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueClose); + double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueClose); + return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition()); } } else { return true; @@ -132,7 +132,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - spinPID = servo.setSpinPos(spindexer); + spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); TELE.addData("Velocity", velo); @@ -142,7 +142,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - if (servo.spinEqual(spindexer)){ + if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){ robot.spin1.setPower(0); robot.spin2.setPower(0); return false; @@ -178,13 +178,12 @@ public class ProtoAutoClose_V3 extends LinearOpMode { robot.turr2.setPower(holdTurrPow); drive.updatePoseEstimate(); - detectTag(); teleStart = drive.localizer.getPose(); if (ticker == 1){ robot.transferServo.setPosition(transferServo_in); - initPos = servo.getSpinPos(); + initPos = servo.getSpinPos(robot.spin1Pos.getVoltage()); finalPos = initPos + 0.6; @@ -205,7 +204,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { } prevPos = currentPos; - currentPos = servo.getSpinPos(); + currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage()); if (zeroNeeded){ if (currentPos - prevPos < -0.5){ zeroPassed = true; @@ -257,27 +256,27 @@ public class ProtoAutoClose_V3 extends LinearOpMode { double s2D = robot.color2.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM); - if (!servo.spinEqual(position)){ - double spinPID = servo.setSpinPos(position); + if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){ + double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); } - if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ + if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){ if (s2D > 60){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1,robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; } } else if (s3D > 33){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ position = spindexer_intakePos2; } } @@ -409,6 +408,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode { 0, 0, 0 )); + servo = new Servos(); + robot.limelight.pipelineSwitch(1); robot.limelight.start(); @@ -443,11 +444,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { redAlliance = !redAlliance; } - double turrPID; - if (redAlliance){ - turrPID = servo.setTurrPos(turret_detectRedClose); - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -465,8 +462,6 @@ public class ProtoAutoClose_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); } else { - turrPID = servo.setTurrPos(turret_detectBlueClose); - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -485,16 +480,11 @@ public class ProtoAutoClose_V3 extends LinearOpMode { .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); } - robot.turr1.setPower(turrPID); - robot.turr2.setPower(-turrPID); - robot.hood.setPosition(hoodAuto); robot.transferServo.setPosition(transferServo_out); - TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.addData("Spin Pos", servo.getSpinPos()); + TELE.addData("Red?", redAlliance); TELE.update(); } @@ -507,8 +497,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { Actions.runBlocking( new ParallelAction( shoot0.build(), - initShooter(AUTO_CLOSE_VEL), - Obelisk() + initShooter(AUTO_CLOSE_VEL) ) ); drive.updatePoseEstimate(); @@ -598,8 +587,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode { bearing = result.getTx(); } } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); + double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); } 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 00ce9a4..fb37b24 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 @@ -169,10 +169,10 @@ public class Red_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - double turretPID = servo.setTurrPos(turret_redClose); + double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); + return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition()); } else { return true; } @@ -199,7 +199,7 @@ public class Red_V2 extends LinearOpMode { teleStart = drive.localizer.getPose(); - return !servo.spinEqual(spindexer); + return !servo.spinEqual(spindexer, robot.spin1Pos.getVoltage()); } }; } @@ -461,7 +461,7 @@ public class Red_V2 extends LinearOpMode { aprilTag.update(); TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); + TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition())); TELE.update(); } @@ -585,8 +585,8 @@ public class Red_V2 extends LinearOpMode { bearing = d24.ftcPose.bearing; TELE.addData("Bear", bearing); } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); + double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 6deb4ee..cd78787 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -14,13 +14,13 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-15, 0, 0); - public static double rx1 = 45, ry1 = -7, rh1 = 0; + public static double rx1 = 40, ry1 = -7, rh1 = 0; public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); - public static double bx1 = 45, by1 = 6, bh1 = 0; + public static double bx1 = 40, by1 = 6, bh1 = 0; public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index fe47cfa..c85b7cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -24,7 +24,7 @@ public class ServoPositions { public static double hoodDefault = 0.6; - public static double hoodAuto = 0.55; + public static double hoodAuto = 0.22; public static double hoodAutoFar = 0.5; //TODO: change this; 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 67ea79d..52b9c8c 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 @@ -128,7 +128,7 @@ public class TeleopV2 extends LinearOpMode { TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); - servo = new Servos(hardwareMap); + servo = new Servos(); flywheel = new Flywheel(); drive = new MecanumDrive(hardwareMap, teleStart); @@ -159,13 +159,13 @@ public class TeleopV2 extends LinearOpMode { robot.backRight.setPower(backRightPower); // PID SERVOS - turretPID = servo.setTurrPos(turretPos); + turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); //TODO: make sure changing position works throughout opmode - if (!servo.spinEqual(spindexPos)){ - spindexPID = servo.setSpinPos(spindexPos); + if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage())){ + spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spindexPID); robot.spin2.setPower(-spindexPID); } else{ @@ -348,7 +348,7 @@ public class TeleopV2 extends LinearOpMode { bearing = d24.ftcPose.bearing; } overrideTurr = true; - turretPos = servo.getTurrPos() - (bearing/1300); + turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing/1300); TELE.addData("Bear", bearing); double bearingCorrection = bearing / 1300; @@ -481,13 +481,13 @@ public class TeleopV2 extends LinearOpMode { boolean shootingDone = false; if (!outtake1) { - outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); + outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage())); } if (!outtake2) { - outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); + outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage())); } if (!outtake3) { - outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); + outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage())); } switch (currentSlot) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 777859d..96fbee2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -98,7 +98,7 @@ public class TeleopV3 extends LinearOpMode { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - servo = new Servos(hardwareMap); + servo = new Servos(); flywheel = new FlywheelV2(); drive = new MecanumDrive(hardwareMap, teleStart); @@ -131,12 +131,12 @@ public class TeleopV3 extends LinearOpMode { robot.backRight.setPower(backRightPower); // PID SERVOS - turretPID = servo.setTurrPos(turretPos); + turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { - spindexPID = servo.setSpinPos(spindexPos); + if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && !gamepad1.right_bumper) { + spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage()); robot.spin1.setPower(spindexPID); robot.spin2.setPower(-spindexPID); } else { @@ -149,7 +149,7 @@ public class TeleopV3 extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); intakeTicker++; if (intakeTicker % 16 == 0) { - spinCurrentPos = servo.getSpinPos(); + spinCurrentPos = servo.getSpinPos(robot.spin1Pos.getVoltage()); if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) { reverse = !reverse; } @@ -311,7 +311,7 @@ public class TeleopV3 extends LinearOpMode { if (result.isValid()) { bearing = result.getTx(); overrideTurr = true; - turretPos = servo.getTurrPos() - (bearing / 1300); + turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300); double bearingCorrection = bearing / 1300; @@ -427,13 +427,13 @@ public class TeleopV3 extends LinearOpMode { boolean shootingDone = false; if (!outtake1) { - outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); + outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage())); } if (!outtake2) { - outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); + outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage())); } if (!outtake3) { - outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); + outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage())); } switch (currentSlot) { 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 index b55608c..b56baae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -53,7 +53,7 @@ public class IntakeTest extends LinearOpMode { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); } robot = new Robot(hardwareMap); - servo = new Servos(hardwareMap); + servo = new Servos(); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); waitForStart(); @@ -65,7 +65,7 @@ public class IntakeTest extends LinearOpMode { if (gamepad1.right_bumper) { ticker++; if (ticker % 16 == 0){ - currentPos = servo.getSpinPos(); + currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage()); if (Math.abs(currentPos - initPos) == 0.0){ reverse = !reverse; } @@ -109,19 +109,19 @@ public class IntakeTest extends LinearOpMode { if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){ if (!ballIn(2)){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos2; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos1; } } else if (!ballIn(3)){ - if (servo.spinEqual(spindexer_intakePos1)){ + if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos3; - } else if (servo.spinEqual(spindexer_intakePos2)){ + } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos1; - } else if (servo.spinEqual(spindexer_intakePos3)){ + } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){ spindexerPos = spindexer_intakePos2; } } @@ -158,7 +158,7 @@ public class IntakeTest extends LinearOpMode { TELE.addData("B1", ballIn(1)); TELE.addData("B2", ballIn(2)); TELE.addData("B3", ballIn(3)); - TELE.addData("Spindex Pos", servo.getSpinPos()); + TELE.addData("Spindex Pos", servo.getSpinPos(robot.spin1Pos.getVoltage())); TELE.update(); } } @@ -187,10 +187,10 @@ public class IntakeTest extends LinearOpMode { } public void spindexer() { - boolean atTarget = servo.spinEqual(spindexerPos); + boolean atTarget = servo.spinEqual(spindexerPos, robot.spin1Pos.getVoltage()); if (!atTarget) { - powPID = servo.setSpinPos(spindexerPos); + powPID = servo.setSpinPos(spindexerPos, robot.spin1Pos.getVoltage()); robot.spin1.setPower(powPID); robot.spin2.setPower(-powPID); 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 804e535..d159785 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 @@ -29,12 +29,12 @@ 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); + servo = new Servos(); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){ - double pos = servo.setSpinPos(spindexPos); + if (spindexPos != 0.501 && !servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && mode == 0){ + double pos = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage()); robot.spin1.setPower(pos); robot.spin2.setPower(-pos); } else if (mode == 0){ @@ -44,8 +44,8 @@ public class PositionalServoProgrammer extends LinearOpMode { robot.spin1.setPower(spindexPow); robot.spin2.setPower(-spindexPow); } - if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ - double pos = servo.setTurrPos(turretPos); + if (turretPos != 0.501 && !servo.turretEqual(turretPos, robot.turr1Pos.getCurrentPosition())){ + double pos = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition()); robot.turr1.setPower(pos); robot.turr2.setPower(-pos); } else if (mode == 0){ @@ -72,8 +72,8 @@ public class PositionalServoProgrammer extends LinearOpMode { // 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 pos", servo.getSpinPos(robot.spin1Pos.getVoltage())); + TELE.addData("turret pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition())); TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage()); TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); TELE.addData("hood pos", robot.hood.getPosition()); 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 967fa78..f2821ec 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 @@ -22,39 +22,38 @@ public class Servos { public static double turret_scalar = 1.009; public static double turret_restPos = 0.0; - public Servos(HardwareMap hardwareMap) { - robot = new Robot(hardwareMap); + public Servos() { 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() { - return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); + public double getSpinPos(double voltage) { + return spin_scalar * ((voltage - spin_restPos) / 3.3); } //TODO: PID warp so 0 and 1 are usable positions - public double setSpinPos(double pos) { + public double setSpinPos(double pos, double voltage) { spinPID.setPIDF(spinP, spinI, spinD, spinF); - return spinPID.calculate(this.getSpinPos(), pos); + return spinPID.calculate(this.getSpinPos(voltage), pos); } - public boolean spinEqual(double pos) { - return Math.abs(pos - this.getSpinPos()) < 0.02; + public boolean spinEqual(double pos, double voltage) { + return Math.abs(pos - this.getSpinPos(voltage)) < 0.02; } - public double getTurrPos() { - return robot.turr1Pos.getCurrentPosition(); + public double getTurrPos(double apos) { + return apos; } - public double setTurrPos(double pos) { + public double setTurrPos(double pos, double apos) { turretPID.setPIDF(turrP, turrI, turrD, turrF); - return spinPID.calculate(this.getTurrPos(), pos); + return spinPID.calculate(this.getTurrPos(apos), pos); } - public boolean turretEqual(double pos) { - return Math.abs(pos - this.getTurrPos()) < 0.01; + public boolean turretEqual(double pos, double apos) { + return Math.abs(pos - this.getTurrPos(apos)) < 0.01; } }