From c470e2dd651334beb1d64cba70ac7d6ea8fb952a Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Fri, 23 Jan 2026 21:19:36 -0800 Subject: [PATCH 1/9] Trying some stuff to make the launcher a single-file component --- .../ftc/sixteen750/commands/TeleCommands.java | 28 ---- .../sixteen750/controls/DriverController.java | 5 +- .../subsystems/LauncherSubsystem.java | 139 +++++++++++------- 3 files changed, 91 insertions(+), 81 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java index cb310130..21e62d5f 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java @@ -14,34 +14,6 @@ public static Command TurnTo90(Robot r) { ); //pose might need to be current pose? } - public static Command Launch(Robot r) { - return Command.create(r.launcherSubsystem::Launch); - } - - public static Command SetFarShoot(Robot r) { - return Command.create(r.launcherSubsystem::FarShoot); - } - - public static Command SetCloseShoot(Robot r) { - return Command.create(r.launcherSubsystem::CloseShoot); - } - - public static Command AutoLaunch1(Robot r) { - return Command.create(r.launcherSubsystem::AutoLaunch1); - } - - public static Command AutoLaunch2(Robot r) { - return Command.create(r.launcherSubsystem::AutoLaunch2); - } - - public static Command FarAutoLaunch(Robot r) { - return Command.create(r.launcherSubsystem::FarAutoLaunch); - } - - public static Command StopLaunch(Robot r) { - return Command.create(r.launcherSubsystem::Stop); - } - public static Command Rumble(Robot r) { return Command.create(r.intakeSubsystem::setRumble); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java index 82326e7a..d16e018c 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java @@ -13,6 +13,7 @@ import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; public class DriverController { @@ -129,8 +130,8 @@ public void bindDriveControls() { } public void bindLaunchControls() { - launchButton.whilePressed(TeleCommands.Launch(robot)); - launchButton.whenReleased(TeleCommands.StopLaunch(robot)); + launchButton.whilePressed(LauncherSubsystem.Commands.Launch(robot.launcherSubsystem)); + launchButton.whenReleased(LauncherSubsystem.Commands.StopLaunch(robot.launcherSubsystem)); // CloseShoot.whenPressed(TeleCommands.SetCloseShoot(robot)); // FarShoot.whenPressed(TeleCommands.SetFarShoot(robot)); MotorIncrease.whenPressed(robot.launcherSubsystem::IncreaseMotorVelocity); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java index 64d98636..a4c071a2 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; @@ -19,17 +20,72 @@ @Configurable public class LauncherSubsystem implements Loggable, Subsystem { - // @Log.Number(name = "Motor Power") - // public static double MOTOR_POWER = 0.65; // 0.5 1.0 + public class Commands { + + public Command Launch(LauncherSubsystem l) { + return Command.create(l::Launch); + } + + public Command SetFarShoot(LauncherSubsystem l) { + return Command.create(l::FarShoot); + } + + public static Command SetCloseShoot(LauncherSubsystem l) { + return Command.create(l::CloseShoot); + } + + public static Command AutoLaunch1(LauncherSubsystem l) { + return Command.create(l::AutoLaunch1); + } + + public static Command AutoLaunch2(LauncherSubsystem l) { + return Command.create(l::AutoLaunch2); + } + + public static Command FarAutoLaunch(LauncherSubsystem l) { + return Command.create(l::FarAutoLaunch); + } + + public static Command StopLaunch(LauncherSubsystem l) { + return Command.create(l::Stop); + } + } + + public static class Config { + + // @Log.Number(name = "Motor Power") + // public static double MOTOR_POWER = 0.65; // 0.5 1.0 + public static double closetargetLaunchVelocity = 1400; + public static double fartargetLaunchVelocity = 1850; + public static double fartargetLaunchVelocityforAuto = 2300; + public static double targetLaunchVelocityforAuto1 = 1950; + public static double targetLaunchVelocityforAuto2 = 1850; + + public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static PIDFCoefficients launcherPI_ForAuto = new PIDFCoefficients( + 0.0015, + 0.0000, + 0.0, + 0 + ); //p = 0.004, i = 0.00020 + public static double SPIN_F_SCALE = 0.00021; + public static double SPIN_VOLT_COMP = 0.0216; + public static double DIFFERENCE = 0.0046; + public static double PEAK_VOLTAGE = 13; + + public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula + public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula + public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 + public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula + public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula + } + @Log.Number(name = "Target Velocity") public static double targetLaunchVelocity = 1150; - public static double closetargetLaunchVelocity = 1400; - public static double fartargetLaunchVelocity = 1850; - public static double fartargetLaunchVelocityforAuto = 2300; - public static double targetLaunchVelocityforAuto1 = 1950; - public static double targetLaunchVelocityforAuto2 = 1850; - @Log.Number(name = "Current Motor Velocity") public static double currentLaunchVelocity = 0.0; @@ -51,17 +107,6 @@ public class LauncherSubsystem implements Loggable, Subsystem { @Log(name = "Target Power: ") public static double targetPower; - public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); - public static PIDFCoefficients launcherPI_ForAuto = new PIDFCoefficients( - 0.0015, - 0.0000, - 0.0, - 0 - ); //p = 0.004, i = 0.00020 - public static double SPIN_F_SCALE = 0.00021; - public static double SPIN_VOLT_COMP = 0.0216; - public static double DIFFERENCE = 0.0046; - public static double PEAK_VOLTAGE = 13; private static PIDFController launcherPID; public static double lastAutoVelocity = 0; @@ -73,14 +118,6 @@ public class LauncherSubsystem implements Loggable, Subsystem { public static double MINIMUM_VELOCITY = 1140; public static double RPM_PER_FOOT = 62.3; - public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula - public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula - public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 - public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula - public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula @Log.Number(name = "AutoAim Velocity") public static double autoVelocity; @@ -108,17 +145,17 @@ public LauncherSubsystem(Hardware h) { launcher2.setPIDFCoefficients(launcherPIDF); //ready = false; ls = new LimelightSubsystem(h); - double ADDITION = (PEAK_VOLTAGE - h.voltage()); + double ADDITION = (Config.PEAK_VOLTAGE - h.voltage()); if (ADDITION == 0) { - SPIN_VOLT_COMP = SPIN_VOLT_COMP + 0.001; + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; } else { - SPIN_VOLT_COMP = SPIN_VOLT_COMP + (ADDITION * DIFFERENCE); + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); } - launcherPID = new PIDFController(launcherPI, target -> + launcherPID = new PIDFController(Config.launcherPI, target -> target == 0 ? 0 - : (SPIN_F_SCALE * target) + - (SPIN_VOLT_COMP * Math.min(PEAK_VOLTAGE, h.voltage())) + : (Config.SPIN_F_SCALE * target) + + (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, h.voltage())) ); // top.setPIDFCoefficients(launcherP); setTargetSpeed(0); @@ -141,21 +178,21 @@ public void Launch() { public void CloseShoot() { // Spin the motors pid goes here if (hasHardware) { - targetLaunchVelocity = closetargetLaunchVelocity; + targetLaunchVelocity = Config.closetargetLaunchVelocity; } } public void FarShoot() { // Spin the motors pid goes here if (hasHardware) { - targetLaunchVelocity = fartargetLaunchVelocity; + targetLaunchVelocity = Config.fartargetLaunchVelocity; } } public void AutoLaunch1() { // Spin the motors pid goes here if (hasHardware) { - setTargetSpeed(targetLaunchVelocityforAuto1); //change to auto aim velocity + setTargetSpeed(Config.targetLaunchVelocityforAuto1); //change to auto aim velocity //launcher1.setVelocity(targetLaunchVelocityforAuto1); //launcher2.setVelocity(targetLaunchVelocityforAuto1); } @@ -164,7 +201,7 @@ public void AutoLaunch1() { public void AutoLaunch2() { // Spin the motors pid goes here if (hasHardware) { - setTargetSpeed(targetLaunchVelocityforAuto2); //change to auto aim velocity + setTargetSpeed(Config.targetLaunchVelocityforAuto2); //change to auto aim velocity // launcher1.setVelocity(TargetLaunchVelocity); // launcher2.setVelocity(TargetLaunchVelocity); } @@ -173,7 +210,7 @@ public void AutoLaunch2() { public void FarAutoLaunch() { // Spin the motors pid goes here if (hasHardware) { - setTargetSpeed(fartargetLaunchVelocityforAuto); //change to auto aim velocity + setTargetSpeed(Config.fartargetLaunchVelocityforAuto); //change to auto aim velocity // launcher1.setVelocity(TargetLaunchVelocity); // launcher2.setVelocity(TargetLaunchVelocity); } @@ -288,15 +325,15 @@ public double autoVelocity() { if (x < 100 && x > 0) { //launcherPI.p = 0.0015; //launcherPI.i = 0; - lastAutoVelocity = REGRESSION_A * x + REGRESSION_B; + lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; return lastAutoVelocity; } if (x < 0) { return lastAutoVelocity; } else { - launcherPI.p = 0.004; - launcherPI.i = 0.0002; - return REGRESSION_C * x + REGRESSION_D; + Config.launcherPI.p = 0.004; + Config.launcherPI.i = 0.0002; + return Config.REGRESSION_C * x + Config.REGRESSION_D; } //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; @@ -309,15 +346,15 @@ public double autoVelocityForAuto() { if (x < 100 && x > 0) { //launcherPI.p = 0.0015; //launcherPI.i = 0; - lastAutoVelocity = REGRESSION_A * x + REGRESSION_B; + lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; return lastAutoVelocity; } if (x < 0) { return lastAutoVelocity; } else { - launcherPI.p = 0.004; - launcherPI.i = 0.0002; - return REGRESSION_C_AUTO * x + REGRESSION_D_AUTO; + Config.launcherPI.p = 0.004; + Config.launcherPI.i = 0.0002; + return Config.REGRESSION_C_AUTO * x + Config.REGRESSION_D_AUTO; } //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; @@ -325,27 +362,27 @@ public double autoVelocityForAuto() { public void setRegressionCAuto() { // Spin the motors pid goes here - REGRESSION_C = REGRESSION_C_AUTO; + Config.REGRESSION_C = Config.REGRESSION_C_AUTO; } public void setRegressionDAuto() { // Spin the motors pid goes here - REGRESSION_D = REGRESSION_D_AUTO; + Config.REGRESSION_D = Config.REGRESSION_D_AUTO; } public void setRegressionCTeleop() { // Spin the motors pid goes here - REGRESSION_C = REGRESSION_C_TELEOP; + Config.REGRESSION_C = Config.REGRESSION_C_TELEOP; } public void setRegressionDTeleop() { // Spin the motors pid goes here - REGRESSION_D = REGRESSION_D_TELEOP; + Config.REGRESSION_D = Config.REGRESSION_D_TELEOP; } public void increaseRegressionDTeleop() { // Spin the motors pid goes here - REGRESSION_D += 15; + Config.REGRESSION_D += 15; } @Override From 55433c3b9bee6d343987213f018960caaa347689 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 05:59:02 -0800 Subject: [PATCH 2/9] Cranked through the codebase, and have most everything self-contained now --- .../firstinspires/ftc/sixteen750/Robot.java | 17 +- .../sixteen750/commands/AltAutoOrient.java | 1 - .../sixteen750/commands/AltAutoVelocity.java | 34 -- .../ftc/sixteen750/commands/TeleCommands.java | 32 -- .../ftc/sixteen750/commands/auto/Paths.java | 3 +- .../sixteen750/controls/DriverController.java | 19 +- .../ftc/sixteen750/opmodes/DualTeleOp.java | 17 +- .../opmodes/auto/Blue12BallFar.java | 10 +- .../sixteen750/opmodes/auto/Blue9BallFar.java | 7 +- .../opmodes/auto/Blue9BallFarNew.java | 10 +- .../auto/BlueNearFirstandSecondLever.java | 7 +- .../opmodes/auto/BlueNearLever.java | 4 +- .../opmodes/auto/BlueNearLeverNoThird.java | 4 +- .../opmodes/auto/BlueNearSecondLever.java | 4 +- .../opmodes/auto/FarZoneDriveForward.java | 7 +- .../sixteen750/opmodes/auto/Red12BallFar.java | 9 +- .../sixteen750/opmodes/auto/Red9BallFar.java | 7 +- .../auto/RedNearFirstandSecondLever.java | 7 +- .../sixteen750/opmodes/auto/RedNearLever.java | 7 +- .../opmodes/auto/RedNearLeverNoThird.java | 7 +- .../opmodes/auto/RedNearSecondLever.java | 7 +- .../sixteen750/opmodes/auto/TestOpmode.java | 3 - .../subsystems/LauncherSubsystem.java | 365 +++++++++++------- .../subsystems/LimelightSubsystem.java | 12 +- .../subsystems/TargetAcquisition.java | 13 + 25 files changed, 306 insertions(+), 307 deletions(-) delete mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java create mode 100644 Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java index b4a2d105..992b2155 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java @@ -42,7 +42,22 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) { this.intakeSubsystem = new IntakeSubsystem(hw); } if (Setup.Connected.LAUNCHERSUBSYSTEM) { - this.launcherSubsystem = new LauncherSubsystem(hw); + // For Review: + // The limelightSubsystem now implements the "TargetAcquisition" interface. This let's + // us completely decouple the LauncherSubsystem from the LimelightSubsystem, and instead + // just requires that we provide *anything* that will tell us the distance to target. + // We could build out a little thing that only uses Odometry to decide where the target + // is, instead, and wouldn't need to change anything in the LauncherSubsystem. + this.launcherSubsystem = new LauncherSubsystem( + hw.launcher1, + hw.launcher2, + limelightSubsystem, + hw::voltage + ); + } else { + // This should let us run code (scheduled commands, for instance...) without any + // hardware in the launcher subsystem + this.launcherSubsystem = new LauncherSubsystem(); } if (Setup.Connected.BRAKESUBSYSTEM) { this.brakeSubsystem = new BrakeSubsystem(hw); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java index cf4ffc55..20a0a325 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java @@ -11,7 +11,6 @@ import com.technototes.library.util.PIDFController; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; @Configurable diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java deleted file mode 100644 index fcddc163..00000000 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.sixteen750.commands; - -import com.bylazar.configurables.annotations.Configurable; -import com.pedropathing.geometry.Pose; -import com.technototes.library.command.Command; -import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; - -@Configurable -public class AltAutoVelocity implements Command { - - public Robot robot; - - public AltAutoVelocity(Robot r) { - robot = r; - } - - @Override - public boolean isFinished() { - //return !robot.follower.isBusy(); - return false; - } - - @Override - public void execute() { - robot.launcherSubsystem.Launch(); - } - - // @Override - // public void end(boolean s) { - // robot.follower.drivetrain.breakFollowing(); - // } -} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java index 21e62d5f..43d21f89 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java @@ -22,14 +22,6 @@ public static Command RumbleOff(Robot r) { return Command.create(r.intakeSubsystem::setRumbleOff); } - public static Command IncreaseMotor(Robot r) { - return Command.create(r.launcherSubsystem::IncreaseMotorVelocity); - } - - public static Command DecreaseMotor(Robot r) { - return Command.create(r.launcherSubsystem::DecreaseMotorVelocity); - } - public static Command Intake(Robot r) { return Command.create(r.intakeSubsystem::Intake); } @@ -89,28 +81,4 @@ public static Command MotorPowerTest(Robot r) { public static Command MotorVelocityTest(Robot r) { return Command.create(r.testSubsystem::setMotorVelocityTest); } - - public static Command ReadVelocity(Robot r) { - return Command.create(r.launcherSubsystem::readVelocity); - } - - public static Command SetRegressionCAuto(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionCAuto); - } - - public static Command SetRegressionDAuto(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionDAuto); - } - - public static Command SetRegressionCTeleop(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionCTeleop); - } - - public static Command SetRegressionDTeleop(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionDTeleop); - } - - public static Command IncreaseRegressionDTeleop(Robot r) { - return Command.create(r.launcherSubsystem::increaseRegressionDTeleop); - } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java index 8f5ab6bb..83f78549 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; import org.firstinspires.ftc.sixteen750.commands.AltAutoOrientFar; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Configurable public class Paths { @@ -26,7 +27,7 @@ public static Command Launching3Balls(Robot r) { new ParallelCommandGroup( TeleCommands.Intake(r), TeleCommands.GateUp(r), - TeleCommands.AutoLaunch1(r) + LauncherCommand.AutoLaunch1() ), new WaitCommand(3), TeleCommands.GateDown(r), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java index d16e018c..8bba887d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java @@ -8,13 +8,10 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; import org.firstinspires.ftc.sixteen750.commands.PedroDriver; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; -import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; public class DriverController { @@ -130,13 +127,13 @@ public void bindDriveControls() { } public void bindLaunchControls() { - launchButton.whilePressed(LauncherSubsystem.Commands.Launch(robot.launcherSubsystem)); - launchButton.whenReleased(LauncherSubsystem.Commands.StopLaunch(robot.launcherSubsystem)); - // CloseShoot.whenPressed(TeleCommands.SetCloseShoot(robot)); - // FarShoot.whenPressed(TeleCommands.SetFarShoot(robot)); - MotorIncrease.whenPressed(robot.launcherSubsystem::IncreaseMotorVelocity); - MotorDecrease.whenPressed(robot.launcherSubsystem::DecreaseMotorVelocity); - increaseD.whenPressed(robot.launcherSubsystem::increaseRegressionDTeleop); + launchButton.whilePressed(LauncherCommand.Launch()); + launchButton.whenReleased(LauncherCommand.StopLaunch()); + // CloseShoot.whenPressed(LauncherCommand.SetCloseShoot()); + // FarShoot.whenPressed(LauncherCommand.SetFarShoot()); + MotorIncrease.whenPressed(LauncherCommand.IncreaseMotor()); + MotorDecrease.whenPressed(LauncherCommand.DecreaseMotor()); + increaseD.whenPressed(LauncherCommand.IncreaseRegressionDTeleop()); } public void bindIntakeControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java index 5b8efe57..7b52b751 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java @@ -3,31 +3,22 @@ import static org.firstinspires.ftc.sixteen750.Setup.HardwareNames.AprilTag_Pipeline; import com.bylazar.telemetry.PanelsTelemetry; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; import com.technototes.library.command.SequentialCommandGroup; import com.technototes.library.structure.CommandOpMode; import com.technototes.library.util.Alliance; -import java.util.Arrays; -import java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.controls.OperatorController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @TeleOp(name = "Dual Control") @SuppressWarnings("unused") @@ -56,8 +47,7 @@ public void uponInit() { new SequentialCommandGroup( HeadingHelper.RestorePreviousPosition(robot.follower), DrivingCommands.ResetGyro(controlsDriver.pedroDriver), - TeleCommands.SetRegressionCTeleop(robot), - TeleCommands.SetRegressionDTeleop(robot) + LauncherCommand.SetRegressionTeleop() ), OpModeState.INIT ); @@ -80,9 +70,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } @Override diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java index f24e7ff2..80c374a1 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue12BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -33,12 +34,11 @@ public void uponInit() { CommandScheduler.register(robot.limelightSubsystem); CommandScheduler.scheduleForState( new SequentialCommandGroup( - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoldIntake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -76,7 +76,7 @@ public void uponInit() { TeleCommands.Intake(robot) ), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java index 04e868ec..ce6ca2df 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java @@ -14,6 +14,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue9BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -32,8 +33,8 @@ public void uponInit() { CommandScheduler.scheduleForState( new SequentialCommandGroup( TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.Intake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -55,7 +56,7 @@ public void uponInit() { Paths.AutoLaunching3Balls(robot), new PedroPathCommand(robot.follower, p.launchfartopark), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java index 6b9d8677..63b5df99 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue9BallFarNew", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -33,12 +34,11 @@ public void uponInit() { CommandScheduler.register(robot.limelightSubsystem); CommandScheduler.scheduleForState( new SequentialCommandGroup( - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoldIntake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -73,7 +73,7 @@ public void uponInit() { TeleCommands.Intake(robot) ), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java index 7d98e915..5f8a8ee0 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java @@ -13,7 +13,6 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; @@ -21,6 +20,7 @@ import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -106,9 +106,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java index 06e1c039..050aa20d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java @@ -8,13 +8,13 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java index a14a0ea5..b22120dd 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java @@ -8,13 +8,13 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣NoThird", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java index a6e8f38b..cdfc1a51 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java @@ -8,13 +8,13 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java index fe1b49f8..09ab07a4 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java @@ -16,6 +16,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "FarZoneDriveForward", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -38,12 +39,12 @@ public void uponInit() { CommandScheduler.scheduleForState( new SequentialCommandGroup( new LLSetup(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.HoodUp(robot), new WaitCommand(2), Paths.AutoLaunching3Balls(robot), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), new WaitCommand(18), new DriveAutoCommand(robot.follower, 0.5), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java index b68eec7e..a5cd9d0a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java @@ -9,7 +9,6 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.LLSetup; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; @@ -17,6 +16,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Red12BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -39,13 +39,12 @@ public void uponInit() { //new AltAutoVelocity(robot).alongWith( new SequentialCommandGroup( new LLSetup(robot), - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), TeleCommands.GateUp(robot), TeleCommands.Intake(robot), - TeleCommands.SetFarShoot(robot), + LauncherCommand.SetFarShoot(), TeleCommands.HoldIntake(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.RStartFartolaunchfar), new WaitCommand(0.5), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java index c67cce5d..07167ddb 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Red9BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -35,8 +36,8 @@ public void uponInit() { new SequentialCommandGroup( new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.Intake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.RStartFartolaunchfar), @@ -57,7 +58,7 @@ public void uponInit() { Paths.AutoLaunching3Balls(robot), new PedroPathCommand(robot.follower, p.Rlaunchfartopark), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java index 149c9f9b..8ebdef36 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java @@ -13,7 +13,6 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; @@ -21,6 +20,7 @@ import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -106,9 +106,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java index 034fb350..f2c2bc92 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java @@ -13,7 +13,6 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; @@ -21,6 +20,7 @@ import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java index 0dbaafe6..2eba154f 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java @@ -13,7 +13,6 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; @@ -21,6 +20,7 @@ import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣NoThird", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java index ec629bad..84a05f49 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java @@ -13,7 +13,6 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; @@ -21,6 +20,7 @@ import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; +import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java index 57c8bb0d..2ce0fd70 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java @@ -4,15 +4,12 @@ import com.technototes.library.command.CommandScheduler; import com.technototes.library.command.ParallelCommandGroup; import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; import com.technototes.library.structure.CommandOpMode; import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.LLSetup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java index a4c071a2..35227f07 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java @@ -11,68 +11,48 @@ import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; import com.technototes.library.util.PIDFController; +import java.util.Locale; +import java.util.function.DoubleSupplier; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.sixteen750.Hardware; -import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; @Configurable public class LauncherSubsystem implements Loggable, Subsystem { - public class Commands { - - public Command Launch(LauncherSubsystem l) { - return Command.create(l::Launch); - } - - public Command SetFarShoot(LauncherSubsystem l) { - return Command.create(l::FarShoot); - } - - public static Command SetCloseShoot(LauncherSubsystem l) { - return Command.create(l::CloseShoot); - } - - public static Command AutoLaunch1(LauncherSubsystem l) { - return Command.create(l::AutoLaunch1); - } - - public static Command AutoLaunch2(LauncherSubsystem l) { - return Command.create(l::AutoLaunch2); - } - - public static Command FarAutoLaunch(LauncherSubsystem l) { - return Command.create(l::FarAutoLaunch); - } - - public static Command StopLaunch(LauncherSubsystem l) { - return Command.create(l::Stop); - } - } - + // This is a little strange: It's a place to tuck away a reference to the Launcher Subsystem, + // so that all the commands can get to it there. + // It let's us do this: + // button.whenPressed(LauncherSubsystem.Commands.IncreaseMotor()); + // Instead of this: + // button.whenPressed(LauncherSubsystem.Commands.IncreaseMotor(r.launcherSubsystem)); + private static LauncherSubsystem self = null; + + // *ALL* the configuration should go in here. I moved some things that had been constants up + // to here, as they are "bot build configuration": are the motors reversed. + // TODO: Make "are there 1 or 2 motors" a configuration setting public static class Config { + // Is the primary intake motor reversed? + public static boolean PrimaryReversed = true; + // Is the secondary intake motor reversed? + public static boolean SecondaryReversed = false; + // @Log.Number(name = "Motor Power") // public static double MOTOR_POWER = 0.65; // 0.5 1.0 - public static double closetargetLaunchVelocity = 1400; - public static double fartargetLaunchVelocity = 1850; - public static double fartargetLaunchVelocityforAuto = 2300; - public static double targetLaunchVelocityforAuto1 = 1950; - public static double targetLaunchVelocityforAuto2 = 1850; + public static double CloseTargetLaunchVelocity = 1400; + public static double FarTargetLaunchVelocity = 1850; + public static double FarTargetLaunchVelocityForAuto = 2300; + public static double TargetLaunchVelocityForAuto1 = 1950; + public static double TargetLaunchVelocityForAuto2 = 1850; + // TODO: Document this better public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); - public static PIDFCoefficients launcherPI_ForAuto = new PIDFCoefficients( - 0.0015, - 0.0000, - 0.0, - 0 - ); //p = 0.004, i = 0.00020 public static double SPIN_F_SCALE = 0.00021; public static double SPIN_VOLT_COMP = 0.0216; public static double DIFFERENCE = 0.0046; public static double PEAK_VOLTAGE = 13; + // TODO: Make this more configurable. Maybe just turn it into a little helper function that + // lives in this class? public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula @@ -83,6 +63,90 @@ public static class Config { public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula } + // *All* commands for the subsystem belong in here. It's easy for the simple "call a method" + // commands, but for more comlicated commands, scroll down to see AutoVelocity + public static class LauncherCommand { + + public static Command Launch() { + return Command.create(self::Launch); + } + + public static Command SetFarShoot() { + return Command.create(self::FarShoot); + } + + public static Command SetCloseShoot() { + return Command.create(self::CloseShoot); + } + + public static Command AutoLaunch1() { + return Command.create(self::AutoLaunch1); + } + + public static Command AutoLaunch2() { + return Command.create(self::AutoLaunch2); + } + + public static Command FarAutoLaunch() { + return Command.create(self::FarAutoLaunch); + } + + public static Command StopLaunch() { + return Command.create(self::Stop); + } + + public static Command IncreaseMotor() { + return Command.create(self::IncreaseMotorVelocity); + } + + public static Command DecreaseMotor() { + return Command.create(self::DecreaseMotorVelocity); + } + + public static Command ReadVelocity() { + return Command.create(self::readVelocity); + } + + public static Command SetRegressionAuto() { + return Command.create(self::setRegressionAuto); + } + + public static Command SetRegressionTeleop() { + return Command.create(self::setRegressionTeleop); + } + + public static Command IncreaseRegressionDTeleop() { + return Command.create(self::increaseRegressionDTeleop); + } + + // This is just to make all commands look the same to the 'outside' user: + // You just call LauncherCommands.AutoVelocity() instead of needing to differentiate + // between simple Command.create's and more complex "class" commands. + public static Command AutoVelocity() { + return new AutoVelocityImplementation(); + } + + // This class is protected to ensure that elsewhere, you can't ever use + // button.whenPressed(new AutoVelocityImplementation()); + // but instead you have to use + // button.whenPressed(LaunchCommand.AutoVelocity()); + protected static class AutoVelocityImplementation implements Command { + + public AutoVelocityImplementation() {} + + @Override + public boolean isFinished() { + //return !robot.follower.isBusy(); + return false; + } + + @Override + public void execute() { + LauncherSubsystem.self.Launch(); + } + } + } + @Log.Number(name = "Target Velocity") public static double targetLaunchVelocity = 1150; @@ -110,110 +174,98 @@ public static class Config { private static PIDFController launcherPID; public static double lastAutoVelocity = 0; - boolean hasHardware; - public Robot robot; - public PIDFCoefficients launcherPIDF = new PIDFCoefficients(0, 0.0, 0.0, 0); - public PIDFController launcherPIDFController; - public static double FEEDFORWARD_COEFFICIENT = 0.0; - - public static double MINIMUM_VELOCITY = 1140; - public static double RPM_PER_FOOT = 62.3; - @Log.Number(name = "AutoAim Velocity") public static double autoVelocity; public double launcherPow; // not tested just placeholder but should be used + + // External components this requires: EncodedMotor launcher1; EncodedMotor launcher2; - LimelightSubsystem ls; + + TargetAcquisition targetAcquisition; + DoubleSupplier voltage; // @Log(name = "Flywheel at Velocity") // public static boolean ready; - public LauncherSubsystem(Hardware h) { - hasHardware = Setup.Connected.LAUNCHERSUBSYSTEM; - // Do stuff in here - if (hasHardware) { - launcher1 = h.launcher1; - launcher2 = h.launcher2; - launcher1.setDirection(DcMotorSimple.Direction.REVERSE); - launcher2.setDirection(DcMotorSimple.Direction.FORWARD); + public LauncherSubsystem( + EncodedMotor primary, + EncodedMotor secondary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + self = this; + launcher1 = primary; + if (launcher1 != null) { + launcher1.setDirection( + Config.PrimaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD + ); launcher1.coast(); - launcher2.coast(); - launcher1.setPIDFCoefficients(launcherPIDF); - launcher2.setPIDFCoefficients(launcherPIDF); - //ready = false; - ls = new LimelightSubsystem(h); - double ADDITION = (Config.PEAK_VOLTAGE - h.voltage()); - if (ADDITION == 0) { - Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; - } else { - Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); - } - launcherPID = new PIDFController(Config.launcherPI, target -> - target == 0 - ? 0 - : (Config.SPIN_F_SCALE * target) + - (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, h.voltage())) + } + launcher2 = secondary; + if (launcher2 != null) { + launcher2.setDirection( + Config.SecondaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD ); - // top.setPIDFCoefficients(launcherP); - setTargetSpeed(0); + launcher2.coast(); + } + // ready = false; + targetAcquisition = targetSubsystem; + voltage = voltageSup; + double ADDITION = (Config.PEAK_VOLTAGE - voltage.getAsDouble()); + if (ADDITION == 0) { + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; } else { - launcher1 = null; - launcher2 = null; + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); } + launcherPID = new PIDFController(Config.launcherPI, target -> + target == 0 + ? 0 + : (Config.SPIN_F_SCALE * target) + + (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) + ); + setTargetSpeed(0); CommandScheduler.register(this); } + public LauncherSubsystem() { + this(null, null, null, () -> 0); + } + public void Launch() { // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(autoVelocity()); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } + setTargetSpeed(autoVelocity()); //change to auto aim velocity } public void CloseShoot() { // Spin the motors pid goes here - if (hasHardware) { - targetLaunchVelocity = Config.closetargetLaunchVelocity; - } + targetLaunchVelocity = Config.CloseTargetLaunchVelocity; } public void FarShoot() { // Spin the motors pid goes here - if (hasHardware) { - targetLaunchVelocity = Config.fartargetLaunchVelocity; - } + targetLaunchVelocity = Config.FarTargetLaunchVelocity; } public void AutoLaunch1() { // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(Config.targetLaunchVelocityforAuto1); //change to auto aim velocity - //launcher1.setVelocity(targetLaunchVelocityforAuto1); - //launcher2.setVelocity(targetLaunchVelocityforAuto1); - } + setTargetSpeed(Config.TargetLaunchVelocityForAuto1); //change to auto aim velocity } public void AutoLaunch2() { // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(Config.targetLaunchVelocityforAuto2); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } + setTargetSpeed(Config.TargetLaunchVelocityForAuto2); //change to auto aim velocity } public void FarAutoLaunch() { // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(Config.fartargetLaunchVelocityforAuto); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } + setTargetSpeed(Config.FarTargetLaunchVelocityForAuto); //change to auto aim velocity } public double readVelocity() { @@ -222,7 +274,7 @@ public double readVelocity() { // } else { // return ready = false; // } - return launcher1.getVelocity(); + return launcher1 != null ? launcher1.getVelocity() : Double.NaN; // Nan = "Not a Number" // 12.25 stationary voltage - had to decrease velocity by 150 (trial one: true, trial two: true) // 11.84 stationary voltage - had to decrease velocity by 100? (trial one: @@ -241,8 +293,10 @@ public double getTargetSpeed() { private void setMotorPower(double pow) { double power = Math.clamp(pow, -1, 1); targetPower = power; - if (launcher1 != null && launcher2 != null) { + if (launcher1 != null) { launcher1.setPower(power); + } + if (launcher2 != null) { launcher2.setPower(power); } } @@ -269,58 +323,45 @@ public double getMotor2Current() { } public void Stop() { - if (hasHardware) { - // launcher1.setVelocity(0); - // launcher2.setVelocity(0); - //launcher1.setPower(0); - //launcher2.setPower(0); - launcherPID.setTarget(0); - } + launcherPID.setTarget(0); } public void IncreaseMotorVelocity() { // Spin the motors pid goes here - if (hasHardware) { - additionAmount += additionDelta; - } + additionAmount += additionDelta; } public void DecreaseMotorVelocity() { // Spin the motors pid goes here - if (hasHardware) { - additionAmount -= additionDelta; - } - } - - public void setMotorVelocityTest() { - launcher1.setVelocity(targetLaunchVelocity); + additionAmount -= additionDelta; } - // public void setMotorPowerTest() { - // launcher1.setPower(MOTOR_POWER); - // CurrentLaunchVelocity = getMotor1Velocity(); - // } - public double getMotor1Velocity() { - return launcher1.getVelocity(); + return launcher1 != null ? launcher1.getVelocity() : Double.NaN; // Not a Number } public double getMotor2Velocity() { - return launcher2.getVelocity(); + return launcher2 != null ? launcher2.getVelocity() : Double.NaN; // Not a Number } + // This is wrong (and, as it turns out, unused), so I just commented it out + // TODO: We should discuss how to accomplish what this is attempting to do + /* public void VelocityShoot() { if ( getMotor1Velocity() == targetLaunchVelocity && getMotor2Velocity() == targetLaunchVelocity ) { + // This doesn't do anything. It *creates* a command, but the command is never + // scheduled, so it won't ever actually do anything. TeleCommands.GateDown(robot); } } + */ public double autoVelocity() { // x = distance in feet - double x = ls.getDistance(); + double x = getTargetDistance(); if (x < 100 && x > 0) { //launcherPI.p = 0.0015; @@ -341,7 +382,7 @@ public double autoVelocity() { public double autoVelocityForAuto() { // x = distance in feet - double x = ls.getDistance(); + double x = getTargetDistance(); if (x < 100 && x > 0) { //launcherPI.p = 0.0015; @@ -360,23 +401,15 @@ public double autoVelocityForAuto() { //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; } - public void setRegressionCAuto() { + public void setRegressionAuto() { // Spin the motors pid goes here Config.REGRESSION_C = Config.REGRESSION_C_AUTO; - } - - public void setRegressionDAuto() { - // Spin the motors pid goes here Config.REGRESSION_D = Config.REGRESSION_D_AUTO; } - public void setRegressionCTeleop() { + public void setRegressionTeleop() { // Spin the motors pid goes here Config.REGRESSION_C = Config.REGRESSION_C_TELEOP; - } - - public void setRegressionDTeleop() { - // Spin the motors pid goes here Config.REGRESSION_D = Config.REGRESSION_D_TELEOP; } @@ -385,6 +418,13 @@ public void increaseRegressionDTeleop() { Config.REGRESSION_D += 15; } + // This lets the 'no hardware' or 'subsystem disabled' thing still work without a functional + // target acquisition subsystem + private double getTargetDistance() { + if (targetAcquisition != null) return targetAcquisition.getDistance(); + return -1; + } + @Override public void periodic() { autoVelocity = autoVelocity(); @@ -399,8 +439,35 @@ public void periodic() { err = launcherPID.getLastError(); motorVelocity = getMotorSpeed(); - power = launcher1.getPower(); + if (launcher1 != null) { + power = launcher1.getPower(); + } else { + power = -1; + } launcher1Current = getMotor1Current(); launcher2Current = getMotor2Current(); } + + // This should be used by a test opmode to check that the basics are working. + public String hardwareValidation(double power1, double power2) { + String res = ""; + if (launcher1 != null) { + launcher1.setPower(power1); + } else { + res += "(no launcher1)"; + } + if (launcher2 != null) { + launcher2.setPower(power2); + } else { + res += "(no launcher2)"; + } + res += String.format( + Locale.ENGLISH, + "Launcher Speed: %.2f, Current1: %.2f, Current2: %.2f", + getMotorSpeed(), + getMotor1Current(), + getMotor2Current() + ); + return res; + } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java index 13b6c1e5..9a9d0158 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java @@ -6,9 +6,6 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -17,7 +14,7 @@ import org.firstinspires.ftc.sixteen750.Setup; @Configurable -public class LimelightSubsystem implements Loggable, Subsystem { +public class LimelightSubsystem implements Loggable, Subsystem, TargetAcquisition { boolean hasHardware; @@ -101,10 +98,15 @@ public double getLimelightRotation() { // if it start turning away from the apriltag } - public double getTX() { + // Don't know if the signs for these two things are correct or not... + public double getHorizontalPosition() { return Xangle; } + public double getVerticalPosition() { + return Yangle; + } + public void LimelightStartup() { limelight.setPollRateHz(100); limelight.pipelineSwitch(AprilTag_Pipeline); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java new file mode 100644 index 00000000..0ef5093f --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java @@ -0,0 +1,13 @@ +package org.firstinspires.ftc.sixteen750.subsystems; + +// This should be promoted to a library interface, so it's available for anything to use/implement +public interface TargetAcquisition { + // This should be in *inches* + double getDistance(); + // Returns the location, in degrees, of the target relative to the current bot perspective + // Clockwise is negative, Counter-clockwise is positive + double getHorizontalPosition(); + // Returns the location, in degrees, of the target relative to the current bot perspective. + // "Above perpsective" is positive, "Below perspective" is negative + double getVerticalPosition(); +} From d15864483a43496dc7de9e06e924a68816f23962 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 11:33:54 -0800 Subject: [PATCH 3/9] It's not a subsystem: It's a *component*. --- .../ftc/sixteen750/Hardware.java | 2 +- .../firstinspires/ftc/sixteen750/Robot.java | 14 +- .../firstinspires/ftc/sixteen750/Setup.java | 2 +- .../sixteen750/commands/AltAutoOrient.java | 2 +- .../sixteen750/commands/AltAutoOrientFar.java | 2 +- .../ftc/sixteen750/commands/auto/Paths.java | 2 +- .../LauncherComponent.java} | 152 +++++++++--------- .../sixteen750/controls/DriverController.java | 4 +- .../ftc/sixteen750/opmodes/DualTeleOp.java | 12 +- .../opmodes/auto/Blue12BallFar.java | 2 +- .../sixteen750/opmodes/auto/Blue9BallFar.java | 2 +- .../opmodes/auto/Blue9BallFarNew.java | 2 +- .../auto/BlueNearFirstandSecondLever.java | 14 +- .../opmodes/auto/BlueNearLever.java | 2 +- .../opmodes/auto/BlueNearLeverNoThird.java | 2 +- .../opmodes/auto/BlueNearSecondLever.java | 2 +- .../opmodes/auto/FarZoneDriveForward.java | 2 +- .../sixteen750/opmodes/auto/Red12BallFar.java | 2 +- .../sixteen750/opmodes/auto/Red9BallFar.java | 2 +- .../auto/RedNearFirstandSecondLever.java | 14 +- .../sixteen750/opmodes/auto/RedNearLever.java | 14 +- .../opmodes/auto/RedNearLeverNoThird.java | 14 +- .../opmodes/auto/RedNearSecondLever.java | 14 +- 23 files changed, 142 insertions(+), 138 deletions(-) rename Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/{subsystems/LauncherSubsystem.java => component/LauncherComponent.java} (81%) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java index 86c5dd37..02e5f904 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java @@ -68,7 +68,7 @@ public Hardware(HardwareMap hwmap) { if (Setup.Connected.INTAKESUBSYSTEM) { intake = this.map.get(DcMotorEx.class, Setup.HardwareNames.INTAKE_MOTOR); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { + if (Setup.Connected.LAUNCHERCOMPONENT) { launcher1 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR1); launcher2 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR2); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java index 992b2155..08433bf9 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java @@ -3,11 +3,11 @@ import com.pedropathing.follower.Follower; import com.technototes.library.logger.Loggable; import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.AimingSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.BrakeSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.SafetySubsystem; import org.firstinspires.ftc.sixteen750.subsystems.TestSubsystem; @@ -20,7 +20,7 @@ public class Robot implements Loggable { public double initialVoltage; public SafetySubsystem safetySubsystem; - public LauncherSubsystem launcherSubsystem; + public LauncherComponent launcherComponent; public IntakeSubsystem intakeSubsystem; public BrakeSubsystem brakeSubsystem; public AimingSubsystem aimingSubsystem; @@ -41,14 +41,14 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) { if (Setup.Connected.INTAKESUBSYSTEM) { this.intakeSubsystem = new IntakeSubsystem(hw); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { + if (Setup.Connected.LAUNCHERCOMPONENT) { // For Review: // The limelightSubsystem now implements the "TargetAcquisition" interface. This let's - // us completely decouple the LauncherSubsystem from the LimelightSubsystem, and instead + // us completely decouple the LauncherComponent from the LimelightSubsystem, and instead // just requires that we provide *anything* that will tell us the distance to target. // We could build out a little thing that only uses Odometry to decide where the target - // is, instead, and wouldn't need to change anything in the LauncherSubsystem. - this.launcherSubsystem = new LauncherSubsystem( + // is, instead, and wouldn't need to change anything in the LauncherComponent. + this.launcherComponent = new LauncherComponent( hw.launcher1, hw.launcher2, limelightSubsystem, @@ -57,7 +57,7 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) { } else { // This should let us run code (scheduled commands, for instance...) without any // hardware in the launcher subsystem - this.launcherSubsystem = new LauncherSubsystem(); + this.launcherComponent = new LauncherComponent(); } if (Setup.Connected.BRAKESUBSYSTEM) { this.brakeSubsystem = new BrakeSubsystem(hw); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java index 0705e557..209fcf00 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java @@ -16,7 +16,7 @@ public static class Connected { public static boolean EXTERNAL_IMU = true; public static boolean OTOS = false; public static boolean INTAKESUBSYSTEM = true; - public static boolean LAUNCHERSUBSYSTEM = true; + public static boolean LAUNCHERCOMPONENT = true; public static boolean AIMINGSUBSYSTEM = true; public static boolean BRAKESUBSYSTEM = true; } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java index 20a0a325..3590048f 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java @@ -72,7 +72,7 @@ public void execute() { // Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation() // ); // robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false); - // LauncherSubsystem.targetPower = 1; + // LauncherComponent.targetPower = 1; // firsttime = false; //} currentPose = robot.follower.getPose(); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java index 2b6df322..89ad3f37 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java @@ -69,7 +69,7 @@ public void execute() { // Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation() // ); // robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false); - // LauncherSubsystem.targetPower = 1; + // LauncherComponent.targetPower = 1; // firsttime = false; //} currentPose = robot.follower.getPose(); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java index 83f78549..cae8e313 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java @@ -15,7 +15,7 @@ import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; import org.firstinspires.ftc.sixteen750.commands.AltAutoOrientFar; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; @Configurable public class Paths { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java similarity index 81% rename from Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java rename to Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java index 35227f07..b7da1436 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.sixteen750.subsystems; +package org.firstinspires.ftc.sixteen750.component; import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -14,17 +14,18 @@ import java.util.Locale; import java.util.function.DoubleSupplier; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.sixteen750.subsystems.TargetAcquisition; @Configurable -public class LauncherSubsystem implements Loggable, Subsystem { +public class LauncherComponent implements Loggable, Subsystem { // This is a little strange: It's a place to tuck away a reference to the Launcher Subsystem, // so that all the commands can get to it there. // It let's us do this: - // button.whenPressed(LauncherSubsystem.Commands.IncreaseMotor()); + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor()); // Instead of this: - // button.whenPressed(LauncherSubsystem.Commands.IncreaseMotor(r.launcherSubsystem)); - private static LauncherSubsystem self = null; + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor(r.launcherComponent)); + private static LauncherComponent self = null; // *ALL* the configuration should go in here. I moved some things that had been constants up // to here, as they are "bot build configuration": are the motors reversed. @@ -46,6 +47,10 @@ public static class Config { // TODO: Document this better public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static double Near_P = 0.004; + public static double Near_I = 0.0002; + public static double Far_P = 0.004; + public static double Far_I = 0.0002; public static double SPIN_F_SCALE = 0.00021; public static double SPIN_VOLT_COMP = 0.0216; public static double DIFFERENCE = 0.0046; @@ -64,7 +69,7 @@ public static class Config { } // *All* commands for the subsystem belong in here. It's easy for the simple "call a method" - // commands, but for more comlicated commands, scroll down to see AutoVelocity + // commands, but for more complicated commands, scroll down to see AutoVelocity/AutoVelocityImpl public static class LauncherCommand { public static Command Launch() { @@ -123,26 +128,27 @@ public static Command IncreaseRegressionDTeleop() { // You just call LauncherCommands.AutoVelocity() instead of needing to differentiate // between simple Command.create's and more complex "class" commands. public static Command AutoVelocity() { - return new AutoVelocityImplementation(); + return new AutoVelocityImpl(); } // This class is protected to ensure that elsewhere, you can't ever use - // button.whenPressed(new AutoVelocityImplementation()); + // button.whenPressed(new AutoVelocityImpl()); // but instead you have to use // button.whenPressed(LaunchCommand.AutoVelocity()); - protected static class AutoVelocityImplementation implements Command { + protected static class AutoVelocityImpl implements Command { - public AutoVelocityImplementation() {} + public AutoVelocityImpl() {} + // This command is designed to *never* finish. + // It should be run in a parallel command group/alongWith/raceWith group. @Override public boolean isFinished() { - //return !robot.follower.isBusy(); return false; } @Override public void execute() { - LauncherSubsystem.self.Launch(); + LauncherComponent.self.Launch(); } } } @@ -177,20 +183,13 @@ public void execute() { @Log.Number(name = "AutoAim Velocity") public static double autoVelocity; - public double launcherPow; - // not tested just placeholder but should be used - - // External components this requires: + // External dependencies this component requires: EncodedMotor launcher1; EncodedMotor launcher2; - TargetAcquisition targetAcquisition; DoubleSupplier voltage; - // @Log(name = "Flywheel at Velocity") - // public static boolean ready; - - public LauncherSubsystem( + public LauncherComponent( EncodedMotor primary, EncodedMotor secondary, TargetAcquisition targetSubsystem, @@ -198,7 +197,7 @@ public LauncherSubsystem( ) { self = this; launcher1 = primary; - if (launcher1 != null) { + if (hasLaunch1()) { launcher1.setDirection( Config.PrimaryReversed ? DcMotorSimple.Direction.REVERSE @@ -207,7 +206,7 @@ public LauncherSubsystem( launcher1.coast(); } launcher2 = secondary; - if (launcher2 != null) { + if (hasLaunch2()) { launcher2.setDirection( Config.SecondaryReversed ? DcMotorSimple.Direction.REVERSE @@ -234,10 +233,18 @@ public LauncherSubsystem( CommandScheduler.register(this); } - public LauncherSubsystem() { + public LauncherComponent() { this(null, null, null, () -> 0); } + public LauncherComponent( + EncodedMotor primary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + this(primary, null, targetSubsystem, voltageSup); + } + public void Launch() { // Spin the motors pid goes here setTargetSpeed(autoVelocity()); //change to auto aim velocity @@ -269,20 +276,15 @@ public void FarAutoLaunch() { } public double readVelocity() { - // if(launcher1.getVelocity() == TARGET_LAUNCH_VELOCITY && launcher2.getVelocity() == TARGET_LAUNCH_VELOCITY) { - // return ready = true; - // } else { - // return ready = false; - // } - return launcher1 != null ? launcher1.getVelocity() : Double.NaN; // Nan = "Not a Number" - - // 12.25 stationary voltage - had to decrease velocity by 150 (trial one: true, trial two: true) - // 11.84 stationary voltage - had to decrease velocity by 100? (trial one: + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Nan = "Not a Number" + } } public void setTargetSpeed(double speed) { targetSpeed = speed; - // top.setVelocity(speed); launcherPID.setTarget(speed); } @@ -293,30 +295,30 @@ public double getTargetSpeed() { private void setMotorPower(double pow) { double power = Math.clamp(pow, -1, 1); targetPower = power; - if (launcher1 != null) { + if (hasLaunch1()) { launcher1.setPower(power); } - if (launcher2 != null) { + if (hasLaunch2()) { launcher2.setPower(power); } } public double getMotorSpeed() { - if (launcher1 != null) { + if (hasLaunch1()) { return launcher1.getVelocity(); } return -1; } public double getMotor1Current() { - if (launcher1 != null) { + if (hasLaunch1()) { return launcher1.getAmperage(CurrentUnit.AMPS); } return -1; } public double getMotor2Current() { - if (launcher2 != null) { + if (hasLaunch2()) { return launcher2.getAmperage(CurrentUnit.AMPS); } return -1; @@ -337,11 +339,19 @@ public void DecreaseMotorVelocity() { } public double getMotor1Velocity() { - return launcher1 != null ? launcher1.getVelocity() : Double.NaN; // Not a Number + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Not a Number + } } public double getMotor2Velocity() { - return launcher2 != null ? launcher2.getVelocity() : Double.NaN; // Not a Number + if (hasLaunch2()) { + return launcher2.getVelocity(); + } else { + return Double.NaN; // Not a Number + } } // This is wrong (and, as it turns out, unused), so I just commented it out @@ -359,43 +369,27 @@ public void VelocityShoot() { } */ + // TOOD: Turn this into a quadratic equation instead, since that's what it's mimicking. public double autoVelocity() { - // x = distance in feet + // x = distance in inches double x = getTargetDistance(); - if (x < 100 && x > 0) { - //launcherPI.p = 0.0015; - //launcherPI.i = 0; - lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; - return lastAutoVelocity; - } if (x < 0) { return lastAutoVelocity; - } else { - Config.launcherPI.p = 0.004; - Config.launcherPI.i = 0.0002; - return Config.REGRESSION_C * x + Config.REGRESSION_D; - } - - //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; - } - - public double autoVelocityForAuto() { - // x = distance in feet - double x = getTargetDistance(); - - if (x < 100 && x > 0) { - //launcherPI.p = 0.0015; - //launcherPI.i = 0; + } else if (x < 100) { + // launcherPI.p = 0.0015; + // launcherPI.i = 0; lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; return lastAutoVelocity; - } - if (x < 0) { - return lastAutoVelocity; } else { - Config.launcherPI.p = 0.004; - Config.launcherPI.i = 0.0002; - return Config.REGRESSION_C_AUTO * x + Config.REGRESSION_D_AUTO; + // NOTE: These two lines don't appear to do anything, because we're not using the + // Near_P and Near_I values anywhere (they were commented out above) + // TODO: Assigning values to something in Config is 'bad form'. Instead, we should + // have the coefficients copied into the Launcher itself in the constructor, and change + // that here. + Config.launcherPI.p = Config.Far_P; + Config.launcherPI.i = Config.Far_I; + return Config.REGRESSION_C * x + Config.REGRESSION_D; } //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; @@ -421,7 +415,9 @@ public void increaseRegressionDTeleop() { // This lets the 'no hardware' or 'subsystem disabled' thing still work without a functional // target acquisition subsystem private double getTargetDistance() { - if (targetAcquisition != null) return targetAcquisition.getDistance(); + if (targetAcquisition != null) { + return targetAcquisition.getDistance(); + } return -1; } @@ -439,7 +435,7 @@ public void periodic() { err = launcherPID.getLastError(); motorVelocity = getMotorSpeed(); - if (launcher1 != null) { + if (hasLaunch1()) { power = launcher1.getPower(); } else { power = -1; @@ -451,12 +447,12 @@ public void periodic() { // This should be used by a test opmode to check that the basics are working. public String hardwareValidation(double power1, double power2) { String res = ""; - if (launcher1 != null) { + if (hasLaunch1()) { launcher1.setPower(power1); } else { res += "(no launcher1)"; } - if (launcher2 != null) { + if (hasLaunch2()) { launcher2.setPower(power2); } else { res += "(no launcher2)"; @@ -470,4 +466,12 @@ public String hardwareValidation(double power1, double power2) { ); return res; } + + private boolean hasLaunch1() { + return launcher1 != null; + } + + private boolean hasLaunch2() { + return launcher2 != null; + } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java index 8bba887d..bbf36409 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java @@ -11,7 +11,7 @@ import org.firstinspires.ftc.sixteen750.commands.PedroDriver; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; public class DriverController { @@ -58,7 +58,7 @@ public DriverController(CommandGamepad g, Robot r) { if (Setup.Connected.DRIVEBASE) { bindDriveControls(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { + if (Setup.Connected.LAUNCHERCOMPONENT) { bindLaunchControls(); } if (Setup.Connected.INTAKESUBSYSTEM) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java index 7b52b751..71d60805 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java @@ -14,11 +14,11 @@ import org.firstinspires.ftc.sixteen750.Setup; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.controls.OperatorController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @TeleOp(name = "Dual Control") @SuppressWarnings("unused") @@ -84,20 +84,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } */ diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java index 80c374a1..8366d21a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java @@ -12,10 +12,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue12BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java index ce6ca2df..42dd146b 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java @@ -11,10 +11,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue9BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java index 63b5df99..bb007dd6 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java @@ -12,10 +12,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Blue9BallFarNew", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java index 5f8a8ee0..240fad0a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java @@ -16,11 +16,11 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -117,20 +117,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java index 050aa20d..29773916 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java @@ -11,10 +11,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java index b22120dd..28585e1d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java @@ -11,10 +11,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever1️⃣NoThird", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java index cdfc1a51..86624bda 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java @@ -11,10 +11,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "BlueNearLever2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java index 09ab07a4..aaabd232 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java @@ -13,10 +13,10 @@ import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.DriveAutoCommand; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "FarZoneDriveForward", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java index a5cd9d0a..a268eb53 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java @@ -13,10 +13,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Red12BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java index 07167ddb..f56e4a4e 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java @@ -12,10 +12,10 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "Red9BallFar", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java index 8ebdef36..afd69791 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java @@ -16,11 +16,11 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -117,20 +117,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java index f2c2bc92..1ce88804 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java @@ -16,11 +16,11 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -114,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java index 2eba154f..7ccee326 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java @@ -16,11 +16,11 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever1️⃣NoThird", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -114,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java index 84a05f49..b27a9612 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java @@ -16,11 +16,11 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem.LauncherCommand; @Autonomous(name = "RedNearLever2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -114,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } From 2ee25826e2668c39c8859cca632476a40a1021d4 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 12:40:42 -0800 Subject: [PATCH 4/9] FeedFwd looks very complicated, but at the end of it...wrong? --- .../component/LauncherComponent.java | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java index b7da1436..2ad142ee 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java @@ -214,19 +214,38 @@ public LauncherComponent( ); launcher2.coast(); } - // ready = false; targetAcquisition = targetSubsystem; - voltage = voltageSup; + voltage = voltageSup != null ? voltageSup : () -> Config.PEAK_VOLTAGE; + // All the Feedfwd stuff below seems...confused. + // Notes: Addition will *increase* as voltage decreases (I think this is expected) + // but it likely won't ever be zero, and it could (with a new battery) be *negative* + // I've tried to model it here: https://www.desmos.com/calculator/gmzlhkyz5a + + // (I is the initial voltage delta (ADDITION), and V is the voltage as the bot is runs. + // Don't animate I, just V, and you'll see that as voltage decreases, so does the output + // power. Drag I around to change the initial voltage delta) + + // The thing that saves us is I, which does what I believe we're expecting: It increase + // power as initial voltage is lower. But then the FF function drops it. + + // My suggestion: rip out all the 'initial' stuff and make the core F function return higher + // values as voltage *decreases*, which is I think what we're really looking for anyway. + double ADDITION = (Config.PEAK_VOLTAGE - voltage.getAsDouble()); if (ADDITION == 0) { + // I'd be stunned if this code runs...ever. Not sure what's supposed to happen here. Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; } else { + // This seems like a sensible thing: We're adding some amount of voltage delta to + // compensate for a lower initial voltage Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); } launcherPID = new PIDFController(Config.launcherPI, target -> target == 0 ? 0 : (Config.SPIN_F_SCALE * target) + + // This is weird: We're going to *reduce* the output power slightly as voltage + // decreases over time. I don't think this is what we're trying to accomplish. (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) ); setTargetSpeed(0); @@ -438,7 +457,7 @@ public void periodic() { if (hasLaunch1()) { power = launcher1.getPower(); } else { - power = -1; + power = Double.NaN; } launcher1Current = getMotor1Current(); launcher2Current = getMotor2Current(); From 84aac4f4d2a2fe9d0c7cda2bc8113f9e5f2eb494 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 13:47:50 -0800 Subject: [PATCH 5/9] Did some fun stuff in the Launcher component. Going to try moving it to Mouse and see if I can just use it --- .../component/LauncherComponent.java | 132 +++++++++++++++++- .../subsystems/LimelightSubsystem.java | 1 + .../library/subsystem}/TargetAcquisition.java | 2 +- 3 files changed, 133 insertions(+), 2 deletions(-) rename {Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems => TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem}/TargetAcquisition.java (91%) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java index 2ad142ee..858825f6 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java @@ -3,18 +3,22 @@ import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; import com.technototes.library.util.PIDFController; import java.util.Locale; import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.sixteen750.subsystems.TargetAcquisition; @Configurable public class LauncherComponent implements Loggable, Subsystem { @@ -248,6 +252,20 @@ public LauncherComponent( // decreases over time. I don't think this is what we're trying to accomplish. (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) ); + // A quick wander around google comes up with something like this: + + // launcherMyPID = new PIDFController(Config.launcherPID, target -> + // (Config.kStaticFriction + Config.kVelocityConstant * target) / voltage.getAsDouble()); + + // The point is that motor RPM scales linearly with voltage, so to compensate, you should + // divide by voltage: Don't try to scale something by a delta from peak. Just divide. + + // To get solve that formula, get a fresh battery, run it at full power and measure the RPM. + // (Well, and figure out kStaticFriction, too: The lowest value that will still get the + // launcher barely moving) + + // These capabilities should probably go in the "validation" function at the bottom :D + setTargetSpeed(0); CommandScheduler.register(this); } @@ -486,6 +504,118 @@ public String hardwareValidation(double power1, double power2) { return res; } + // TODO: Code that measures kStaticFriction and kVelocityConstant for a FeedFwd function + public void feedFwdHelper(Telemetry tel, Gamepad gamepad) { + // Wait until a button's been pressed, then first identify kStaticFriction, by watching to + // see when the motor just barely starts moving (then back off by a couple percent) + motorHelperPower(0); + while (!anyButtonsPressed(gamepad)) { + tel.addLine("Please press a button on the gamepad to begin test"); + tel.update(); + } + while (anyButtonsPressed(gamepad)) { + tel.addLine("Release the button to begin the test"); + tel.update(); + } + // Okay, slowly raise the power applied to launcher1 until w + double staticFriction = 0.001; + double staticFrictionStep = 0.001; + ElapsedTime lastUpdate = new ElapsedTime(); + while (true) { + double v = voltage.getAsDouble(); + motorHelperPower(staticFriction / v); + tel.addLine("Press a button to abort"); + tel.addData("kStaticFriction", staticFriction); + tel.addData("Voltage", v); + tel.update(); + if (anyButtonsPressed(gamepad)) { + motorHelperPower(0); + return; + } + if (lastUpdate.milliseconds() >= 50) { + // We update every 25 milliseconds, just to give it time to trigger + if (getMotor1Velocity() != 0) { + break; + } + lastUpdate.reset(); + staticFriction += staticFrictionStep; + } + } + // If we're here, the system started moving. Stop the motors and set kStaticFriction to + // just below what was necessary to start the system moving. + staticFriction -= staticFrictionStep; + motorHelperPower(0); + // Display the kStaticFriction value we got: + while (!anyButtonsPressed(gamepad)) { + tel.addData("kStaticFriction-->>", staticFriction); + tel.addLine("Press a button to start the velocity measurement"); + tel.update(); + } + // Next up: Velocity! + while (anyButtonsPressed(gamepad)) { + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Release the button to start the velocity measurement"); + tel.update(); + } + lastUpdate.reset(); + double velocityConstant = 0; + MovingStatistics velocityConstantStats = new MovingStatistics(50); + while (!anyButtonsPressed(gamepad)) { + motorHelperPower(1); + if (lastUpdate.milliseconds() >= 50) { + double vel = getMotor1Velocity(); + double vol = voltage.getAsDouble(); + if (vel != 0) { + // power = (kStaticFriction + kVelocity * RPM) / v + // So + // 1 = (kSF + kV * RPM) / v; + // Multiply both sides by v + // v = kSF + kV * RPM + // move kStaticFriction over: + // v - KSF = kV * RPM + // swap sides + // kv * RPM = v - kSF + // and divide both sides by RPM + // kv = (v - kSF) / RPM + velocityConstant = (vol - staticFriction) / vel; + velocityConstantStats.add(velocityConstant); + } + } + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Press a button to stop velocity measurement"); + tel.addData("kVelocityConstant", velocityConstant); + tel.addData("Average kV", velocityConstantStats.getMean()); + tel.update(); + } + } + + private static boolean anyButtonsPressed(Gamepad g) { + return ( + g.a || + g.b || + g.x || + g.y || + g.dpad_down || + g.dpad_up || + g.dpad_left || + g.dpad_right || + g.options || + g.share || + g.left_bumper || + g.right_bumper || + g.start + ); + } + + private void motorHelperPower(double p) { + if (hasLaunch1()) { + launcher1.setPower(p); + } + if (hasLaunch2()) { + launcher2.setPower(p); + } + } + private boolean hasLaunch1() { return launcher1 != null; } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java index 9a9d0158..705b3c9a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java @@ -9,6 +9,7 @@ import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; import java.util.List; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Setup; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java similarity index 91% rename from Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java rename to TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java index 0ef5093f..7eb6fbff 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/TargetAcquisition.java +++ b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.sixteen750.subsystems; +package com.technototes.library.subsystem; // This should be promoted to a library interface, so it's available for anything to use/implement public interface TargetAcquisition { From 795f065144e031e0d201fb161fbee73650b12b2e Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 16:08:37 -0800 Subject: [PATCH 6/9] Getting Mouse working with the launcher component --- .../learnbot/component/LauncherComponent.java | 627 ++++++++++++++++++ .../ftc/learnbot/opmodes/LaucherFeedFwd.java | 36 + .../learnbot/opmodes/LauncherValidator.java | 67 ++ 3 files changed, 730 insertions(+) create mode 100644 LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java create mode 100644 LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java create mode 100644 LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java new file mode 100644 index 00000000..26ae8548 --- /dev/null +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java @@ -0,0 +1,627 @@ +package org.firstinspires.ftc.learnbot.component; + +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.technototes.library.command.Command; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; +import com.technototes.library.util.PIDFController; +import java.util.Locale; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; + +@Configurable +public class LauncherComponent implements Loggable, Subsystem { + + // This is a little strange: It's a place to tuck away a reference to the Launcher Subsystem, + // so that all the commands can get to it there. + // It let's us do this: + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor()); + // Instead of this: + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor(r.launcherComponent)); + private static LauncherComponent self = null; + + // *ALL* the configuration should go in here. I moved some things that had been constants up + // to here, as they are "bot build configuration": are the motors reversed. + // TODO: Make "are there 1 or 2 motors" a configuration setting + public static class Config { + + // Is the primary intake motor reversed? + public static boolean PrimaryReversed = true; + // Is the secondary intake motor reversed? + public static boolean SecondaryReversed = false; + + // @Log.Number(name = "Motor Power") + // public static double MOTOR_POWER = 0.65; // 0.5 1.0 + public static double CloseTargetLaunchVelocity = 1400; + public static double FarTargetLaunchVelocity = 1850; + public static double FarTargetLaunchVelocityForAuto = 2300; + public static double TargetLaunchVelocityForAuto1 = 1950; + public static double TargetLaunchVelocityForAuto2 = 1850; + + // TODO: Document this better + public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static double Near_P = 0.004; + public static double Near_I = 0.0002; + public static double Far_P = 0.004; + public static double Far_I = 0.0002; + public static double SPIN_F_SCALE = 0.00021; + public static double SPIN_VOLT_COMP = 0.0216; + public static double DIFFERENCE = 0.0046; + public static double PEAK_VOLTAGE = 13; + + // TODO: Make this more configurable. Maybe just turn it into a little helper function that + // lives in this class? + public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula + public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula + public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 + public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula + public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula + } + + // *All* commands for the subsystem belong in here. It's easy for the simple "call a method" + // commands, but for more complicated commands, scroll down to see AutoVelocity/AutoVelocityImpl + public static class LauncherCommand { + + public static Command Launch() { + return Command.create(self::Launch); + } + + public static Command SetFarShoot() { + return Command.create(self::FarShoot); + } + + public static Command SetCloseShoot() { + return Command.create(self::CloseShoot); + } + + public static Command AutoLaunch1() { + return Command.create(self::AutoLaunch1); + } + + public static Command AutoLaunch2() { + return Command.create(self::AutoLaunch2); + } + + public static Command FarAutoLaunch() { + return Command.create(self::FarAutoLaunch); + } + + public static Command StopLaunch() { + return Command.create(self::Stop); + } + + public static Command IncreaseMotor() { + return Command.create(self::IncreaseMotorVelocity); + } + + public static Command DecreaseMotor() { + return Command.create(self::DecreaseMotorVelocity); + } + + public static Command ReadVelocity() { + return Command.create(self::readVelocity); + } + + public static Command SetRegressionAuto() { + return Command.create(self::setRegressionAuto); + } + + public static Command SetRegressionTeleop() { + return Command.create(self::setRegressionTeleop); + } + + public static Command IncreaseRegressionDTeleop() { + return Command.create(self::increaseRegressionDTeleop); + } + + // This is just to make all commands look the same to the 'outside' user: + // You just call LauncherCommands.AutoVelocity() instead of needing to differentiate + // between simple Command.create's and more complex "class" commands. + public static Command AutoVelocity() { + return new AutoVelocityImpl(); + } + + // This class is protected to ensure that elsewhere, you can't ever use + // button.whenPressed(new AutoVelocityImpl()); + // but instead you have to use + // button.whenPressed(LaunchCommand.AutoVelocity()); + protected static class AutoVelocityImpl implements Command { + + public AutoVelocityImpl() {} + + // This command is designed to *never* finish. + // It should be run in a parallel command group/alongWith/raceWith group. + @Override + public boolean isFinished() { + return false; + } + + @Override + public void execute() { + LauncherComponent.self.Launch(); + } + } + } + + @Log.Number(name = "Target Velocity") + public static double targetLaunchVelocity = 1150; + + @Log.Number(name = "Current Motor Velocity") + public static double currentLaunchVelocity = 0.0; + + public static double motorVelocity; + public static double additionAmount; + public static double additionDelta = 5; + + //@Log(name = "Launcher Power: ") + public static double power; + + //@Log(name = "Error") + public static double err; + public static double launcher1Current; + public static double launcher2Current; + + @Log(name = "Target Speed: ") + public static double targetSpeed; + + @Log(name = "Target Power: ") + public static double targetPower; + + private static PIDFController launcherPID; + public static double lastAutoVelocity = 0; + + @Log.Number(name = "AutoAim Velocity") + public static double autoVelocity; + + // External dependencies this component requires: + EncodedMotor launcher1; + EncodedMotor launcher2; + TargetAcquisition targetAcquisition; + DoubleSupplier voltage; + + public LauncherComponent( + EncodedMotor primary, + EncodedMotor secondary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + self = this; + launcher1 = primary; + if (hasLaunch1()) { + launcher1.setDirection( + Config.PrimaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD + ); + launcher1.coast(); + } + launcher2 = secondary; + if (hasLaunch2()) { + launcher2.setDirection( + Config.SecondaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD + ); + launcher2.coast(); + } + targetAcquisition = targetSubsystem; + voltage = voltageSup != null ? voltageSup : () -> Config.PEAK_VOLTAGE; + // All the Feedfwd stuff below seems...confused. + // Notes: Addition will *increase* as voltage decreases (I think this is expected) + // but it likely won't ever be zero, and it could (with a new battery) be *negative* + // I've tried to model it here: https://www.desmos.com/calculator/gmzlhkyz5a + + // (I is the initial voltage delta (ADDITION), and V is the voltage as the bot is runs. + // Don't animate I, just V, and you'll see that as voltage decreases, so does the output + // power. Drag I around to change the initial voltage delta) + + // The thing that saves us is I, which does what I believe we're expecting: It increase + // power as initial voltage is lower. But then the FF function drops it. + + // My suggestion: rip out all the 'initial' stuff and make the core F function return higher + // values as voltage *decreases*, which is I think what we're really looking for anyway. + + double ADDITION = (Config.PEAK_VOLTAGE - voltage.getAsDouble()); + if (ADDITION == 0) { + // I'd be stunned if this code runs...ever. Not sure what's supposed to happen here. + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; + } else { + // This seems like a sensible thing: We're adding some amount of voltage delta to + // compensate for a lower initial voltage + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); + } + launcherPID = new PIDFController(Config.launcherPI, target -> + target == 0 + ? 0 + : (Config.SPIN_F_SCALE * target) + + // This is weird: We're going to *reduce* the output power slightly as voltage + // decreases over time. I don't think this is what we're trying to accomplish. + (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) + ); + // A quick wander around google comes up with something like this: + + // launcherMyPID = new PIDFController(Config.launcherPID, target -> + // (Config.kStaticFriction + Config.kVelocityConstant * target) / voltage.getAsDouble()); + + // The point is that motor RPM scales linearly with voltage, so to compensate, you should + // divide by voltage: Don't try to scale something by a delta from peak. Just divide. + + // To get solve that formula, get a fresh battery, run it at full power and measure the RPM. + // (Well, and figure out kStaticFriction, too: The lowest value that will still get the + // launcher barely moving) + + // These capabilities should probably go in the "validation" function at the bottom :D + + setTargetSpeed(0); + CommandScheduler.register(this); + } + + public LauncherComponent() { + this(null, null, null, () -> 0); + } + + public LauncherComponent( + EncodedMotor primary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + this(primary, null, targetSubsystem, voltageSup); + } + + public void Launch() { + // Spin the motors pid goes here + setTargetSpeed(autoVelocity()); //change to auto aim velocity + } + + public void CloseShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.CloseTargetLaunchVelocity; + } + + public void FarShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.FarTargetLaunchVelocity; + } + + public void AutoLaunch1() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto1); //change to auto aim velocity + } + + public void AutoLaunch2() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto2); //change to auto aim velocity + } + + public void FarAutoLaunch() { + // Spin the motors pid goes here + setTargetSpeed(Config.FarTargetLaunchVelocityForAuto); //change to auto aim velocity + } + + public double readVelocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Nan = "Not a Number" + } + } + + public void setTargetSpeed(double speed) { + targetSpeed = speed; + launcherPID.setTarget(speed); + } + + public double getTargetSpeed() { + return targetSpeed; + } + + private void setMotorPower(double pow) { + double power = Math.clamp(pow, -1, 1); + targetPower = power; + if (hasLaunch1()) { + launcher1.setPower(power); + } + if (hasLaunch2()) { + launcher2.setPower(power); + } + } + + public double getMotorSpeed() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } + return -1; + } + + public double getMotor1Current() { + if (hasLaunch1()) { + return launcher1.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public double getMotor2Current() { + if (hasLaunch2()) { + return launcher2.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public void Stop() { + launcherPID.setTarget(0); + } + + public void IncreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount += additionDelta; + } + + public void DecreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount -= additionDelta; + } + + public double getMotor1Velocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + public double getMotor2Velocity() { + if (hasLaunch2()) { + return launcher2.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + // This is wrong (and, as it turns out, unused), so I just commented it out + // TODO: We should discuss how to accomplish what this is attempting to do + /* + public void VelocityShoot() { + if ( + getMotor1Velocity() == targetLaunchVelocity && + getMotor2Velocity() == targetLaunchVelocity + ) { + // This doesn't do anything. It *creates* a command, but the command is never + // scheduled, so it won't ever actually do anything. + TeleCommands.GateDown(robot); + } + } + */ + + // TOOD: Turn this into a quadratic equation instead, since that's what it's mimicking. + public double autoVelocity() { + // x = distance in inches + double x = getTargetDistance(); + + if (x < 0) { + return lastAutoVelocity; + } else if (x < 100) { + // launcherPI.p = 0.0015; + // launcherPI.i = 0; + lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; + return lastAutoVelocity; + } else { + // NOTE: These two lines don't appear to do anything, because we're not using the + // Near_P and Near_I values anywhere (they were commented out above) + // TODO: Assigning values to something in Config is 'bad form'. Instead, we should + // have the coefficients copied into the Launcher itself in the constructor, and change + // that here. + Config.launcherPI.p = Config.Far_P; + Config.launcherPI.i = Config.Far_I; + return Config.REGRESSION_C * x + Config.REGRESSION_D; + } + + //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; + } + + public void setRegressionAuto() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_AUTO; + Config.REGRESSION_D = Config.REGRESSION_D_AUTO; + } + + public void setRegressionTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_TELEOP; + Config.REGRESSION_D = Config.REGRESSION_D_TELEOP; + } + + public void increaseRegressionDTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_D += 15; + } + + // This lets the 'no hardware' or 'subsystem disabled' thing still work without a functional + // target acquisition subsystem + private double getTargetDistance() { + if (targetAcquisition != null) { + return targetAcquisition.getDistance(); + } + return -1; + } + + @Override + public void periodic() { + autoVelocity = autoVelocity(); + currentLaunchVelocity = readVelocity(); + if (launcherPID.getTarget() != 0) { + setMotorPower(launcherPID.update(getMotorSpeed())); + } else { + setMotorPower(0); + launcherPID.update(getMotorSpeed()); + launcherPID.reset(); + } + + err = launcherPID.getLastError(); + motorVelocity = getMotorSpeed(); + if (hasLaunch1()) { + power = launcher1.getPower(); + } else { + power = Double.NaN; + } + launcher1Current = getMotor1Current(); + launcher2Current = getMotor2Current(); + } + + // This should be used by a test opmode to check that the basics are working. + public String hardwareValidation(double power1, double power2) { + String res = ""; + if (hasLaunch1()) { + launcher1.setPower(power1); + } else { + res += "(no launcher1)"; + } + if (hasLaunch2()) { + launcher2.setPower(power2); + } else { + res += "(no launcher2)"; + } + res += String.format( + Locale.ENGLISH, + "Launcher Speed: %.2f, Current1: %.2f, Current2: %.2f", + getMotorSpeed(), + getMotor1Current(), + getMotor2Current() + ); + return res; + } + + // TODO: Code that measures kStaticFriction and kVelocityConstant for a FeedFwd function + public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opModeIsActive) { + // Wait until a button's been pressed, then first identify kStaticFriction, by watching to + // see when the motor just barely starts moving (then back off by a couple percent) + motorHelperPower(0); + while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addLine("Please press a button on the gamepad to begin test"); + tel.update(); + } + while (anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addLine("Release the button to begin the test"); + tel.update(); + } + // Okay, slowly raise the power applied to launcher1 until w + double staticFriction = 0.001; + double staticFrictionStep = 0.001; + ElapsedTime lastUpdate = new ElapsedTime(); + while (opModeIsActive.getAsBoolean()) { + double v = voltage.getAsDouble(); + motorHelperPower(staticFriction / v); + tel.addLine("Press a button to abort"); + tel.addData("kStaticFriction", staticFriction); + tel.addData("Voltage", v); + tel.update(); + if (anyButtonsPressed(gamepad)) { + motorHelperPower(0); + return; + } + if (lastUpdate.milliseconds() >= 50) { + // We update every 25 milliseconds, just to give it time to trigger + if (getMotor1Velocity() != 0) { + break; + } + lastUpdate.reset(); + staticFriction += staticFrictionStep; + } + } + // If we're here, the system started moving. Stop the motors and set kStaticFriction to + // just below what was necessary to start the system moving. + staticFriction -= staticFrictionStep; + motorHelperPower(0); + // Display the kStaticFriction value we got: + while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addData("kStaticFriction-->>", staticFriction); + tel.addLine("Press a button to start the velocity measurement"); + tel.update(); + } + // Next up: Velocity! + while (anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Release the button to start the velocity measurement"); + tel.update(); + } + lastUpdate.reset(); + double velocityConstant = 0; + MovingStatistics velocityConstantStats = new MovingStatistics(50); + while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + motorHelperPower(1); + if (lastUpdate.milliseconds() >= 50) { + double vel = getMotor1Velocity(); + double vol = voltage.getAsDouble(); + if (vel != 0) { + // power = (kStaticFriction + kVelocity * RPM) / v + // So + // 1 = (kSF + kV * RPM) / v; + // Multiply both sides by v + // v = kSF + kV * RPM + // move kStaticFriction over: + // v - KSF = kV * RPM + // swap sides + // kv * RPM = v - kSF + // and divide both sides by RPM + // kv = (v - kSF) / RPM + velocityConstant = (vol - staticFriction) / vel; + velocityConstantStats.add(velocityConstant); + } + } + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Press a button to stop velocity measurement"); + tel.addData("kVelocityConstant", velocityConstant); + tel.addData("Average kV", velocityConstantStats.getMean()); + tel.update(); + } + } + + private static boolean anyButtonsPressed(Gamepad g) { + return ( + g.a || + g.b || + g.x || + g.y || + g.dpad_down || + g.dpad_up || + g.dpad_left || + g.dpad_right || + g.options || + g.share || + g.left_bumper || + g.right_bumper || + g.start + ); + } + + private void motorHelperPower(double p) { + if (hasLaunch1()) { + launcher1.setPower(p); + } + if (hasLaunch2()) { + launcher2.setPower(p); + } + } + + private boolean hasLaunch1() { + return launcher1 != null; + } + + private boolean hasLaunch2() { + return launcher2 != null; + } +} diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java new file mode 100644 index 00000000..a209dcd4 --- /dev/null +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.learnbot.opmodes; + +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.technototes.library.hardware.motor.EncodedMotor; +import java.util.List; +import org.firstinspires.ftc.learnbot.component.LauncherComponent; +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +@Configurable +@SuppressWarnings("unused") +@TeleOp(name = "Launcher FeedFwd Calc") +public class LaucherFeedFwd extends LinearOpMode { + + public static String MotorName = "fl"; + + @Override + public void runOpMode() throws InterruptedException { + DcMotorEx m = this.hardwareMap.get(DcMotorEx.class, MotorName); + List hubs = hardwareMap.getAll(LynxModule.class); + LauncherComponent lc = new LauncherComponent(new EncodedMotor<>(m, MotorName), null, () -> { + double volt = 0; + double count = 0; + for (LynxModule lm : hubs) { + count += 1; + volt += lm.getInputVoltage(VoltageUnit.VOLTS); + } + return volt / count; + }); + waitForStart(); + lc.feedFwdHelper(telemetry, gamepad1, this::opModeIsActive); + } +} diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java new file mode 100644 index 00000000..f31e6ec1 --- /dev/null +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.learnbot.opmodes; + +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.technototes.library.hardware.motor.EncodedMotor; +import java.util.List; +import org.firstinspires.ftc.learnbot.component.LauncherComponent; +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +@Configurable +@SuppressWarnings("unused") +@TeleOp(name = "Launcher Cfg") +public class LauncherValidator extends LinearOpMode { + + public static String MotorName = "rr"; + + @Override + public void runOpMode() throws InterruptedException { + DcMotorEx m = this.hardwareMap.get(DcMotorEx.class, MotorName); + List hubs = hardwareMap.getAll(LynxModule.class); + LauncherComponent lc = new LauncherComponent(new EncodedMotor<>(m, MotorName), null, () -> { + double volt = 0; + double count = 0; + for (LynxModule lm : hubs) { + count += 1; + volt += lm.getInputVoltage(VoltageUnit.VOLTS); + } + return volt / count; + }); + telemetry.addLine("Press dpad for validation, buttons for feedwd"); + telemetry.update(); + waitForStart(); + while (true) { + if ( + gamepad1.dpadUpWasReleased() || + gamepad1.dpadDownWasReleased() || + gamepad1.dpadLeftWasReleased() || + gamepad1.dpadRightWasReleased() + ) { + lc.feedFwdHelper(telemetry, gamepad1, this::opModeIsActive); + break; + } else if ( + gamepad1.aWasReleased() || + gamepad1.bWasReleased() || + gamepad1.xWasReleased() || + gamepad1.yWasReleased() + ) { + while (true) { + telemetry.addLine( + "Press left trigger for Launcher1, right trigger for Launcher2, dpad (any) to stop" + ); + if (gamepad1.dpad_up || gamepad1.dpad_down) { + break; + } else { + telemetry.addLine( + lc.hardwareValidation(gamepad1.left_trigger, gamepad1.right_trigger) + ); + } + telemetry.update(); + } + } + } + } +} From 97fa7ab2c0fd986cb22ae75183c6ae244e554470 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 18:27:07 -0800 Subject: [PATCH 7/9] LaunchComponent auto FeedForward opmode actually seems to be working --- .../learnbot/component/LauncherComponent.java | 111 ++++++++++-------- .../ftc/learnbot/opmodes/LaucherFeedFwd.java | 36 ------ .../learnbot/opmodes/LauncherValidator.java | 2 +- 3 files changed, 62 insertions(+), 87 deletions(-) delete mode 100644 LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java index 26ae8548..8d9c61ed 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java @@ -505,19 +505,11 @@ public String hardwareValidation(double power1, double power2) { return res; } - // TODO: Code that measures kStaticFriction and kVelocityConstant for a FeedFwd function + // Code that measures kStaticFriction and kVelocityConstant for the FeedFwd function public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opModeIsActive) { // Wait until a button's been pressed, then first identify kStaticFriction, by watching to // see when the motor just barely starts moving (then back off by a couple percent) motorHelperPower(0); - while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { - tel.addLine("Please press a button on the gamepad to begin test"); - tel.update(); - } - while (anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { - tel.addLine("Release the button to begin the test"); - tel.update(); - } // Okay, slowly raise the power applied to launcher1 until w double staticFriction = 0.001; double staticFrictionStep = 0.001; @@ -525,20 +517,21 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode while (opModeIsActive.getAsBoolean()) { double v = voltage.getAsDouble(); motorHelperPower(staticFriction / v); - tel.addLine("Press a button to abort"); + tel.addLine("Press a button to abort (and just be patient)"); tel.addData("kStaticFriction", staticFriction); tel.addData("Voltage", v); + tel.addData("Power", staticFriction / v); tel.update(); - if (anyButtonsPressed(gamepad)) { + if (anyButtonsReleased(gamepad)) { motorHelperPower(0); return; } - if (lastUpdate.milliseconds() >= 50) { - // We update every 25 milliseconds, just to give it time to trigger + if (lastUpdate.milliseconds() >= 75) { + lastUpdate.reset(); + // We update every 75 milliseconds, just to give it time to trigger if (getMotor1Velocity() != 0) { break; } - lastUpdate.reset(); staticFriction += staticFrictionStep; } } @@ -547,37 +540,28 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode staticFriction -= staticFrictionStep; motorHelperPower(0); // Display the kStaticFriction value we got: - while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { - tel.addData("kStaticFriction-->>", staticFriction); - tel.addLine("Press a button to start the velocity measurement"); - tel.update(); - } // Next up: Velocity! - while (anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { - tel.addData("kStaticFriction!", staticFriction); - tel.addLine("Release the button to start the velocity measurement"); + while (!anyButtonsReleased(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addData("kStaticFriction-->>", staticFriction); + tel.addLine("Hit a button to start the velocity measurement"); tel.update(); } lastUpdate.reset(); double velocityConstant = 0; MovingStatistics velocityConstantStats = new MovingStatistics(50); - while (!anyButtonsPressed(gamepad) && opModeIsActive.getAsBoolean()) { + double vel = 0; + double vol = 0; + while (!anyButtonsReleased(gamepad) && opModeIsActive.getAsBoolean()) { motorHelperPower(1); if (lastUpdate.milliseconds() >= 50) { - double vel = getMotor1Velocity(); - double vol = voltage.getAsDouble(); + vel = getMotor1Velocity(); + vol = voltage.getAsDouble(); if (vel != 0) { - // power = (kStaticFriction + kVelocity * RPM) / v + // power = (kStaticFriction + kVelocityConstant * RPM) / v // So // 1 = (kSF + kV * RPM) / v; - // Multiply both sides by v - // v = kSF + kV * RPM - // move kStaticFriction over: - // v - KSF = kV * RPM - // swap sides - // kv * RPM = v - kSF - // and divide both sides by RPM - // kv = (v - kSF) / RPM + // solve for kV: + // kV = (v - kSF) / RPM velocityConstant = (vol - staticFriction) / vel; velocityConstantStats.add(velocityConstant); } @@ -586,26 +570,53 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode tel.addLine("Press a button to stop velocity measurement"); tel.addData("kVelocityConstant", velocityConstant); tel.addData("Average kV", velocityConstantStats.getMean()); + tel.addData("Velocity", vel); + tel.addData("Voltage", vol); tel.update(); } + MovingStatistics error = new MovingStatistics(1000); + velocityConstant = velocityConstantStats.getMean(); + error.add(0); + double targetVelocity = vel * 0.5; + lastUpdate.reset(); + vel = 0; + while (!anyButtonsReleased(gamepad) && opModeIsActive.getAsBoolean()) { + tel.addData("Measured kStaticFriction", staticFriction); + tel.addData("Measured kVelocityConstant", velocityConstant); + tel.addLine("Press a button to stop, dpad left/right to change target"); + tel.addData("target", targetVelocity); + tel.addData("measured", vel); + tel.addData("Average Error", error.getMean()); + tel.addData("stddev", error.getStandardDeviation()); + tel.update(); + double pow = + (Math.copySign(staticFriction, targetVelocity) + + velocityConstant * targetVelocity) / + voltage.getAsDouble(); + motorHelperPower(targetVelocity == 0 ? 0 : pow); + if (lastUpdate.milliseconds() > 250) { + vel = getMotor1Velocity(); + error.add(targetVelocity - vel); + lastUpdate.reset(); + } + if (gamepad.dpadLeftWasPressed()) { + targetVelocity -= 100; + lastUpdate.reset(); + } else if (gamepad.dpadRightWasPressed()) { + targetVelocity += 100; + lastUpdate.reset(); + } else if (gamepad.dpadUpWasPressed()) { + targetVelocity += 10; + lastUpdate.reset(); + } else if (gamepad.dpadDownWasPressed()) { + targetVelocity -= 10; + lastUpdate.reset(); + } + } } - private static boolean anyButtonsPressed(Gamepad g) { - return ( - g.a || - g.b || - g.x || - g.y || - g.dpad_down || - g.dpad_up || - g.dpad_left || - g.dpad_right || - g.options || - g.share || - g.left_bumper || - g.right_bumper || - g.start - ); + private static boolean anyButtonsReleased(Gamepad g) { + return g.aWasReleased() || g.bWasReleased() || g.xWasReleased() || g.yWasReleased(); } private void motorHelperPower(double p) { diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java deleted file mode 100644 index a209dcd4..00000000 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LaucherFeedFwd.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.firstinspires.ftc.learnbot.opmodes; - -import com.bylazar.configurables.annotations.Configurable; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.technototes.library.hardware.motor.EncodedMotor; -import java.util.List; -import org.firstinspires.ftc.learnbot.component.LauncherComponent; -import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; - -@Configurable -@SuppressWarnings("unused") -@TeleOp(name = "Launcher FeedFwd Calc") -public class LaucherFeedFwd extends LinearOpMode { - - public static String MotorName = "fl"; - - @Override - public void runOpMode() throws InterruptedException { - DcMotorEx m = this.hardwareMap.get(DcMotorEx.class, MotorName); - List hubs = hardwareMap.getAll(LynxModule.class); - LauncherComponent lc = new LauncherComponent(new EncodedMotor<>(m, MotorName), null, () -> { - double volt = 0; - double count = 0; - for (LynxModule lm : hubs) { - count += 1; - volt += lm.getInputVoltage(VoltageUnit.VOLTS); - } - return volt / count; - }); - waitForStart(); - lc.feedFwdHelper(telemetry, gamepad1, this::opModeIsActive); - } -} diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java index f31e6ec1..6e517b6e 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java @@ -30,7 +30,7 @@ public void runOpMode() throws InterruptedException { } return volt / count; }); - telemetry.addLine("Press dpad for validation, buttons for feedwd"); + telemetry.addLine("Press dpad for feedfwd, buttons for validation"); telemetry.update(); waitForStart(); while (true) { From 524d9fd0940b487825dc915fbdf625382b8a2d0c Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sat, 24 Jan 2026 22:42:14 -0800 Subject: [PATCH 8/9] Updated/trimmed LauncherComponent in Mouse --- .../learnbot/component/LauncherComponent.java | 110 ++++-------------- 1 file changed, 25 insertions(+), 85 deletions(-) diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java index 8d9c61ed..405b773f 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java @@ -2,7 +2,7 @@ import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.util.ElapsedTime; @@ -42,24 +42,18 @@ public static class Config { // Is the secondary intake motor reversed? public static boolean SecondaryReversed = false; - // @Log.Number(name = "Motor Power") - // public static double MOTOR_POWER = 0.65; // 0.5 1.0 public static double CloseTargetLaunchVelocity = 1400; public static double FarTargetLaunchVelocity = 1850; public static double FarTargetLaunchVelocityForAuto = 2300; public static double TargetLaunchVelocityForAuto1 = 1950; public static double TargetLaunchVelocityForAuto2 = 1850; + public static double additionDelta = 10; // TODO: Document this better - public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); - public static double Near_P = 0.004; - public static double Near_I = 0.0002; - public static double Far_P = 0.004; - public static double Far_I = 0.0002; - public static double SPIN_F_SCALE = 0.00021; - public static double SPIN_VOLT_COMP = 0.0216; - public static double DIFFERENCE = 0.0046; - public static double PEAK_VOLTAGE = 13; + public static PIDFCoefficients launchPID = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static double kStaticFriction = 0.415; + public static double kVelocityConstant = 0.00044; + public static double PeakVoltage = 13.6; // TODO: Make this more configurable. Maybe just turn it into a little helper function that // lives in this class? @@ -166,13 +160,7 @@ public void execute() { public static double motorVelocity; public static double additionAmount; - public static double additionDelta = 5; - //@Log(name = "Launcher Power: ") - public static double power; - - //@Log(name = "Error") - public static double err; public static double launcher1Current; public static double launcher2Current; @@ -203,57 +191,20 @@ public LauncherComponent( self = this; launcher1 = primary; if (hasLaunch1()) { - launcher1.setDirection( - Config.PrimaryReversed - ? DcMotorSimple.Direction.REVERSE - : DcMotorSimple.Direction.FORWARD - ); + launcher1.setDirection(Config.PrimaryReversed ? Direction.REVERSE : Direction.FORWARD); launcher1.coast(); } launcher2 = secondary; if (hasLaunch2()) { launcher2.setDirection( - Config.SecondaryReversed - ? DcMotorSimple.Direction.REVERSE - : DcMotorSimple.Direction.FORWARD + Config.SecondaryReversed ? Direction.REVERSE : Direction.FORWARD ); launcher2.coast(); } targetAcquisition = targetSubsystem; - voltage = voltageSup != null ? voltageSup : () -> Config.PEAK_VOLTAGE; - // All the Feedfwd stuff below seems...confused. - // Notes: Addition will *increase* as voltage decreases (I think this is expected) - // but it likely won't ever be zero, and it could (with a new battery) be *negative* - // I've tried to model it here: https://www.desmos.com/calculator/gmzlhkyz5a - - // (I is the initial voltage delta (ADDITION), and V is the voltage as the bot is runs. - // Don't animate I, just V, and you'll see that as voltage decreases, so does the output - // power. Drag I around to change the initial voltage delta) - - // The thing that saves us is I, which does what I believe we're expecting: It increase - // power as initial voltage is lower. But then the FF function drops it. - - // My suggestion: rip out all the 'initial' stuff and make the core F function return higher - // values as voltage *decreases*, which is I think what we're really looking for anyway. - - double ADDITION = (Config.PEAK_VOLTAGE - voltage.getAsDouble()); - if (ADDITION == 0) { - // I'd be stunned if this code runs...ever. Not sure what's supposed to happen here. - Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; - } else { - // This seems like a sensible thing: We're adding some amount of voltage delta to - // compensate for a lower initial voltage - Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); - } - launcherPID = new PIDFController(Config.launcherPI, target -> - target == 0 - ? 0 - : (Config.SPIN_F_SCALE * target) + - // This is weird: We're going to *reduce* the output power slightly as voltage - // decreases over time. I don't think this is what we're trying to accomplish. - (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) - ); - // A quick wander around google comes up with something like this: + voltage = voltageSup != null ? voltageSup : () -> Config.PeakVoltage; + + // A quick wander around google comes up with something like this for motor feedfwd: // launcherMyPID = new PIDFController(Config.launcherPID, target -> // (Config.kStaticFriction + Config.kVelocityConstant * target) / voltage.getAsDouble()); @@ -264,8 +215,13 @@ public LauncherComponent( // To get solve that formula, get a fresh battery, run it at full power and measure the RPM. // (Well, and figure out kStaticFriction, too: The lowest value that will still get the // launcher barely moving) - - // These capabilities should probably go in the "validation" function at the bottom :D + launcherPID = new PIDFController(Config.launchPID, target -> + target == 0 + ? 0 + : (Math.copySign(Config.kStaticFriction, target) + + Config.kVelocityConstant * target) / + voltage.getAsDouble() + ); setTargetSpeed(0); CommandScheduler.register(this); @@ -368,12 +324,12 @@ public void Stop() { public void IncreaseMotorVelocity() { // Spin the motors pid goes here - additionAmount += additionDelta; + additionAmount += Config.additionDelta; } public void DecreaseMotorVelocity() { // Spin the motors pid goes here - additionAmount -= additionDelta; + additionAmount -= Config.additionDelta; } public double getMotor1Velocity() { @@ -412,25 +368,15 @@ public double autoVelocity() { // x = distance in inches double x = getTargetDistance(); + // TODO: Clean this up, make it a formula if (x < 0) { return lastAutoVelocity; } else if (x < 100) { - // launcherPI.p = 0.0015; - // launcherPI.i = 0; lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; return lastAutoVelocity; } else { - // NOTE: These two lines don't appear to do anything, because we're not using the - // Near_P and Near_I values anywhere (they were commented out above) - // TODO: Assigning values to something in Config is 'bad form'. Instead, we should - // have the coefficients copied into the Launcher itself in the constructor, and change - // that here. - Config.launcherPI.p = Config.Far_P; - Config.launcherPI.i = Config.Far_I; return Config.REGRESSION_C * x + Config.REGRESSION_D; } - - //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; } public void setRegressionAuto() { @@ -463,21 +409,15 @@ private double getTargetDistance() { public void periodic() { autoVelocity = autoVelocity(); currentLaunchVelocity = readVelocity(); + motorVelocity = getMotorSpeed(); if (launcherPID.getTarget() != 0) { - setMotorPower(launcherPID.update(getMotorSpeed())); + setMotorPower(launcherPID.update(motorVelocity) + additionAmount); } else { setMotorPower(0); - launcherPID.update(getMotorSpeed()); + // When we want to stop, reset the PID controller + launcherPID.update(motorVelocity); launcherPID.reset(); } - - err = launcherPID.getLastError(); - motorVelocity = getMotorSpeed(); - if (hasLaunch1()) { - power = launcher1.getPower(); - } else { - power = Double.NaN; - } launcher1Current = getMotor1Current(); launcher2Current = getMotor2Current(); } From 97bc4e8a5099d87ae1b2fd702bb54495eda43718 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Sun, 25 Jan 2026 14:27:41 -0800 Subject: [PATCH 9/9] Untested additions to the whole power constants automatic thing --- .../learnbot/component/LauncherComponent.java | 43 +++++++---- .../learnbot/opmodes/LauncherValidator.java | 73 +++++++++++-------- 2 files changed, 68 insertions(+), 48 deletions(-) diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java index 405b773f..3038fa44 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/LauncherComponent.java @@ -54,6 +54,11 @@ public static class Config { public static double kStaticFriction = 0.415; public static double kVelocityConstant = 0.00044; public static double PeakVoltage = 13.6; + // GoBilda says stall current of 9.2A at 12V, so V = I * R, R = 12 / 9.2 (about 1.3) + // As the motor heats up, resistance also increase, so we could increase this a little bit + // or maybe increase it over time to counteract that, but this is probably good enough for + // now + public static double MotorResistance = 9.2 / 12; // TODO: Make this more configurable. Maybe just turn it into a little helper function that // lives in this class? @@ -219,7 +224,8 @@ public LauncherComponent( target == 0 ? 0 : (Math.copySign(Config.kStaticFriction, target) + - Config.kVelocityConstant * target) / + Config.kVelocityConstant * target + + getMotor1Current() * Config.MotorResistance) / voltage.getAsDouble() ); @@ -428,16 +434,16 @@ public String hardwareValidation(double power1, double power2) { if (hasLaunch1()) { launcher1.setPower(power1); } else { - res += "(no launcher1)"; + res += "(no launcher1) "; } if (hasLaunch2()) { launcher2.setPower(power2); } else { - res += "(no launcher2)"; + res += "(no launcher2) "; } res += String.format( Locale.ENGLISH, - "Launcher Speed: %.2f, Current1: %.2f, Current2: %.2f", + "Speed: %.2f, Current1: %.2f, Current2: %.2f", getMotorSpeed(), getMotor1Current(), getMotor2Current() @@ -456,19 +462,21 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode ElapsedTime lastUpdate = new ElapsedTime(); while (opModeIsActive.getAsBoolean()) { double v = voltage.getAsDouble(); - motorHelperPower(staticFriction / v); + double amps = getMotor1Current(); + double power = (staticFriction + amps * Config.MotorResistance) / v; + motorHelperPower(power); tel.addLine("Press a button to abort (and just be patient)"); tel.addData("kStaticFriction", staticFriction); tel.addData("Voltage", v); - tel.addData("Power", staticFriction / v); + tel.addData("Power", power); tel.update(); if (anyButtonsReleased(gamepad)) { motorHelperPower(0); return; } - if (lastUpdate.milliseconds() >= 75) { + if (lastUpdate.milliseconds() >= 100) { lastUpdate.reset(); - // We update every 75 milliseconds, just to give it time to trigger + // We update every 100 milliseconds, just to give it time to trigger the encoder if (getMotor1Velocity() != 0) { break; } @@ -491,18 +499,21 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode MovingStatistics velocityConstantStats = new MovingStatistics(50); double vel = 0; double vol = 0; + double amps = 0; while (!anyButtonsReleased(gamepad) && opModeIsActive.getAsBoolean()) { motorHelperPower(1); if (lastUpdate.milliseconds() >= 50) { + lastUpdate.reset(); vel = getMotor1Velocity(); vol = voltage.getAsDouble(); + amps = getMotor1Current(); if (vel != 0) { // power = (kStaticFriction + kVelocityConstant * RPM) / v // So - // 1 = (kSF + kV * RPM) / v; + // 1 = (kSF + kV * RPM + motorAmperage * motorResistance) / v; // solve for kV: - // kV = (v - kSF) / RPM - velocityConstant = (vol - staticFriction) / vel; + // kV = (v - kSF - motorAmperage * motorResistance) / RPM + velocityConstant = (vol - staticFriction - amps * Config.MotorResistance) / vel; velocityConstantStats.add(velocityConstant); } } @@ -535,22 +546,22 @@ public void feedFwdHelper(Telemetry tel, Gamepad gamepad, BooleanSupplier opMode voltage.getAsDouble(); motorHelperPower(targetVelocity == 0 ? 0 : pow); if (lastUpdate.milliseconds() > 250) { + lastUpdate.reset(); vel = getMotor1Velocity(); error.add(targetVelocity - vel); - lastUpdate.reset(); } if (gamepad.dpadLeftWasPressed()) { - targetVelocity -= 100; lastUpdate.reset(); + targetVelocity -= 100; } else if (gamepad.dpadRightWasPressed()) { - targetVelocity += 100; lastUpdate.reset(); + targetVelocity += 100; } else if (gamepad.dpadUpWasPressed()) { - targetVelocity += 10; lastUpdate.reset(); + targetVelocity += 10; } else if (gamepad.dpadDownWasPressed()) { - targetVelocity -= 10; lastUpdate.reset(); + targetVelocity -= 10; } } } diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java index 6e517b6e..41ad6895 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/LauncherValidator.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.technototes.library.hardware.motor.EncodedMotor; import java.util.List; +import java.util.function.DoubleSupplier; import org.firstinspires.ftc.learnbot.component.LauncherComponent; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -17,48 +18,56 @@ public class LauncherValidator extends LinearOpMode { public static String MotorName = "rr"; + public boolean anyDpad() { + return ( + gamepad1.dpadUpWasReleased() || + gamepad1.dpadDownWasReleased() || + gamepad1.dpadLeftWasReleased() || + gamepad1.dpadRightWasReleased() + ); + } + + public boolean anyButtons() { + return ( + gamepad1.aWasReleased() || + gamepad1.bWasReleased() || + gamepad1.xWasReleased() || + gamepad1.yWasReleased() + ); + } + @Override public void runOpMode() throws InterruptedException { - DcMotorEx m = this.hardwareMap.get(DcMotorEx.class, MotorName); + // We need to get the voltage for List hubs = hardwareMap.getAll(LynxModule.class); - LauncherComponent lc = new LauncherComponent(new EncodedMotor<>(m, MotorName), null, () -> { - double volt = 0; - double count = 0; + DoubleSupplier getVoltage = () -> { + double volts = 0; for (LynxModule lm : hubs) { - count += 1; - volt += lm.getInputVoltage(VoltageUnit.VOLTS); + volts += lm.getInputVoltage(VoltageUnit.VOLTS); } - return volt / count; - }); - telemetry.addLine("Press dpad for feedfwd, buttons for validation"); - telemetry.update(); + return volts / hubs.size(); + }; + EncodedMotor m = new EncodedMotor<>( + hardwareMap.get(DcMotorEx.class, MotorName), + MotorName + ); + LauncherComponent lc = new LauncherComponent(m, null, getVoltage); waitForStart(); - while (true) { - if ( - gamepad1.dpadUpWasReleased() || - gamepad1.dpadDownWasReleased() || - gamepad1.dpadLeftWasReleased() || - gamepad1.dpadRightWasReleased() - ) { + telemetry.addLine(">>> Press the dpad for feedfwd <<<"); + telemetry.addLine(">>> Press any button for validation <<<"); + telemetry.update(); + while (opModeIsActive()) { + if (anyDpad()) { lc.feedFwdHelper(telemetry, gamepad1, this::opModeIsActive); break; - } else if ( - gamepad1.aWasReleased() || - gamepad1.bWasReleased() || - gamepad1.xWasReleased() || - gamepad1.yWasReleased() - ) { - while (true) { + } else if (anyButtons()) { + while (opModeIsActive() && !anyDpad()) { + telemetry.addLine(">>> Press left trigger for Launcher1 control"); + telemetry.addLine(">>> Press right trigger for Launcher2 control"); + telemetry.addLine(">>> Hit the dpad to stop"); telemetry.addLine( - "Press left trigger for Launcher1, right trigger for Launcher2, dpad (any) to stop" + lc.hardwareValidation(gamepad1.left_trigger, gamepad1.right_trigger) ); - if (gamepad1.dpad_up || gamepad1.dpad_down) { - break; - } else { - telemetry.addLine( - lc.hardwareValidation(gamepad1.left_trigger, gamepad1.right_trigger) - ); - } telemetry.update(); } }