From 65f18d233a3da26d0e4da4b3b6308977846dcf87 Mon Sep 17 00:00:00 2001 From: Soupborsh <100529201+Soupborsh@users.noreply.github.com> Date: Sat, 25 Oct 2025 12:19:05 +0500 Subject: [PATCH 01/61] Fix direction typo(change right to left) Robot actually goes to the left(and documentation says so) but the code comments and driver hub text mistakenly call that it goes to the right leading to confusion. This fixes this typo. --- .../org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 9f5d275f03ff..c50907c3aa71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -617,7 +617,7 @@ public void loop() { /** * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot - * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting * them to zero power. The deceleration, or negative acceleration, is then measured until the robot * stops. The accelerations across the entire time the robot is slowing down is then averaged and * that number is then printed. This is used to determine how the robot will decelerate in the @@ -644,7 +644,7 @@ public void init() {} /** This initializes the drive motors as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); telemetryM.debug("Make sure you have enough room."); telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); From 016e9d6ee5afa4956c33fe017d0091a1a7931776 Mon Sep 17 00:00:00 2001 From: vlad1k337 <147236370+vlad1k337@users.noreply.github.com> Date: Mon, 24 Nov 2025 23:06:15 -0600 Subject: [PATCH 02/61] feat: add telemetry to PIDF Tuners for error Allows it to be graphed on panels. Ref: #31 --- .../ftc/teamcode/pedroPathing/Tuning.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index c58d7fed70f2..29a29ecd876b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -15,6 +15,7 @@ import com.bylazar.field.Style; import com.bylazar.telemetry.PanelsTelemetry; import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.ErrorCalculator; import com.pedropathing.follower.Follower; import com.pedropathing.geometry.*; import com.pedropathing.math.*; @@ -790,6 +791,9 @@ public void loop() { } telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); + telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); telemetryM.update(telemetry); } } @@ -862,6 +866,8 @@ public void loop() { } telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); telemetryM.update(telemetry); } } @@ -905,7 +911,7 @@ public void init_loop() { public void start() { follower.deactivateAllPIDFs(); follower.activateDrive(); - + forwards = follower.pathBuilder() .setGlobalDeceleration() .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) @@ -941,6 +947,8 @@ public void loop() { } telemetryM.debug("Driving forward?: " + forward); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); telemetryM.update(telemetry); } } From 081ae9bfc067060fb7fc2e380729822e0bb202f9 Mon Sep 17 00:00:00 2001 From: Professor348 <141444315+Professor348@users.noreply.github.com> Date: Tue, 2 Dec 2025 20:24:56 -0500 Subject: [PATCH 03/61] Got NextFTC's command pathing working, did not use on dec6th auto. --- .../LOADCode/Main_/Auto_/Auto_Dec6th.java | 25 +++++++- .../LOADCode/Main_/Auto_/Auto_Main_.java | 47 ++++++++++----- .../Auto_/NextFTC_Pedro_Example_Auto.java | 54 +++++++++++++++++ .../LOADCode/Main_/Hardware_/Commands.java | 4 +- .../LOADCode/Main_/Teleop_/Teleop_Main_.java | 60 +++++++++++++++++++ .../Main_/Teleop_/Teleop_Outreach_.java | 16 ----- .../ftc/teamcode/pedroPathing/Constants.java | 4 +- 7 files changed, 174 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java index c1fae0517a70..a50fec1b7185 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java @@ -1,9 +1,15 @@ package org.firstinspires.ftc.teamcode.LOADCode.Main_.Auto_; // make sure this aligns with class location +import static org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass.selectedAlliance; + import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.skeletonarmy.marrow.prompts.OptionPrompt; +import com.skeletonarmy.marrow.prompts.Prompter; + +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; @Autonomous(name = "Auto_Dec6th", group = "TestAuto", preselectTeleOp="Teleop_Main_") public class Auto_Dec6th extends OpMode { @@ -13,6 +19,9 @@ public class Auto_Dec6th extends OpMode { DcMotorEx BL; DcMotorEx BR; + // Create the prompter object for selecting Alliance and Auto + Prompter prompter = null; + /** * Copied over from LinearOpMode. * @param milliseconds The number of milliseconds to sleep. @@ -35,12 +44,26 @@ public void init() { FL.setDirection(DcMotorSimple.Direction.REVERSE); BL.setDirection(DcMotorSimple.Direction.REVERSE); + + prompter = new Prompter(this); + prompter.prompt("alliance", + new OptionPrompt<>("Select Alliance", + LoadHardwareClass.Alliance.RED, + LoadHardwareClass.Alliance.BLUE + )); + prompter.onComplete(() -> { + selectedAlliance = prompter.get("alliance"); + telemetry.addData("Selection", "Complete"); + telemetry.addData("Alliance", selectedAlliance); + telemetry.update(); + } + ); } /** This method is called continuously after Init while waiting for "play". **/ @Override public void init_loop() { - + prompter.run(); } /** This method is called once at the start of the OpMode. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index 45ccb6d84d3f..dac8c7759eb7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Commands; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.Pedro_Paths; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; @@ -19,6 +20,7 @@ import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelGroup; import dev.nextftc.core.commands.groups.SequentialGroup; +import dev.nextftc.extensions.pedro.FollowPath; import dev.nextftc.extensions.pedro.PedroComponent; import dev.nextftc.ftc.NextFTCOpMode; @@ -27,7 +29,7 @@ public class Auto_Main_ extends NextFTCOpMode { // Define other PedroPathing constants - private final Pose startPose = new Pose(87, 8.8, Math.toRadians(90)); // Start Pose of our robot. + private Pose startPose = new Pose(87, 8.8, Math.toRadians(90)); // Start Pose of our robot. private enum Auto { LEAVE_NEAR_LAUNCH, @@ -44,12 +46,15 @@ private enum Auto { // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); - @Override - public void onInit() { - // Initialize all hardware of the robot + public Auto_Main_() { addComponents( new PedroComponent(Constants::createFollower) ); + } + + @Override + public void onInit() { + // Initialize all hardware of the robot Robot.init(startPose); Robot.drivetrain.follower = PedroComponent.follower(); @@ -69,6 +74,9 @@ public void onInit() { selectedAlliance = prompter.get("alliance"); selectedAuto = prompter.get("auto"); telemetry.addData("Selection", "Complete"); + telemetry.addData("Alliance", selectedAlliance); + telemetry.addData("Auto", selectedAuto); + telemetry.update(); } ); Robot.drivetrain.paths.buildPaths(selectedAlliance); @@ -84,25 +92,28 @@ public void onStartButtonPressed() { // Schedule the proper auto switch (selectedAuto) { case LEAVE_NEAR_LAUNCH: - Leave_Near_Launch().invoke(); + Leave_Near_Launch().schedule(); + startPose = new Pose(0,0,0); return; case LEAVE_FAR_HP: - Leave_Far_HP().invoke(); + Leave_Far_HP().schedule(); + startPose = new Pose(0,0,0); return; + case MOVEMENT_RP_DEC6: + Movement_RP_Dec6th().schedule(); + startPose = new Pose(0,0,0); } } @Override public void onUpdate() { - if (turretOn){ - switch (selectedAlliance) { - case RED: - Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), true); - return; - case BLUE: - Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), false); - return; - } + switch (selectedAlliance) { + case RED: + Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), true); + return; + case BLUE: + Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), false); + return; } telemetry.addData("selectedAuto", selectedAuto); @@ -165,4 +176,10 @@ private Command Leave_Near_Launch() { Commands.runPath(Robot.drivetrain.paths.start1_to_leave1, true, 0.6) ); } + + private Command Movement_RP_Dec6th(){ + return new SequentialGroup( + new FollowPath(Robot.drivetrain.paths.basicMoveRPPath, true, 0.5) + ); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java new file mode 100644 index 000000000000..bcbe78d74df4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java @@ -0,0 +1,54 @@ +//package org.firstinspires.ftc.teamcode.LOADCode.Main_.Auto_; +// +//import com.acmerobotics.roadrunner.path.BezierLine; +//import com.pedropathing.geometry.Pose; +//import com.pedropathing.paths.PathChain; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +// +//import dev.nextftc.core.commands.groups.SequentialGroup; +//import dev.nextftc.core.components.SubsystemComponent; +//import dev.nextftc.extensions.pedro.FollowPath; +//import dev.nextftc.extensions.pedro.PedroComponent; +//import dev.nextftc.ftc.NextFTCOpMode; +// +//@Autonomous(name = "blueBottom") +//public class NextFTC_Pedro_Example_Auto extends NextFTCOpMode { +// private final Pose startPose = new Pose(85.5, 8.3, Math.toRadians(90.0)); +// private final Pose depositPose = new Pose(84.3, 61.9, Math.toRadians(0.0)); +// private final Pose curvePoint = new Pose(138.2, 48.1); +// +// private PathChain skib; +// +// public NextFTC_Pedro_Example_Auto() { +// addComponents( +// new PedroComponent(Constants::createFollower) +// ); +// } +// +// private void buildPaths() { +// skib = follower.pathBuilder() +// .addPath(new BezierLine(startPose, depositPose)) +// .setLinearHeadingInterpolation(startPose.heading, depositPose.heading) +// .build(); +// } +// +// public Command getSecondRoutine() { +// return new SequentialGroup( +// new FollowPath(skib) +// ); +// } +// +// @Override +// public void onInit() { +// follower.setMaxPower(0.7); +// follower.setStartingPose(startPose); +// buildPaths(); +// } +// +// @Override +// public void onStartButtonPressed() { +// getSecondRoutine().run(); +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java index 49131376c7c5..d363fa3a21ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java @@ -17,10 +17,10 @@ public class Commands { public static Object intakeSystem = null; public static Command runPath(PathChain path, boolean holdEnd) { - return new FollowPath(path, holdEnd).requires(driveSystem); + return new FollowPath(path, holdEnd); } public static Command runPath(PathChain path, boolean holdEnd, double maxPower) { - return new FollowPath(path, holdEnd, maxPower).requires(driveSystem); + return new FollowPath(path, holdEnd, maxPower); } public static Command setFlywheelState(LoadHardwareClass Robot, Turret.flywheelstate state) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index 9fde0019cdff..3c2a398a3c67 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -57,6 +57,7 @@ public class Teleop_Main_ extends LinearOpMode { public static double DylanStickDeadzones = 0.2; public int shootingState = 0; public TimerEx shootingDelay = new TimerEx(1); + public static double autoaimmultiplier = 100; Prompter prompter = null; @@ -194,6 +195,65 @@ public void Gamepad1(){ ); } + public void Gamepad1Dec6(){ + + Robot.drivetrain.speedMultiplier = 0.66; + if (gamepad1.left_trigger >= 0.5) { + Robot.drivetrain.speedMultiplier -= 0.33; + } + if (gamepad1.right_trigger >= 0.5){ + Robot.drivetrain.speedMultiplier += 0.33; + } + + double turnPower = gamepad1.right_stick_x; + double turnMult = 2; + if (gamepad1.left_stick_y == 0 && gamepad1.left_stick_x == 0){ + turnMult = 1; + } + turnPower = turnPower/turnMult; + + double targetHeading = 0; + telemetry.addData("Alliance", LoadHardwareClass.selectedAlliance); + if (gamepad1.right_stick_button){ + switch (LoadHardwareClass.selectedAlliance){ + case RED: + turnPower = Robot.drivetrain.follower.getHeading() - + Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), false); + targetHeading = Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), false); + telemetry.addData("Pose", Robot.drivetrain.follower.getPose()); + telemetry.addData("Target Heading", targetHeading); + return; + case BLUE: + turnPower = Robot.drivetrain.follower.getHeading() - + Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), true); + targetHeading = Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), true); + telemetry.addData("Pose", Robot.drivetrain.follower.getPose()); + telemetry.addData("Target Heading", targetHeading); + return; + } + turnPower = turnPower/autoaimmultiplier; + } + telemetry.addData("TurnPower", turnPower); + + if (gamepad1.guide){ + switch (LoadHardwareClass.selectedAlliance){ + case RED: + Robot.drivetrain.follower.setPose(new Pose(144-8.25,7.25, Math.toRadians(90))); + return; + case BLUE: + Robot.drivetrain.follower.setPose(new Pose(8.25,7.25, Math.toRadians(90))); + return; + } + } + + Robot.drivetrain.pedroMecanumDrive( + gamepad1.left_stick_y, + gamepad1.left_stick_x, + turnPower, + true + ); + } + /** *
Flywheel ToggleHood Up OverrideHood Down OverrideN/AN/AN/AN/ATranfer Belt IN ONLYIntake Direction/PowerN/AN/AN/AN/AN/AKicker/Flap/FeederN/AFlywheel ToggleN/AN/AN/AN/AN/AN/AN/AN/AMode.INTAKINGMode.SHOOTINGMode.REVERSINGMode.OFFintakeMode.INTAKINGintakeMode.SHOOTINGintakeMode.REVERSINGintakeMode.OFFMode.INTAKINGMode.SHOOTINGMode.REVERSINGMode.OFFintakeMode.INTAKINGintakeMode.SHOOTINGintakeMode.REVERSINGintakeMode.OFF
+ * The spline is guaranteed to pass through each control point exactly. Moreover, assuming the control points are
+ * monotonic (Y is non-decreasing or non-increasing) then the interpolated values will also be monotonic.
+ *
+ * @throws IllegalArgumentException if the X or Y arrays are null, have different lengths or have fewer than 2 values.
+ */
+ //public static LUTWithInterpolator createLUT(List uLR8_o65m9toKLQ5`Y(ipT{XE{JCxoV
z^XWq!?uJsq`z5Qx@p=_=SDhZ3H_}QP3MILpUQmX-@$$ZqE_>I~y>hK;t)6HiZ7=u|
z0Rdit<1mrA$?hy|ALqe~cYB%H;c&MVGCT`HGx_zp)W6ML#Nb_kq{nxoB4jr`m}T^<
zGqvPD4Fz+RK?fvsU>=nAv)8A>bbMV({5
zg;9$1Zj3X7SspdQ@0f)jxS ne-Nei3+J@~zU2biy0yjussi
z-Chhe^W>_k!rD!=gdsblLRdJ}TlQ>!5QX*b8~Y8De?}~V2^sHp%S7F74KcH@@i9I<
zdNng+mXOfRzxh4>z_m!tvSQRjDW~@2B=_Oppd8BrnT0j}8JL*Rk&^?PN~GPJaS5=;
zpaSL&CNL$$tyi&asKi3rIy%zBBkmS%!J6*Kw&A=qm1$&bocZx%C1>A#GUm7``>Huk
z0fE!S#zt$>CZTfI<4!KZ&eX|~fpP|}xUtS>vOc!+5@~V=Gr5I2Jaz-Wvu*0Pf*Kmm
zyFYy*OiawUN7WimcHH1|)TapM@qxv~l|yR9;e1+ZLx|gEPSslui@0Lp %IU@p@P9ah#{_<9;}wpswx$egqo=9!EcA
zQr@>Na&d6s)XIHq`!zL1HtDr!>hk=4R72hVgjMu|!s`7ALLKh}ayo)wj@{j`iGD
zhjqW#zI11st3zMr!_jgh@Km9BkjwGjWS|g1#x%7Jhuc7bu585zeFJ@!0yRp4K-fIW
zr^j8E&BR)o>nhCpKF%3#IU>&2ZBSJ&NTMiBn8;9i>N*r8U=ORVu7<=EDguUV!(bi+
z**$P>b83$&Jf{31X@&Iez*8`ocXrY&NMfGGWSc|_-0i`m!SF5@muhZ5n>My<3~2uM
zTIE3D6VlUYA4QVTmct>ioQc|>0OBR=B7wD^WD_~_U4#}SplkqH{R<0~ZQ^w$>F2U>
zZ=2DvDV*<^#rQ1AIa#e&!Z#5gFPNO8{>V5aEZhbhrM41n?ofr}5p<=n8eDPO=u7<7
zKVG+ej39H_UmLHn+?-if%%TY)aWcvZ%IsT3zxFZM(%S5Br= Fz~$ow0E5_xs-OoU?v-?Q5fxHRpWhGsn2c9TOAr>PNSjV%oRwg{vs=QZg}p
z7Wmwh&~s$^lHehb2bdcSSax12z*{jp)v`HWJ$C^6 0+^p# %_A+S)-Df5jV2s7kwp_W!S-&S^9%Smi*J7KGN%
zo3=>PRvrBA`#sk&dfy@EhW8g?a^14aXr7a41qB7)zthvud}^E?THS@WG!sOgqCO)%
zsLkS3b4hD(l>`HOH!R{%y0qP;_HyOOuthO}`ml<9s^6u7Nw7cr`XmNrufA|_Xu6Zp
zOTX0bFmLghcK}`Jd#9a}H{yQ=QGOC$;Iza8x-rnJf!Wt&osYFS-}$f8!Ts+N9QGDe
z;Ew`MWGndk&-^~1j*`5#u;}|Q7eFKvtaK<_EQ-|?GK)sYn#g1r=8q0-967rtL5M-w
zqQeAS6RNCdeG|`BZPk+Uy$q}R-mdf}EmbgcsfJ@Fy}0Tog+oS0rjfSvJ9%Jv(fkRF
zYdPgWDFLHKMOn_1G3nC{Zy%omXS<1!zjY`Uh&l#Dg0WeNgI`8`p$-gulwdGePPAaP
zl^(>uO+}8XM$`3zKOP%*vC*kB>z5rV)Ek7w8TK#wi>DOj%V{f*&qp#N556^#*#Doz
zahOow^qCGJOAEb)Dx0xHZoijRTzldt{`}P9JLTU}+^>GhK=C7CU%CY2_Pt!LR?T92
z(jYt>9AR|H5|cmKkZ3>N*%^ct79L(Y`N)@Ac$7!>d(FZAWD7L|=fW=c>CljU?W>TelemetryObject.
+ * There should only be one of these per program.
+ */
+ public class TelemetryManager{
+ ListTelemetryManager.
+ * It will contain all the relevant data for lookup and sorting, as well as functionality for live runtime editing of keys and values
+ */
+ public class TelemetryObject{
+
+ Object name;
+ Object key;
+ Object value;
+ telemetrySortCategory category;
+
+ public TelemetryObject(Object initialName,
+ Object initialKey,
+ Object initialValue,
+ telemetrySortCategory telemCategory,
+ TelemetryManager telemetryManagerParent){
+ name = initialName;
+ key = initialKey;
+ value = initialValue;
+ category = telemCategory;
+ }
+
+ public Object getName(){
+ return name;
+ }
+ public Object getKey(){
+ return key;
+ }
+ public Object getValue(){
+ return value;
+ }
+ public telemetrySortCategory getTelemCategory(){
+ return category;
+ }
+
+ }
}
From a3c9f025544936a2fd0d54da3396997d544c67f7 Mon Sep 17 00:00:00 2001
From: Professor348 <141444315+Professor348@users.noreply.github.com>
Date: Mon, 15 Dec 2025 18:17:13 -0500
Subject: [PATCH 11/61] Worked on cleaning up some hardware functions, tested
REV Color Sensor, and did some flywheel testing.
---
.../LOADCode/Main_/Auto_/Auto_Main_.java | 4 +--
.../Main_/Hardware_/Actuators_/Devices.java | 29 +++++++++++++++
.../Main_/Hardware_/Actuators_/Intake.java | 13 +++++--
.../Main_/Hardware_/Actuators_/Turret.java | 36 +++++++++----------
.../LOADCode/Main_/Hardware_/Commands.java | 2 +-
.../LOADCode/Main_/Teleop_/Teleop_Main_.java | 16 ++++-----
.../Main_/Teleop_/Teleop_Outreach_.java | 16 +++++++--
.../ftc/teamcode/LOADCode/Notes and TODOs | 2 +-
8 files changed, 82 insertions(+), 36 deletions(-)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
index 3567279d71bb..5cfbc244226d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
@@ -108,10 +108,10 @@ public void onStartButtonPressed() {
public void onUpdate() {
switch (selectedAlliance) {
case RED:
- Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), true);
+ Robot.turret.updateAimbot(Robot, true);
return;
case BLUE:
- Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), false);
+ Robot.turret.updateAimbot(Robot, false);
return;
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
index 22daf0c179e9..6db114ed4315 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
@@ -7,8 +7,13 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import com.qualcomm.robotcore.hardware.Servo;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
import dev.nextftc.control.ControlSystem;
import dev.nextftc.control.KineticState;
import dev.nextftc.control.feedback.PIDCoefficients;
@@ -263,4 +268,28 @@ public double getAngle(){
return servo.getPosition();
}
}
+
+ public static class REVColorSensorV3Class {
+ private NormalizedColorSensor sensor;
+
+ public void init(@NonNull OpMode opmode, String sensorName){
+ sensor = opmode.hardwareMap.get(NormalizedColorSensor.class, sensorName);
+ }
+
+ public NormalizedRGBA getNormalizedColors(){
+ return sensor.getNormalizedColors();
+ }
+
+ public double getGain(){
+ return sensor.getGain();
+ }
+
+ public void setGain(double gain){
+ sensor.setGain((float) gain);
+ }
+
+ public double getDistance(DistanceUnit units){
+ return ((DistanceSensor) sensor).getDistance(units);
+ }
+ }
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
index 13eff1a3a1f7..aa619ed9d38d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
@@ -7,9 +7,13 @@
public class Intake {
// RESET THESE TO PRIVATE AFTER DECEMBER 6TH!
- public final Devices.DcMotorExClass intake = new Devices.DcMotorExClass();
- public final Devices.CRServoClass belt = new Devices.CRServoClass();
- public final Devices.ServoClass transfer = new Devices.ServoClass();
+ private final Devices.DcMotorExClass intake = new Devices.DcMotorExClass();
+ private final Devices.CRServoClass belt = new Devices.CRServoClass();
+ private final Devices.ServoClass transfer = new Devices.ServoClass();
+ public final Devices.REVColorSensorV3Class color1 = new Devices.REVColorSensorV3Class();
+ public final Devices.REVColorSensorV3Class color2 = new Devices.REVColorSensorV3Class();
+ public final Devices.REVColorSensorV3Class color3 = new Devices.REVColorSensorV3Class();
+ public final Devices.REVColorSensorV3Class color4 = new Devices.REVColorSensorV3Class();
public enum intakeMode {
INTAKING,
@@ -27,11 +31,14 @@ public void init(OpMode opmode){
intake.init(opmode, "intake");
belt.init(opmode, "belt");
transfer.init(opmode, "transfer");
+ color1.init(opmode, "color1");
intake.setZeroPowerBehaviour(DcMotor.ZeroPowerBehavior.BRAKE);
intake.setDirection(DcMotorSimple.Direction.REVERSE);
belt.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ color1.setGain(2);
}
/**
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
index 9ec43097d921..a748086bd5d0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_;
+import androidx.annotation.NonNull;
+
import com.bylazar.configurables.annotations.Configurable;
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@@ -7,6 +9,8 @@
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
+import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
+
import dev.nextftc.control.feedback.PIDCoefficients;
import dev.nextftc.control.feedforward.BasicFeedforwardParameters;
@@ -16,19 +20,18 @@ public class Turret {
// Hardware definitions
public final Devices.DcMotorExClass rotation = new Devices.DcMotorExClass();
public final Devices.DcMotorExClass flywheel = new Devices.DcMotorExClass();
- public final Devices.DcMotorExClass flywheel2 = new Devices.DcMotorExClass();
- public final Devices.ServoClass hood = new Devices.ServoClass();
- public final Devices.ServoClass gate = new Devices.ServoClass();
+ private final Devices.DcMotorExClass flywheel2 = new Devices.DcMotorExClass();
+ private final Devices.ServoClass hood = new Devices.ServoClass();
+ private final Devices.ServoClass gate = new Devices.ServoClass();
// Turret PID coefficients
public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.06, 0, 0); // 223RPM
//public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.3, 0, 0.007); // 1150RPM
// Flywheel PID coefficients
- //public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.0003, 0.0001, 0.0001);
- //public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.00002899,0,0);
- public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.00005, 0, 0);
- public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.0003,0,0);
+ // 4500RPM
+ public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.0002, 0, 0);
+ public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.000026,0,0);
public enum gatestate {
OPEN,
@@ -76,15 +79,14 @@ public void updatePIDs(){
}
/**
- * @param robotPose The pose of the robot, gotten from PedroPathing's localization
+ * @param robot The pose of the robot, gotten from PedroPathing's localization
* @param targetRedGoal Set this to true to target the red goal, otherwise targets the blue goal.
*/
- public void updateAimbot(Pose robotPose, boolean targetRedGoal){
- rotation.setAngle(calcLocalizer(robotPose, targetRedGoal));
+ public void updateAimbot(@NonNull LoadHardwareClass robot, boolean targetRedGoal){
+ rotation.setAngle(calcLocalizer(robot.drivetrain.follower.getPose(), targetRedGoal));
}
-
- public void setGate(gatestate state){
+ public void setGateState(gatestate state){
if (state == gatestate.OPEN){
gate.setAngle(0.5);
}else if (state == gatestate.CLOSED){
@@ -137,23 +139,21 @@ public double calcLocalizer (Pose robotPose, boolean targetRedGoal){
* @param rpm
* RPM Range [0,6000]
*/
- public void setFlywheelRPM(double rpm){
+ private void setFlywheelRPM(double rpm){
flywheel.setRPM(rpm);
flywheel2.setPower(flywheel.getPower());
}
- /**
- * @return double flywheel.getRPM(); - RPM Range [0,6000]
- */
public double getFlywheelRPM(){
return flywheel.getRPM();
}
/**
- * Sets the target state of the Flywheel
+ * Sets the target state of the Flywheel.
+ * updateFlywheel() must be called every loop for this to be effective.
* @param state The state to set the flywheel to (ON/OFF)
*/
- public void setFlywheel(flywheelstate state){
+ public void setFlywheelState(flywheelstate state){
flywheelState = state;
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
index 9ecdc2411d6e..4a91ea4f26df 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
@@ -26,7 +26,7 @@ public static Command runPath(PathChain path, boolean holdEnd, double maxPower)
public static Command setFlywheelState(LoadHardwareClass Robot, Turret.flywheelstate state) {
return new LambdaCommand("setFlywheelState()")
.setInterruptible(false)
- .setStart(() -> Robot.turret.setFlywheel(state))
+ .setStart(() -> Robot.turret.setFlywheelState(state))
.setIsDone(() -> {
if (state == Turret.flywheelstate.ON){
return Robot.turret.getFlywheelRPM() > 5900;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
index 44511b6e7d33..a5f79ab5faa7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
@@ -252,9 +252,9 @@ public void Gamepad2() {
// Turret Aimbot
if (selectedAlliance == Alliance.RED) {
- Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), true);
+ Robot.turret.updateAimbot(Robot, true);
} else if (selectedAlliance == Alliance.BLUE) {
- Robot.turret.updateAimbot(Robot.drivetrain.follower.getPose(), false);
+ Robot.turret.updateAimbot(Robot, false);
}
//Intake Controls (Left Stick Y)
@@ -271,9 +271,9 @@ public void Gamepad2() {
//Flywheel Toggle Control (Y Button)
if (gamepad2.yWasPressed()) {
if (Robot.turret.flywheelState == flywheelstate.OFF) {
- Robot.turret.setFlywheel(flywheelstate.ON);
+ Robot.turret.setFlywheelState(flywheelstate.ON);
} else {
- Robot.turret.setFlywheel(flywheelstate.OFF);
+ Robot.turret.setFlywheelState(flywheelstate.OFF);
}
}
}
@@ -299,7 +299,7 @@ public void Gamepad2() {
return;
case 1:
Robot.intake.setMode(intakeMode.INTAKING);
- Robot.turret.setGate(gatestate.OPEN);
+ Robot.turret.setGateState(gatestate.OPEN);
telemetry.addData("Shooting State", "STARTED");
return;
case 2:
@@ -316,14 +316,12 @@ public void Gamepad2() {
telemetry.addData("Shooting State", "DELAY");
return;
case 4:
- Robot.turret.setFlywheel(flywheelstate.OFF);
- Robot.turret.setGate(gatestate.CLOSED);
+ Robot.turret.setFlywheelState(flywheelstate.OFF);
+ Robot.turret.setGateState(gatestate.CLOSED);
Robot.intake.setMode(intakeMode.OFF);
Robot.intake.setTransfer(transferState.DOWN);
telemetry.addData("Shooting State", "RESET");
shootingState = 0;
}
-
-
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Outreach_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Outreach_.java
index d75c5851e3f1..50167883006f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Outreach_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Outreach_.java
@@ -34,8 +34,10 @@
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
@TeleOp(name="Teleop_Outreach_", group="TeleOp")
@@ -80,9 +82,9 @@ public void runOpMode() {
);
if (gamepad1.x){
- Robot.turret.rotation.setAngle(180);
+ //Robot.turret.rotation.setAngle(180);
}else{
- Robot.turret.rotation.setAngle(Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), true));
+ //Robot.turret.rotation.setAngle(Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), true));
}
telemetry.addData("Target Angle", Robot.turret.calcLocalizer(Robot.drivetrain.follower.getPose(), true));
telemetry.addData("Turret Angle", Robot.turret.rotation.getAngleAbsolute());
@@ -98,6 +100,16 @@ public void runOpMode() {
Robot.turret.setHood(hoodAngle);
telemetry.addData("Hood Angle", Robot.turret.getHood());
+ telemetry.addLine();
+ telemetry.addData("Gain", Robot.intake.color1.getGain());
+ telemetry.addData("Distance", Robot.intake.color1.getDistance(DistanceUnit.INCH));
+ telemetry.addData("Color", Robot.intake.color1.getNormalizedColors().toColor());
+ NormalizedRGBA colors = Robot.intake.color1.getNormalizedColors();
+ telemetry.addData("R", colors.red);
+ telemetry.addData("G", colors.green);
+ telemetry.addData("B", colors.blue);
+
+ telemetry.addLine();
telemetry.addData("Robot Pose X", Math.round(Robot.drivetrain.follower.getPose().getX()));
telemetry.addData("Robot Pose Y", Math.round(Robot.drivetrain.follower.getPose().getY()));
telemetry.addData("Robot Pose H", Math.round(Math.toDegrees(Robot.drivetrain.follower.getPose().getHeading())));
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
index 3186bc743189..1cbabd505c3d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
@@ -10,7 +10,7 @@
//TODO, Add a more advanced telemetry handler for better organization, readability, and debugging
-//TODO, Add proper functionality for manual turret control to Turret.java
+//DONE, Add proper functionality for manual turret control to Turret.java
//DONE, Shoot mode state machine Off->On->LastShot->Delay->ResetActuators->Off etc. cycled with B button on Gamepad2
From 5871b1665b8c56dc052117a640a90c94596095ae Mon Sep 17 00:00:00 2001
From: Professor348 <141444315+Professor348@users.noreply.github.com>
Date: Tue, 16 Dec 2025 21:05:34 -0500
Subject: [PATCH 12/61] Tuned PedroPathing and worked on the auto system
---
.../LOADCode/Main_/Auto_/Auto_Dec6th.java | 3 +-
.../LOADCode/Main_/Auto_/Auto_Main_.java | 60 +--
.../LOADCode/Main_/Hardware_/Commands.java | 1 +
.../Drivetrain_/MecanumDrivetrainClass.java | 24 +-
.../Hardware_/Drivetrain_/Pedro_Paths.java | 346 +++++++++---------
.../Main_/Hardware_/LoadHardwareClass.java | 18 +-
.../LOADCode/Main_/Teleop_/Teleop_Main_.java | 1 +
.../ftc/teamcode/LOADCode/Notes and TODOs | 14 +-
.../ftc/teamcode/pedroPathing/Constants.java | 21 +-
.../ftc/teamcode/pedroPathing/Tuning.java | 19 +-
10 files changed, 268 insertions(+), 239 deletions(-)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java
index a50fec1b7185..fc0786fae05d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Dec6th.java
@@ -3,6 +3,7 @@
import static org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass.selectedAlliance;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -10,7 +11,7 @@
import com.skeletonarmy.marrow.prompts.Prompter;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
-
+@Disabled
@Autonomous(name = "Auto_Dec6th", group = "TestAuto", preselectTeleOp="Teleop_Main_")
public class Auto_Dec6th extends OpMode {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
index 5cfbc244226d..4ae1a0e636f2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
@@ -2,15 +2,17 @@
import static org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass.selectedAlliance;
+import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.Path;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.skeletonarmy.marrow.prompts.OptionPrompt;
import com.skeletonarmy.marrow.prompts.Prompter;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Commands;
+import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.Pedro_Paths;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -23,7 +25,6 @@
import dev.nextftc.extensions.pedro.PedroComponent;
import dev.nextftc.ftc.NextFTCOpMode;
-@Disabled
@Autonomous(name = "Auto_Main_", group = "Main", preselectTeleOp="Teleop_Main_")
public class Auto_Main_ extends NextFTCOpMode {
@@ -33,11 +34,13 @@ public class Auto_Main_ extends NextFTCOpMode {
private enum Auto {
LEAVE_NEAR_LAUNCH,
LEAVE_FAR_HP,
- MOVEMENT_RP_DEC6
+ TEST_AUTO
}
private Auto selectedAuto = null;
- private boolean turretOn = false;
+ private boolean turretOn = true;
+
+ Pedro_Paths paths = new Pedro_Paths();
// Create the prompter object for selecting Alliance and Auto
Prompter prompter = null;
@@ -53,10 +56,6 @@ public Auto_Main_() {
@Override
public void onInit() {
- // Initialize all hardware of the robot
- Robot.init(startPose);
- Robot.drivetrain.follower = PedroComponent.follower();
-
prompter = new Prompter(this);
prompter.prompt("alliance",
new OptionPrompt<>("Select Alliance",
@@ -67,7 +66,7 @@ public void onInit() {
new OptionPrompt<>("Select Auto",
Auto.LEAVE_NEAR_LAUNCH,
Auto.LEAVE_FAR_HP,
- Auto.MOVEMENT_RP_DEC6
+ Auto.TEST_AUTO
));
prompter.onComplete(() -> {
selectedAlliance = prompter.get("alliance");
@@ -78,7 +77,6 @@ public void onInit() {
telemetry.update();
}
);
- Robot.drivetrain.paths.buildPaths(selectedAlliance);
}
@Override
@@ -92,27 +90,29 @@ public void onStartButtonPressed() {
switch (selectedAuto) {
case LEAVE_NEAR_LAUNCH:
Leave_Near_Launch().schedule();
- startPose = new Pose(0,0,0);
return;
case LEAVE_FAR_HP:
Leave_Far_HP().schedule();
- startPose = new Pose(0,0,0);
return;
- case MOVEMENT_RP_DEC6:
- Movement_RP_Dec6th().schedule();
- startPose = new Pose(0,0,0);
+ case TEST_AUTO:
+ test_Auto().schedule();
}
+ // Initialize all hardware of the robot
+ Robot.init(startPose, PedroComponent.follower());
+ paths.buildPaths(selectedAlliance, PedroComponent.follower());
}
@Override
public void onUpdate() {
- switch (selectedAlliance) {
- case RED:
- Robot.turret.updateAimbot(Robot, true);
- return;
- case BLUE:
- Robot.turret.updateAimbot(Robot, false);
- return;
+ if (turretOn){
+ switch (selectedAlliance) {
+ case RED:
+ Robot.turret.updateAimbot(Robot, true);
+ return;
+ case BLUE:
+ Robot.turret.updateAimbot(Robot, false);
+ return;
+ }
}
telemetry.addData("selectedAuto", selectedAuto);
@@ -127,10 +127,11 @@ public void onUpdate() {
*/
private Command Leave_Far_HP() {
turretOn = true;
+ startPose = Robot.drivetrain.paths.farStart;
return new SequentialGroup(
Commands.setFlywheelState(Robot, Turret.flywheelstate.ON),
new ParallelGroup(
- new WaitUntil(() -> Robot.turret.getFlywheelRPM() > 5900),
+ new WaitUntil(() -> Robot.turret.getFlywheelRPM() > Turret.onSpeed),
new Delay(5)
),
Commands.setIntakeMode(Robot, Intake.intakeMode.SHOOTING),
@@ -144,7 +145,7 @@ private Command Leave_Far_HP() {
Commands.setTransferState(Robot, Intake.transferState.DOWN),
Commands.setIntakeMode(Robot, Intake.intakeMode.OFF),
Commands.setFlywheelState(Robot, Turret.flywheelstate.OFF),
- Commands.runPath(Robot.drivetrain.paths.start2_to_leave3, true, 0.6)
+ Commands.runPath(Robot.drivetrain.paths.farStart_to_farLeave, true, 0.6)
);
}
@@ -155,10 +156,11 @@ private Command Leave_Far_HP() {
*/
private Command Leave_Near_Launch() {
turretOn = true;
+ startPose = Robot.drivetrain.paths.nearStart;
return new SequentialGroup(
Commands.setFlywheelState(Robot, Turret.flywheelstate.ON),
new ParallelGroup(
- new WaitUntil(() -> Robot.turret.getFlywheelRPM() > 5900),
+ new WaitUntil(() -> Robot.turret.getFlywheelRPM() > Turret.onSpeed),
new Delay(5)
),
Commands.setIntakeMode(Robot, Intake.intakeMode.SHOOTING),
@@ -172,13 +174,11 @@ private Command Leave_Near_Launch() {
Commands.setTransferState(Robot, Intake.transferState.DOWN),
Commands.setIntakeMode(Robot, Intake.intakeMode.OFF),
Commands.setFlywheelState(Robot, Turret.flywheelstate.OFF),
- Commands.runPath(Robot.drivetrain.paths.start1_to_leave1, true, 0.6)
+ Commands.runPath(Robot.drivetrain.paths.nearShoot_to_nearLeave, true, 0.6)
);
}
- private Command Movement_RP_Dec6th(){
- return new SequentialGroup(
- new FollowPath(Robot.drivetrain.paths.basicMoveRPPath, true, 0.5)
- );
+ private Command test_Auto(){
+ return new FollowPath(new Path(new BezierCurve(paths.farStart, paths.farLeave)), true, 0.6);
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
index 4a91ea4f26df..cfba3a807f2f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
@@ -48,4 +48,5 @@ public static Command setTransferState(LoadHardwareClass Robot, Intake.transferS
.setStart(() -> Robot.intake.setTransfer(state))
.requires(intakeSystem));
}
+
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
index ceda43b9eb0f..5eebc1484a7a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
@@ -7,6 +7,7 @@
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
public class MecanumDrivetrainClass {
@@ -15,7 +16,7 @@ public class MecanumDrivetrainClass {
// Misc Constants
public Follower follower = null;
- public Pedro_Paths paths = null;
+ public Pedro_Paths paths = new Pedro_Paths();
/**
* Initializes the PedroPathing follower.
@@ -23,13 +24,30 @@ public class MecanumDrivetrainClass {
* @param myOpMode Allows the follower access to the robot hardware.
* @param initialPose The starting pose of the robot.
*/
- public void init (@NonNull OpMode myOpMode, Pose initialPose){
+ public void init (@NonNull OpMode myOpMode, Pose initialPose, LoadHardwareClass.Alliance alliance){
// PedroPathing initialization
follower = Constants.createFollower(myOpMode.hardwareMap); // Initializes the PedroPathing path follower
- paths = new Pedro_Paths(follower);
follower.setStartingPose(initialPose); // Sets the initial position of the robot on the field
follower.update(); // Applies the initialization
+ //TODO uncommment this: paths.buildPaths(alliance, follower);
+ }
+
+ /**
+ * Initializes the PedroPathing follower.
+ * Needs to be run once after all hardware is initialized.
+ * @param myOpMode Allows the follower access to the robot hardware.
+ * @param initialPose The starting pose of the robot.
+ * @param follow The PedroPathing follower object
+ */
+ public void init (@NonNull OpMode myOpMode, Pose initialPose, LoadHardwareClass.Alliance alliance, Follower follow){
+ // PedroPathing initialization
+ follower = follow; // Initializes the PedroPathing path follower
+ follower.setPose(initialPose); // Sets the initial position of the robot on the field
+ follower.update(); // Applies the initialization
+ //TODO uncommment this: paths.buildPaths(alliance, follower);
+ }
+ public void startTeleOpDrive(){
follower.startTeleopDrive();
follower.update();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
index d45fa0c3d763..6955f64d93ef 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
@@ -15,46 +15,43 @@ public class Pedro_Paths {
/**
* Define primary poses to be used in paths
*/
- // Dummy Stand-In Poses
- public Pose dummyStartPose = new Pose(0,0,0);
- public Pose dummyMoveRPPose = new Pose(0,28,0);
// Start Poses
- public Pose startPose1 = new Pose(112, 136.6, Math.toRadians(270));
- public Pose startPose2 = new Pose(88, 7.4, Math.toRadians(90));
+ public Pose nearStart = new Pose(112, 136.6, Math.toRadians(270));
+ public Pose farStart = new Pose(88, 7.4, Math.toRadians(90));
// Preload Poses
- public Pose preloadPose1 = new Pose(130.000, 83.500, Math.toRadians(90));
- public Pose preloadPose2 = new Pose(132.000, 59.500, Math.toRadians(90));
- public Pose preloadPose3 = new Pose(132.000, 35.500, Math.toRadians(90));
+ public Pose nearPreload = new Pose(130.000, 83.500, Math.toRadians(90));
+ public Pose midPreload = new Pose(132.000, 59.500, Math.toRadians(90));
+ public Pose farPreload = new Pose(132.000, 35.500, Math.toRadians(90));
// Shooting Poses
- public Pose shootingPose1 = new Pose(115, 120, Math.toRadians(-35));
- public Pose shootingPose2 = new Pose(85, 85, Math.toRadians(-15));
- public Pose shootingPose3 = new Pose(85, 15, Math.toRadians(60));
+ public Pose nearShoot = new Pose(115, 120, Math.toRadians(-35));
+ public Pose midShoot = new Pose(85, 85, Math.toRadians(-15));
+ public Pose farShoot = new Pose(85, 15, Math.toRadians(60));
// Leave Poses
- public Pose leavePose1 = new Pose(90,120);
- public Pose leavePose2 = new Pose(95,55);
- public Pose leavePose3 = new Pose(115,20);
+ public Pose nearLeave = new Pose(90,120);
+ public Pose midLeave = new Pose(95,55);
+ public Pose farLeave = new Pose(115,20);
// Define all path variables
// Dummy Paths
public PathChain basicMoveRPPath;
// Start Poses to Preloads
- public PathChain startPose1_to_preload1, startPose1_to_preload2, startPose1_to_preload3;
- public PathChain startPose2_to_preload1, startPose2_to_preload2, startPose2_to_preload3;
+ public PathChain nearStart_to_nearPreload, nearStart_to_midPreload, nearStart_to_farPreload;
+ public PathChain farStart_to_nearPreload, farStart_to_midPreload, farStart_to_farPreload;
// Preloads to Shooting Positions
- public PathChain preload1_to_shooting1, preload1_to_shooting2, preload1_to_shooting3;
- public PathChain preload2_to_shooting1, preload2_to_shooting2, preload2_to_shooting3;
- public PathChain preload3_to_shooting1, preload3_to_shooting2, preload3_to_shooting3;
+ public PathChain nearPreload_to_nearShoot, nearPreload_to_midShoot, nearPreload_to_farShoot;
+ public PathChain midPreload_to_nearShoot, midPreload_to_midShoot, midPreload_to_farShoot;
+ public PathChain farPreload_to_nearShoot, farPreload_to_midShoot, farPreload_to_farShoot;
// Shooting Positions to Preloads
- public PathChain shooting1_to_preload1, shooting1_to_preload2, shooting1_to_preload3;
- public PathChain shooting2_to_preload1, shooting2_to_preload2, shooting2_to_preload3;
- public PathChain shooting3_to_preload1, shooting3_to_preload2, shooting3_to_preload3;
+ public PathChain nearShoot_to_nearPreload, nearShoot_to_midPreload, nearShoot_to_farPreload;
+ public PathChain midShoot_to_nearPreload, midShoot_to_midPreload, midShoot_to_farPreload;
+ public PathChain farShoot_to_nearPreload, farShoot_to_midPreload, farShoot_to_farPreload;
// Shooting Positions to Leave Positions
- public PathChain shooting1_to_leave1, shooting1_to_leave2;
- public PathChain shooting2_to_leave1, shooting2_to_leave2;
- public PathChain shooting3_to_leave2, shooting3_to_leave3;
+ public PathChain nearShoot_to_nearLeave, nearShoot_to_midLeave;
+ public PathChain midShoot_to_nearLeave, midShoot_to_midLeave;
+ public PathChain farShoot_to_midLeave, farShoot_to_farLeave;
// Start Positions to Leave Positions
- public PathChain start1_to_leave1, start1_to_leave2;
- public PathChain start2_to_leave2, start2_to_leave3;
+ public PathChain nearStart_to_nearLeave, nearStart_to_midLeave;
+ public PathChain farStart_to_midLeave, farStart_to_farLeave;
public Pose autoMirror(Pose pose, LoadHardwareClass.Alliance alliance){
int mult = 1;
@@ -68,349 +65,340 @@ public Pose autoMirror(Pose pose, LoadHardwareClass.Alliance alliance){
);
}
- /**
- * DON'T EVER USE THIS PATH!!!
- * This path is ONLY for the December 6th scrimmage, for movement RP bonus only!
- */
- public void buildMoveRPPath(){
- basicMoveRPPath = follower.pathBuilder()
- .addPath(new BezierCurve(dummyStartPose,dummyMoveRPPose))
- .setLinearHeadingInterpolation(dummyStartPose.getHeading(), dummyMoveRPPose.getHeading())
- .build();
- }
public void buildStart1ToPreloads(LoadHardwareClass.Alliance alliance) {
- startPose1_to_preload1 = follower.pathBuilder()
+ nearStart_to_nearPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose1, alliance),
+ autoMirror(nearStart, alliance),
autoMirror(new Pose(89.000, 136.600), alliance),
autoMirror(new Pose(89.000, 78.000), alliance),
autoMirror(new Pose(95.000, 83.500), alliance),
autoMirror(new Pose(110.000, 83.500), alliance)
))
- .setLinearHeadingInterpolation(startPose1.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(nearStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 83.500), alliance),
- autoMirror(preloadPose1, alliance)
+ autoMirror(nearPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
- startPose1_to_preload2 = follower.pathBuilder()
+ nearStart_to_midPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose1, alliance),
+ autoMirror(nearStart, alliance),
autoMirror(new Pose(89.000, 136.600), alliance),
autoMirror(new Pose(89.000, 54.000), alliance),
autoMirror(new Pose(95.000, 59.500), alliance),
autoMirror(new Pose(110.000, 59.500), alliance)
))
- .setLinearHeadingInterpolation(startPose1.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(nearStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 59.500), alliance),
- autoMirror(preloadPose2, alliance)
+ autoMirror(midPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
- startPose1_to_preload3 = follower.pathBuilder()
+ nearStart_to_farPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose1, alliance),
+ autoMirror(nearStart, alliance),
autoMirror(new Pose(89.000, 136.600), alliance),
autoMirror(new Pose(89.000, 30.000), alliance),
autoMirror(new Pose(95.000, 35.500), alliance),
autoMirror(new Pose(110.000, 35.500), alliance)
))
- .setLinearHeadingInterpolation(startPose1.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(nearStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 35.500), alliance),
- autoMirror(preloadPose3, alliance)
+ autoMirror(farPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
}
public void buildStart2ToPreloads(LoadHardwareClass.Alliance alliance){
- startPose2_to_preload1 = follower.pathBuilder()
+ farStart_to_nearPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose2, alliance),
+ autoMirror(farStart, alliance),
autoMirror(new Pose(89.000, 89.000), alliance),
autoMirror(new Pose(95.000, 83.500), alliance),
autoMirror(new Pose(110.000, 83.500), alliance)
))
- .setLinearHeadingInterpolation(startPose2.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(farStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 83.500), alliance),
- autoMirror(preloadPose1, alliance)
+ autoMirror(nearPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
- startPose2_to_preload2 = follower.pathBuilder()
+ farStart_to_midPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose2, alliance),
+ autoMirror(farStart, alliance),
autoMirror(new Pose(89.000, 136.600), alliance),
autoMirror(new Pose(89.000, 65.000), alliance),
autoMirror(new Pose(95.000, 59.500), alliance),
autoMirror(new Pose(110.000, 59.500), alliance)
))
- .setLinearHeadingInterpolation(startPose1.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(nearStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 59.500), alliance),
- autoMirror(preloadPose2, alliance)
+ autoMirror(midPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
- startPose2_to_preload3 = follower.pathBuilder()
+ farStart_to_farPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose2, alliance),
+ autoMirror(farStart, alliance),
autoMirror(new Pose(89.000, 136.600), alliance),
autoMirror(new Pose(89.000, 41.000), alliance),
autoMirror(new Pose(95.000, 35.500), alliance),
autoMirror(new Pose(110.000, 35.500), alliance)
))
- .setLinearHeadingInterpolation(startPose2.getHeading(), Math.toRadians(0))
+ .setLinearHeadingInterpolation(farStart.getHeading(), Math.toRadians(0))
.addPath(new BezierLine(
autoMirror(new Pose(110.000, 35.500), alliance),
- autoMirror(preloadPose3, alliance)
+ autoMirror(farPreload, alliance)
))
.setTangentHeadingInterpolation()
.build();
}
public void buildPreload1ToShootings(LoadHardwareClass.Alliance alliance){
- preload1_to_shooting1 = follower.pathBuilder()
+ nearPreload_to_nearShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose1, alliance),
- autoMirror(shootingPose1, alliance)
+ autoMirror(nearPreload, alliance),
+ autoMirror(nearShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose1.getHeading(), shootingPose1.getHeading())
+ .setLinearHeadingInterpolation(nearPreload.getHeading(), nearShoot.getHeading())
.build();
- preload1_to_shooting2 = follower.pathBuilder()
+ nearPreload_to_midShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose1, alliance),
- autoMirror(shootingPose2, alliance)
+ autoMirror(nearPreload, alliance),
+ autoMirror(midShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose1.getHeading(), shootingPose2.getHeading())
+ .setLinearHeadingInterpolation(nearPreload.getHeading(), midShoot.getHeading())
.build();
- preload1_to_shooting3 = follower.pathBuilder()
+ nearPreload_to_farShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose1, alliance),
+ autoMirror(nearPreload, alliance),
autoMirror(new Pose(80.000, 83.500), alliance),
- autoMirror(shootingPose3, alliance)
+ autoMirror(farShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose1.getHeading(), shootingPose3.getHeading())
+ .setLinearHeadingInterpolation(nearPreload.getHeading(), farShoot.getHeading())
.build();
}
public void buildPreload2ToShootings(LoadHardwareClass.Alliance alliance){
- preload2_to_shooting1 = follower.pathBuilder()
+ midPreload_to_nearShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
+ autoMirror(midPreload, alliance),
autoMirror(new Pose(65,59.5), alliance),
- autoMirror(shootingPose1, alliance)
+ autoMirror(nearShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose1.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), nearShoot.getHeading())
.build();
- preload2_to_shooting2 = follower.pathBuilder()
+ midPreload_to_midShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
+ autoMirror(midPreload, alliance),
autoMirror(new Pose(65,59.5), alliance),
- autoMirror(shootingPose2, alliance)
+ autoMirror(midShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose2.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), midShoot.getHeading())
.build();
- preload2_to_shooting3 = follower.pathBuilder()
+ midPreload_to_farShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
+ autoMirror(midPreload, alliance),
autoMirror(new Pose(90.000, 59.500), alliance),
- autoMirror(shootingPose3, alliance)
+ autoMirror(farShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose3.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), farShoot.getHeading())
.build();
}
public void buildPreload3ToShootings(LoadHardwareClass.Alliance alliance){
- preload3_to_shooting1 = follower.pathBuilder()
+ farPreload_to_nearShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
+ autoMirror(midPreload, alliance),
autoMirror(new Pose(75,30), alliance),
autoMirror(new Pose(80,100), alliance),
- autoMirror(shootingPose1, alliance)
+ autoMirror(nearShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose1.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), nearShoot.getHeading())
.build();
- preload3_to_shooting2 = follower.pathBuilder()
+ farPreload_to_midShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
+ autoMirror(midPreload, alliance),
autoMirror(new Pose(85,32), alliance),
- autoMirror(shootingPose2, alliance)
+ autoMirror(midShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose2.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), midShoot.getHeading())
.build();
- preload3_to_shooting3 = follower.pathBuilder()
+ farPreload_to_farShoot = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(preloadPose2, alliance),
- autoMirror(shootingPose3, alliance)
+ autoMirror(midPreload, alliance),
+ autoMirror(farShoot, alliance)
))
- .setLinearHeadingInterpolation(preloadPose2.getHeading(), shootingPose3.getHeading())
+ .setLinearHeadingInterpolation(midPreload.getHeading(), farShoot.getHeading())
.build();
}
public void buildShooting1ToPreloads(LoadHardwareClass.Alliance alliance){
- shooting1_to_preload1 = follower.pathBuilder()
+ nearShoot_to_nearPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose1, alliance),
+ autoMirror(nearShoot, alliance),
autoMirror(new Pose(60, 80), alliance),
- autoMirror(preloadPose1, alliance)
+ autoMirror(nearPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose1.getHeading(), preloadPose1.getHeading())
+ .setLinearHeadingInterpolation(nearShoot.getHeading(), nearPreload.getHeading())
.build();
- shooting1_to_preload2 = follower.pathBuilder()
+ nearShoot_to_midPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose1, alliance),
+ autoMirror(nearShoot, alliance),
autoMirror(new Pose(60, 55), alliance),
- autoMirror(preloadPose2, alliance)
+ autoMirror(midPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose1.getHeading(), preloadPose2.getHeading())
+ .setLinearHeadingInterpolation(nearShoot.getHeading(), midPreload.getHeading())
.build();
- shooting1_to_preload3 = follower.pathBuilder()
+ nearShoot_to_farPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose1, alliance),
+ autoMirror(nearShoot, alliance),
autoMirror(new Pose(60, 27), alliance),
- autoMirror(preloadPose3, alliance)
+ autoMirror(farPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose1.getHeading(), preloadPose3.getHeading())
+ .setLinearHeadingInterpolation(nearShoot.getHeading(), farPreload.getHeading())
.build();
}
public void buildShooting2ToPreloads(LoadHardwareClass.Alliance alliance){
- shooting2_to_preload1 = follower.pathBuilder()
+ midShoot_to_nearPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose2, alliance),
- autoMirror(preloadPose1, alliance)
+ autoMirror(midShoot, alliance),
+ autoMirror(nearPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose2.getHeading(), preloadPose1.getHeading())
+ .setLinearHeadingInterpolation(midShoot.getHeading(), nearPreload.getHeading())
.build();
- shooting2_to_preload2 = follower.pathBuilder()
+ midShoot_to_midPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose2, alliance),
+ autoMirror(midShoot, alliance),
autoMirror(new Pose(75, 56), alliance),
- autoMirror(preloadPose2, alliance)
+ autoMirror(midPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose2.getHeading(), preloadPose2.getHeading())
+ .setLinearHeadingInterpolation(midShoot.getHeading(), midPreload.getHeading())
.build();
- shooting2_to_preload3 = follower.pathBuilder()
+ midShoot_to_farPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose2, alliance),
+ autoMirror(midShoot, alliance),
autoMirror(new Pose(68, 30), alliance),
- autoMirror(preloadPose3, alliance)
+ autoMirror(farPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose2.getHeading(), preloadPose3.getHeading())
+ .setLinearHeadingInterpolation(midShoot.getHeading(), farPreload.getHeading())
.build();
}
public void buildShooting3ToPreloads(LoadHardwareClass.Alliance alliance){
- shooting3_to_preload1 = follower.pathBuilder()
+ farShoot_to_nearPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose3, alliance),
+ autoMirror(farShoot, alliance),
autoMirror(new Pose(73, 88), alliance),
- autoMirror(preloadPose1, alliance)
+ autoMirror(nearPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose3.getHeading(), preloadPose1.getHeading())
+ .setLinearHeadingInterpolation(farShoot.getHeading(), nearPreload.getHeading())
.build();
- shooting3_to_preload2 = follower.pathBuilder()
+ farShoot_to_midPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose3, alliance),
+ autoMirror(farShoot, alliance),
autoMirror(new Pose(78, 62), alliance),
- autoMirror(preloadPose2, alliance)
+ autoMirror(midPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose3.getHeading(), preloadPose2.getHeading())
+ .setLinearHeadingInterpolation(farShoot.getHeading(), midPreload.getHeading())
.build();
- shooting3_to_preload3 = follower.pathBuilder()
+ farShoot_to_farPreload = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose3, alliance),
+ autoMirror(farShoot, alliance),
autoMirror(new Pose(82.5, 35), alliance),
- autoMirror(preloadPose3, alliance)
+ autoMirror(farPreload, alliance)
))
- .setLinearHeadingInterpolation(shootingPose3.getHeading(), preloadPose3.getHeading())
+ .setLinearHeadingInterpolation(farShoot.getHeading(), farPreload.getHeading())
.build();
}
public void buildShooting1ToLeaves(LoadHardwareClass.Alliance alliance){
- shooting1_to_leave1 = follower.pathBuilder()
+ nearShoot_to_nearLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose1, alliance),
- autoMirror(leavePose1, alliance)
+ autoMirror(nearShoot, alliance),
+ autoMirror(nearLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose1.getHeading())
+ .setConstantHeadingInterpolation(nearShoot.getHeading())
.build();
- shooting1_to_leave2 = follower.pathBuilder()
+ nearShoot_to_midLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose1, alliance),
- autoMirror(leavePose2, alliance)
+ autoMirror(nearShoot, alliance),
+ autoMirror(midLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose1.getHeading())
+ .setConstantHeadingInterpolation(nearShoot.getHeading())
.build();
}
public void buildShooting2ToLeaves(LoadHardwareClass.Alliance alliance){
- shooting2_to_leave1 = follower.pathBuilder()
+ midShoot_to_nearLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose2, alliance),
- autoMirror(leavePose1, alliance)
+ autoMirror(midShoot, alliance),
+ autoMirror(nearLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose2.getHeading())
+ .setConstantHeadingInterpolation(midShoot.getHeading())
.build();
- shooting2_to_leave2 = follower.pathBuilder()
+ midShoot_to_midLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose2, alliance),
- autoMirror(leavePose2, alliance)
+ autoMirror(midShoot, alliance),
+ autoMirror(midLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose2.getHeading())
+ .setConstantHeadingInterpolation(midShoot.getHeading())
.build();
}
public void buildShooting3ToLeaves(LoadHardwareClass.Alliance alliance){
- shooting3_to_leave2 = follower.pathBuilder()
+ farShoot_to_midLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose3, alliance),
- autoMirror(leavePose2, alliance)
+ autoMirror(farShoot, alliance),
+ autoMirror(midLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose3.getHeading())
+ .setConstantHeadingInterpolation(farShoot.getHeading())
.build();
- shooting3_to_leave3 = follower.pathBuilder()
+ farShoot_to_farLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(shootingPose3, alliance),
- autoMirror(leavePose3, alliance)
+ autoMirror(farShoot, alliance),
+ autoMirror(farLeave, alliance)
))
- .setConstantHeadingInterpolation(shootingPose3.getHeading())
+ .setConstantHeadingInterpolation(farShoot.getHeading())
.build();
}
public void buildStart1ToLeaves(LoadHardwareClass.Alliance alliance){
- start1_to_leave1 = follower.pathBuilder()
+ nearStart_to_nearLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose1, alliance),
- autoMirror(leavePose1, alliance)
+ autoMirror(nearStart, alliance),
+ autoMirror(nearLeave, alliance)
))
- .setConstantHeadingInterpolation(startPose1.getHeading())
+ .setConstantHeadingInterpolation(nearStart.getHeading())
.build();
- start1_to_leave2 = follower.pathBuilder()
+ nearStart_to_midLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose1, alliance),
- autoMirror(leavePose3, alliance)
+ autoMirror(nearStart, alliance),
+ autoMirror(farLeave, alliance)
))
- .setConstantHeadingInterpolation(startPose1.getHeading())
+ .setConstantHeadingInterpolation(nearStart.getHeading())
.build();
}
public void buildStart2ToLeaves(LoadHardwareClass.Alliance alliance){
- start2_to_leave2 = follower.pathBuilder()
+ farStart_to_midLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose2, alliance),
- autoMirror(leavePose1, alliance)
+ autoMirror(farStart, alliance),
+ autoMirror(nearLeave, alliance)
))
- .setConstantHeadingInterpolation(startPose2.getHeading())
+ .setConstantHeadingInterpolation(farStart.getHeading())
.build();
- start2_to_leave3 = follower.pathBuilder()
+ farStart_to_farLeave = follower.pathBuilder()
.addPath(new BezierCurve(
- autoMirror(startPose2, alliance),
- autoMirror(leavePose3, alliance)
+ autoMirror(farStart, alliance),
+ autoMirror(farLeave, alliance)
))
- .setConstantHeadingInterpolation(startPose2.getHeading())
+ .setConstantHeadingInterpolation(farStart.getHeading())
.build();
}
/**
* Builds all the paths, mirroring them to the other side of the field if necessary
*/
- public void buildPaths(LoadHardwareClass.Alliance alliance){
+ public void buildPaths(LoadHardwareClass.Alliance alliance, Follower follow){
+ follower = follow;
+
/// All paths are for the RED side of the field. they will be mirrored if necessary.
- buildMoveRPPath();
// Paths going from each start position to each of the preloads.
buildStart1ToPreloads(alliance);
buildStart2ToPreloads(alliance);
@@ -430,12 +418,4 @@ public void buildPaths(LoadHardwareClass.Alliance alliance){
buildStart1ToLeaves(alliance);
buildStart2ToLeaves(alliance);
}
-
- /**
- * Must be called after MecanumDrivetrainClass is initialized.
- * @param follow PedroPathing's follower object, gotten from MecanumDrivetrainClass
- */
- public Pedro_Paths(Follower follow){
- follower = follow;
- }
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
index 697b59448911..91a7ee9afc51 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
@@ -30,6 +30,7 @@
package org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_;
import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@@ -88,7 +89,22 @@ public LoadHardwareClass(OpMode opmode) {
*/
public void init(Pose initialPose) {
// Initialize all subclasses
- drivetrain.init(myOpMode, initialPose);
+ drivetrain.init(myOpMode, initialPose, selectedAlliance);
+ turret.init(myOpMode);
+ intake.init(myOpMode);
+
+ // Misc telemetry
+ myOpMode.telemetry.addData(">", "Hardware Initialized");
+ myOpMode.telemetry.update();
+ }
+
+ /**
+ * Initializes all hardware for the robot.
+ * Must be called once at the start of each op-mode.
+ */
+ public void init(Pose initialPose, Follower follower) {
+ // Initialize all subclasses
+ drivetrain.init(myOpMode, initialPose, selectedAlliance, follower);
turret.init(myOpMode);
intake.init(myOpMode);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
index a5f79ab5faa7..fff9cfefe75f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java
@@ -105,6 +105,7 @@ public void runOpMode() {
// Initialize all hardware of the robot
Robot.init(startPose);
+ Robot.drivetrain.startTeleOpDrive();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
index 1cbabd505c3d..39976888d7a7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
@@ -1,7 +1,5 @@
//TODO, implement all our external libraries and functionality.
-//TODO, add 2 generic autos for preloads and shooting and stuff
-
//DONE, Implement controls graph to TeleOp
//DONE, Consolidate the generic actuator functionalities folder into one file
@@ -20,4 +18,14 @@
//DONE, Finish translating Dylan's December 6th controls into the program.
-//DONE, Figure out how to use NextFTC's Pedropathing extension for auto.
\ No newline at end of file
+//DONE, Figure out how to use NextFTC's Pedropathing extension for auto.
+
+
+
+//1. TODO, test the auto paths used in the generic autos
+//2. TODO, create two generic autos for testing and competition
+//3. TODO, test the turret rotation autoaim with shooting
+//4. TODO, establish static (non-changing) flywheel speeds for near/far zones
+//5. TODO, make hood autoaim using InterpLUT
+//6. TODO, test turret hood and rotation autoaim together
+//7. TODO, test turret autoaim with the generic autos
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
index 10b4639b554f..d4338b8b7e58 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
@@ -16,16 +16,17 @@
public class Constants {
public static FollowerConstants followerConstants = new FollowerConstants()
- .mass(5) // TODO: Change this to the actual weight of the robot
- .forwardZeroPowerAcceleration(-45.7735)
- .lateralZeroPowerAcceleration(-53.7000)
+ .mass(13.6) // TODO: Change this to the actual weight of the robot
+ .forwardZeroPowerAcceleration(-42.5878)
+ .lateralZeroPowerAcceleration(-68.2022)
// Set following parameters to true to enable dual PID
.useSecondaryTranslationalPIDF(false)
.useSecondaryHeadingPIDF(false)
.useSecondaryDrivePIDF(false)
- .translationalPIDFCoefficients(new PIDFCoefficients(.1, 0, 0.01, 0.05))
- .headingPIDFCoefficients(new PIDFCoefficients(3, 0.3, 0.2, 0.05))
- .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.3, 0.1, 0.01, 0.6, 0.6));
+ .centripetalScaling(0.0002)
+ .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.01, 0.05))
+ .headingPIDFCoefficients(new PIDFCoefficients(2, 0, 0.1, 0.026))
+ .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.001, 0.6, 0.02));
public static PathConstraints pathConstraints = new PathConstraints(
0.99,
@@ -48,12 +49,12 @@ public class Constants {
.leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
.rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
.rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
- .xVelocity(59.3402)
- .yVelocity(46.4157);
+ .xVelocity(65.1356)
+ .yVelocity(39.2622);
public static PinpointConstants localizerConstants = new PinpointConstants()
- .forwardPodY((double) 3/8)
- .strafePodX(6)
+ .forwardPodY(-3.25)
+ .strafePodX(-6.25)
.distanceUnit(DistanceUnit.INCH)
.hardwareMapName("pinpoint")
.encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
index b2688ac39e13..5e656da719d4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
@@ -1,8 +1,8 @@
package org.firstinspires.ftc.teamcode.pedroPathing;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes;
-import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
@@ -15,13 +15,16 @@
import com.bylazar.field.Style;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
-import com.pedropathing.ErrorCalculator;
import com.pedropathing.follower.Follower;
-import com.pedropathing.geometry.*;
-import com.pedropathing.math.*;
-import com.pedropathing.paths.*;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.math.Vector;
+import com.pedropathing.paths.HeadingInterpolator;
+import com.pedropathing.paths.Path;
+import com.pedropathing.paths.PathChain;
import com.pedropathing.telemetry.SelectableOpMode;
-import com.pedropathing.util.*;
+import com.pedropathing.util.PoseHistory;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -537,7 +540,7 @@ public void loop() {
*/
class ForwardZeroPowerAccelerationTuner extends OpMode {
private final ArrayListDefine all path variables
+ */
// Start Poses to Preloads
public PathChain nearStart_to_nearPreload, nearStart_to_midPreload, nearStart_to_farPreload;
public PathChain farStart_to_nearPreload, farStart_to_midPreload, farStart_to_farPreload;
@@ -54,15 +54,14 @@ public class Pedro_Paths {
public PathChain farStart_to_midLeave, farStart_to_farLeave;
public Pose autoMirror(Pose pose, LoadHardwareClass.Alliance alliance){
- int mult = 1;
if (alliance == LoadHardwareClass.Alliance.BLUE){
- mult = -1;
+ return new Pose(
+ 144 - pose.getX(),
+ 144 - pose.getY(),
+ Math.atan2(Math.sin(pose.getHeading()), -Math.cos(pose.getHeading()))
+ );
}
- return new Pose(
- 144 - pose.getX(),
- 144 - pose.getY(),
- Math.atan2(Math.sin(pose.getHeading()), mult * Math.cos(pose.getHeading()))
- );
+ return pose;
}
public void buildStart1ToPreloads(LoadHardwareClass.Alliance alliance) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
index 91a7ee9afc51..b0943a033cc5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java
@@ -89,7 +89,7 @@ public LoadHardwareClass(OpMode opmode) {
*/
public void init(Pose initialPose) {
// Initialize all subclasses
- drivetrain.init(myOpMode, initialPose, selectedAlliance);
+ drivetrain.init(myOpMode, initialPose);
turret.init(myOpMode);
intake.init(myOpMode);
@@ -104,7 +104,7 @@ public void init(Pose initialPose) {
*/
public void init(Pose initialPose, Follower follower) {
// Initialize all subclasses
- drivetrain.init(myOpMode, initialPose, selectedAlliance, follower);
+ drivetrain.init(myOpMode, initialPose, follower);
turret.init(myOpMode);
intake.init(myOpMode);
From 2488e368ffffd7d89910a4b064d61cff02b3baba Mon Sep 17 00:00:00 2001
From: Professor348 <141444315+professor348@users.noreply.github.com>
Date: Sat, 20 Dec 2025 12:04:20 -0500
Subject: [PATCH 14/61] Added DualProximitySensorClass to Devices.java and
implemented sensor read functions in Intake.java
---
.../LOADCode/Main_/Auto_/Auto_Main_.java | 2 +-
.../Main_/Hardware_/Actuators_/Devices.java | 26 +++++++++++++++
.../Main_/Hardware_/Actuators_/Intake.java | 33 ++++++++++++-------
.../Main_/Hardware_/Actuators_/Turret.java | 4 +--
.../LOADCode/Main_/Hardware_/Commands.java | 2 +-
5 files changed, 51 insertions(+), 16 deletions(-)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
index da840d0fb149..ca2d3709d055 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
@@ -167,7 +167,7 @@ private Command Leave_Near_Launch() {
return new SequentialGroup(
Commands.setFlywheelState(Robot, Turret.flywheelstate.ON),
new ParallelGroup(
- new WaitUntil(() -> Robot.turret.getFlywheelRPM() > Turret.onSpeed),
+ new WaitUntil(() -> Robot.turret.getFlywheelRPM() > Turret.flywheelSpeed),
new Delay(5)
),
Commands.setIntakeMode(Robot, Intake.intakeMode.SHOOTING),
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
index 6db114ed4315..0162b89399f8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
@@ -292,4 +292,30 @@ public double getDistance(DistanceUnit units){
return ((DistanceSensor) sensor).getDistance(units);
}
}
+
+ public static class DualProximitySensorClass {
+ private final REVColorSensorV3Class sensor1 = new REVColorSensorV3Class();
+ private final REVColorSensorV3Class sensor2 = new REVColorSensorV3Class();
+
+ public double threshold = 2;
+ public DistanceUnit units = DistanceUnit.CM;
+
+ public void init(@NonNull OpMode opmode, String sensor1Name, String sensor2Name){
+ sensor1.init(opmode, sensor1Name);
+ sensor2.init(opmode, sensor2Name);
+ }
+
+ public void setGain(double gain){
+ sensor1.setGain(gain);
+ sensor2.setGain(gain);
+ }
+
+ public boolean objectDetected(){
+ return (sensor1.getDistance(units) < threshold || sensor2.getDistance(units) < threshold);
+ }
+
+ public double[] getDistances(){
+ return new double[]{sensor1.getDistance(units), sensor2.getDistance(units)};
+ }
+ }
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
index 13d4379fd9ec..3ed7dbf88de4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java
@@ -4,16 +4,16 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
public class Intake {
// RESET THESE TO PRIVATE AFTER DECEMBER 6TH!
private final Devices.DcMotorExClass intake = new Devices.DcMotorExClass();
private final Devices.CRServoClass belt = new Devices.CRServoClass();
private final Devices.ServoClass transfer = new Devices.ServoClass();
- public final Devices.REVColorSensorV3Class color1 = new Devices.REVColorSensorV3Class();
- public final Devices.REVColorSensorV3Class color2 = new Devices.REVColorSensorV3Class();
- public final Devices.REVColorSensorV3Class color3 = new Devices.REVColorSensorV3Class();
- public final Devices.REVColorSensorV3Class color4 = new Devices.REVColorSensorV3Class();
+ public final Devices.DualProximitySensorClass topSensor = new Devices.DualProximitySensorClass();
+ public final Devices.DualProximitySensorClass bottomSensor = new Devices.DualProximitySensorClass();
public enum intakeMode {
INTAKING,
@@ -27,14 +27,14 @@ public enum transferState {
DOWN
}
+ public static double proximitySensorThreshold = 20;
+
public void init(OpMode opmode){
intake.init(opmode, "intake");
belt.init(opmode, "belt");
transfer.init(opmode, "transfer");
- color1.init(opmode, "color1");
- color2.init(opmode, "color2");
- color3.init(opmode, "color3");
- color4.init(opmode, "color4");
+ topSensor.init(opmode, "color1", "color2");
+ bottomSensor.init(opmode, "color3", "color4");
intake.setZeroPowerBehaviour(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -42,10 +42,12 @@ public void init(OpMode opmode){
belt.setDirection(DcMotorSimple.Direction.REVERSE);
- color1.setGain(2);
- color2.setGain(2);
- color3.setGain(2);
- color4.setGain(2);
+ topSensor.setGain(2);
+ topSensor.units = DistanceUnit.MM;
+ topSensor.threshold = proximitySensorThreshold;
+ bottomSensor.setGain(2);
+ bottomSensor.units = DistanceUnit.MM;
+ bottomSensor.threshold = proximitySensorThreshold;
}
/**
@@ -106,4 +108,11 @@ public void setTransfer(transferState state) {
transfer.setAngle(.05);
}
}
+
+ public boolean getTopSensorState(){
+ return topSensor.objectDetected();
+ }
+ public boolean getBottomSensorState(){
+ return bottomSensor.objectDetected();
+ }
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
index a748086bd5d0..4529d7032eda 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java
@@ -44,7 +44,7 @@ public enum flywheelstate {
}
public flywheelstate flywheelState = flywheelstate.OFF;
- public static double onSpeed = 4500;
+ public static double flywheelSpeed = 4500;
public void init(OpMode opmode){
rotation.init(opmode, "turret", 145.1 * ((double) 131 /24)); //Previously 103.8
@@ -162,7 +162,7 @@ public void setFlywheelState(flywheelstate state){
*/
public void updateFlywheel(){
if (flywheelState == flywheelstate.ON){
- setFlywheelRPM(onSpeed);
+ setFlywheelRPM(flywheelSpeed);
} else if (flywheelState == flywheelstate.OFF){
setFlywheelRPM(0);
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
index 86a881bdbff7..3b05f53fd8ae 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
@@ -25,7 +25,7 @@ public static Command setFlywheelState(LoadHardwareClass Robot, Turret.flywheels
.setStart(() -> Robot.turret.setFlywheelState(state))
.setIsDone(() -> {
if (state == Turret.flywheelstate.ON){
- return Robot.turret.getFlywheelRPM() > Turret.onSpeed;
+ return Robot.turret.getFlywheelRPM() > Turret.flywheelSpeed;
}else{
return Robot.turret.getFlywheelRPM() < 100;
}
From 33dc49887f2741068ea5a81a02e89a2f276f967f Mon Sep 17 00:00:00 2001
From: Professor348 <141444315+Professor348@users.noreply.github.com>
Date: Sat, 20 Dec 2025 20:54:09 -0500
Subject: [PATCH 15/61] Installed Marrow's NextFTC Extension
---
TeamCode/build.gradle | 1 +
.../LOADCode/Main_/Auto_/Auto_Main_.java | 16 +++++++++++++++-
.../ftc/teamcode/LOADCode/Main_/Utils_.java | 18 +++++++++---------
3 files changed, 25 insertions(+), 10 deletions(-)
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index dd6160908114..651c811d8a36 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -26,6 +26,7 @@ android {
dependencies {
implementation project(':FtcRobotController')
implementation 'io.github.skeleton-army.marrow:core:0.1.2'
+ implementation 'io.github.skeleton-army.marrow:nextftc:0.1.2' // For NextFTC
implementation 'dev.nextftc:ftc:1.0.1'
implementation 'dev.nextftc.extensions:pedro:1.0.0'
implementation "com.bylazar:graph:1.0.4"
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
index ca2d3709d055..03b75b9437e4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java
@@ -5,6 +5,7 @@
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.skeletonarmy.marrow.TimerEx;
import com.skeletonarmy.marrow.prompts.OptionPrompt;
import com.skeletonarmy.marrow.prompts.Prompter;
@@ -16,6 +17,7 @@
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import dev.nextftc.core.commands.Command;
+import dev.nextftc.core.commands.conditionals.IfElseCommand;
import dev.nextftc.core.commands.delays.Delay;
import dev.nextftc.core.commands.delays.WaitUntil;
import dev.nextftc.core.commands.groups.ParallelGroup;
@@ -41,6 +43,9 @@ private enum Auto {
// Create the prompter object for selecting Alliance and Auto
Prompter prompter = null;
+ // Create a TimerEx object for time tracking
+ TimerEx time = new TimerEx(30);
+
// Create a new instance of our Robot class
LoadHardwareClass Robot = new LoadHardwareClass(this);
// Create a Paths object for accessing modular auto paths
@@ -100,6 +105,7 @@ public void onStartButtonPressed() {
}
// Initialize all hardware of the robot
Robot.init(startPose, follower());
+
telemetry.addData("Initialized", "");
telemetry.update();
}
@@ -186,6 +192,14 @@ private Command Leave_Near_Launch() {
}
private Command test_Auto(){
- return Commands.runPath(paths.farStart_to_nearPreload, true);
+ return new IfElseCommand(
+ () -> time.isLessThan(5),
+ new SequentialGroup(
+
+ ),
+ new SequentialGroup(
+
+ )
+ );
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Utils_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Utils_.java
index a5de6ba89773..8131e9dfb13b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Utils_.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Utils_.java
@@ -6,15 +6,6 @@
public class Utils_ {
- public enum telemetrySortCategory {
- DRIVETRAIN,
- INTAKE,
- TRANSFER,
- TURRET,
- TESTINGVALUES,
- OTHER,
- }
-
/**
* Performs spline interpolation given a set of control points.
*/
@@ -158,6 +149,15 @@ public String toString() {
}
+ public enum telemetrySortCategory {
+ DRIVETRAIN,
+ INTAKE,
+ TRANSFER,
+ TURRET,
+ TESTINGVALUES,
+ OTHER,
+ }
+
/**
* This class is for the runtime per-loop sorting and displaying of every single TelemetryObject.
* There should only be one of these per program.
From 817e9558361761cb58f9fb3e4c847d157d74e7e5 Mon Sep 17 00:00:00 2001
From: NotABitcoinScam <152728412+NotABitcoinScam@users.noreply.github.com>
Date: Mon, 22 Dec 2025 16:41:18 -0500
Subject: [PATCH 16/61] Add files via upload
Updated flowcharts and diagrams for the README in Dev branch
---
File_Structure.png | Bin 0 -> 233641 bytes
Robot_Controls.png | Bin 0 -> 809370 bytes
Robot_Wiring.png | Bin 0 -> 180567 bytes
3 files changed, 0 insertions(+), 0 deletions(-)
create mode 100644 File_Structure.png
create mode 100644 Robot_Controls.png
create mode 100644 Robot_Wiring.png
diff --git a/File_Structure.png b/File_Structure.png
new file mode 100644
index 0000000000000000000000000000000000000000..166df00c2742afbbab6b2376a0d48143e3fb438d
GIT binary patch
literal 233641
zcmeFZWn5KV+b@cpfFdFxARa;QIRc2F`_
zL|HYYibuD_^D*2Axi1;VW**l&iF~FevTpbY`Nvh?{sMjP*RNgHJB8SadKZT)`f@cG
z=G-=V6l!H7xo%@MjMaMIxN+lPe}7`5=G7VX<}>l?4r6bYmzOImD`6B$r1kamQacU4
zwYIuWhAF6u>PEP4RgBW96wi|$j*(WP(Iv=?>`EosId$q31A6^?^7D|kSgtKk(*4J*
zX>IY3^xI(eUAuMbJ}qZ#TWV^m=LPJljb0TP0=6ur^DV|YIun1LyRx}7I(qao{%}2Q
zWo4zVuCASXO)Nk#U&DUbR>9bKPSR~PM7!brhPwT1cLvO4SWBBLD?xYX=H_&*-4EB|
zopB8r@#S+=%UpJ>sGq0mChNrUy6$fDX|P4N^6igdCnYA*tCg?AOwA0lw_ieUHB)kO
za_W3?TAqyLUhm-VN)->WwdGXP*Vi{SUD#WyuGVjli+ZHbyEk9Jq@kv;X}mVos_wo=
z#nfY3GECWI#AEd>WfkVndM!4e4e=341%r7`CGn+8mzdP2Zu2;;_f;R-^7DJT>@3$}
zk6N|R$)n_Q`aDyfCjm*bJjp*yWo?Zje*bF}l=LOioR0p6g5!GB=_lsCGMWQc@U{
z2xGTjR5dm>*3{G#6NBM6?>RP;7B3+oVdvWKyfHL16cZEkJcLqLN9XL>v+D-+-{SfB
zn3%qsm5}nfD4K={Hzoz}mcxLsWET|3P-e-`5!(-u361wCXwcEp&Q)u^3l4H~a|__v
zRDAK`#g>p9X2SgZe8p1LpbpdXe(CB?gR7XBVH_|tdn*C%LmeF-u|k@wj}DgZQ&7y<
zaHe%pSB%4LTw00`47_&xwyLzW)&906$LX=Lv6-0}BZkb1tgHvm=@}UK99NTF%8Yt4
zS&VxdpP@q!Lv%!xWo2beOtPYNTgoMQHo9Xttk<1f6%-=6$#-79e2Ehs9=
@}dF;(Cb+;Ye{v?7&dcv{#Z81SRt10&)A|hJo)5&%1dF9eIn&UEwKX!L_FZRVW
zDMJ+v<$#7|bU~D9a;V3wWU0SE$W-PXbV{VD9~r4N7_I!OlaBt@UtzyAms_1fZ)#xh
zqtr5Kz7v*Ketv$j`RK^Oi0iYYyyWBweZIY@fp?WIJFC$Bi(b}xrKhKFZ*S*T9}W}=
zZ~oeXMPAU(OWUAYY*j1KLv=7M_9rE*y`>wv_Udy >n~KFep!;wiGio3$)FUvB&Mg9s>bPyl(WO&i2)kbWf||XK3=*+rd{-m9LG0;
zUPHpIJXiaFu=SQvBt|o@f8u!WkOre<>t}+Q>8A{53JzL9pr4CGnfco*c>&-@i4iXL
zeibBRKqd;Rmvc3bmox#MK>o;B>e;hvF6HnlU-b_Ux7)n9xV#_nMYP-K$derN_yg|r
zL(4xHc!AWy9bGJBGU$_Ku2BlvO1b@)_eM{#17ycJP;3;v!~hbel6zlLtO)8)kd)a2
zTCg7T3J#pTurh%3`R>R3SQO-MfVu(-31Ef1F92>RC?^TflSPXs2=K$j=-01(b3^AV
zZr~f^5OynLqM)s0x4S^qH0gXViUvAZtrf?Yj-H5$HUkUP{|0HM5%dE$4wL~o3`&JU
z&TH{9S+g}_w!vzQ~GuEyL!q0OX5ef}w=2R}t
zOYx>`cf*^2zb=(BFH~2&5Zry%rM6m?~skse{TNK-P3e
zSmw1G0)4rf&1XMFAlG|0Hzx2lC_v7bC)3AjSg{a+f61|92~<
csM=J%LH&qpx9Dz#gOM{xi
zoqwvc&t026Yw{tnCzUQ1;?P^dXe1;5W`e+9nZQL=l%U@Rmxc{Uu5BDeA}vznnsxp<
zW1|j&{O1tBCMvZU