diff --git a/.gitignore b/.gitignore index 2adcaa6..e0be891 100644 --- a/.gitignore +++ b/.gitignore @@ -87,3 +87,5 @@ lint/tmp/ # lint/reports/ # Linked git worktrees for parallel sessions .worktrees/ +# Local Android SDK install +/android-sdk/ diff --git a/README.md b/README.md index 6fd496f..2b752ba 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ Robot code for Lebob Robotics, team 29550, competing in FIRST Tech Challenge BIOBUZZ 2026/27 at the Western Australia Qualifier. This repository is an unmodified import of the FIRST Tech Challenge SDK with -our own code in `TeamCode`. +our own code in `TeamCode`. ## Requirements @@ -14,11 +14,11 @@ our own code in `TeamCode`. The required SDK packages: -| Package | Why | -| --- | --- | +| Package | Why | +| ---------------------- | ------------------------------------------------- | | `platforms;android-30` | `build.common.gradle` sets `compileSdkVersion 30` | -| `build-tools;35.0.0` | the revision AGP 8.13.2 resolves to | -| `platform-tools` | provides `adb`, used to deploy to the Control Hub | +| `build-tools;35.0.0` | the revision AGP 8.13.2 resolves to | +| `platform-tools` | provides `adb`, used to deploy to the Control Hub | ### Getting the SDK without Android Studio @@ -98,14 +98,12 @@ above restarts it; `adb reboot` also works and takes longer. A one-line version of build-and-deploy, once the connection is up: ``` -./gradlew :TeamCode:assembleDebug && \ - adb install -r TeamCode/build/outputs/apk/debug/TeamCode-debug.apk && \ - adb shell am start -n com.qualcomm.ftcrobotcontroller/org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper +./gradlew :TeamCode:assembleDebug && adb install -r TeamCode/build/outputs/apk/debug/TeamCode-debug.apk && adb shell am start -n com.qualcomm.ftcrobotcontroller/org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper ``` ### Using Android Studio instead -Android Studio download: +Android Studio download: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java index 3a9752b..d18dbf0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java @@ -5,21 +5,24 @@ @TeleOp(name = "BioBuzz TeleOp", group = "Robot") public class Main extends OpMode { - private Robot robot; + private Robot robot; - @Override - public void init() { - robot = new Robot(hardwareMap, telemetry, gamepad1); - robot.init(); - } + @Override + public void init() { + robot = new Robot(hardwareMap, telemetry, gamepad1); + robot.init(); + } - @Override - public void loop() { - robot.periodic(); - } + @Override + public void loop() { + robot.periodic(); + } - @Override - public void stop() { - robot.stop(); + @Override + public void stop() { + // robot is null if init() threw (e.g. a device name missing from the config). + if (robot != null) { + robot.stop(); } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 3b08345..ac57cba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -6,57 +6,92 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState; import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem.ShooterState; public class Robot { - private final Telemetry telemetry; - private final Gamepad driver; - - public final MecanumDriveSubsystem drive; - public final OdometrySubsystem odometry; - - public Robot( - HardwareMap hardwareMap, - Telemetry telemetry, - Gamepad driver - ) { - this.telemetry = telemetry; - this.driver = driver; - - CommandScheduler.getInstance().reset(); - - // Construct subsystems. - drive = new MecanumDriveSubsystem(hardwareMap); - odometry = new OdometrySubsystem(hardwareMap); - // intake = new IntakeSubsystem(hardwareMap); - // outtake = new OuttakeSubsystem(hardwareMap); - } + private final Telemetry telemetry; + private final Gamepad driver; - /** Called once when the OpMode enters INIT. */ - public void init() { - odometry.init(); - } + public final IntakeSubsystem intake; + public final MecanumDriveSubsystem drive; + public final OdometrySubsystem odometry; + public final ShooterSubsystem shooter; + + public static final double triggerLimit = 0.5; + + public Robot( + HardwareMap hardwareMap, + Telemetry telemetry, + Gamepad driver) { + this.telemetry = telemetry; + this.driver = driver; + + CommandScheduler.getInstance().reset(); + + // Construct subsystems. + intake = new IntakeSubsystem(hardwareMap); + drive = new MecanumDriveSubsystem(hardwareMap); + odometry = new OdometrySubsystem(hardwareMap); + shooter = new ShooterSubsystem(hardwareMap); - /** Called repeatedly while the OpMode is running. */ - public void periodic() { - CommandScheduler.getInstance().run(); + } - if (driver.a) { - odometry.resetHeading(); - } + /** Called once when the OpMode enters INIT. */ + public void init() { + odometry.init(); + shooter.setShooterState(ShooterState.IDLE); + } - // Hold left bumper to drive robot-relative; otherwise drive field-relative. - drive.drive(-driver.left_stick_y, driver.left_stick_x, driver.right_stick_x, - !driver.left_bumper, odometry.getPose().getHeading(AngleUnit.RADIANS)); + /** Called repeatedly while the OpMode is running. */ + public void periodic() { + CommandScheduler.getInstance().run(); - telemetry.addData("Pose", odometry.getPose()); - telemetry.update(); + if (driver.a) { + odometry.resetHeading(); } - /** Called once when the OpMode stops. */ - public void stop() { - CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().reset(); + if (driver.right_trigger > triggerLimit) { + shooter.setShooterState(ShooterState.SHOOT); + } else if (driver.b) { + shooter.setShooterState(ShooterState.EJECT); + } else if (driver.x) { + shooter.setShooterState(ShooterState.STOP); + } else { + shooter.setShooterState(ShooterState.IDLE); } + + // The intake also runs while shooting: the indexer can't pull in balls waiting + // where the intake meets it, so with the intake stopped they sit there. It + // pauses while the indexer backs off a jam, so it doesn't pack the jam tighter. + boolean feeding = driver.right_trigger > triggerLimit && shooter.isIndexerFeeding(); + if (driver.b) { + intake.setIntakeState(IntakeState.EJECT); + } else if (driver.left_trigger > triggerLimit || feeding) { + intake.setIntakeState(IntakeState.INTAKE); + } else { + intake.setIntakeState(IntakeState.STOP); + } + + // Hold left bumper to drive robot-relative; otherwise drive field-relative. + drive.drive(-driver.left_stick_y, driver.left_stick_x, driver.right_stick_x, + !driver.left_bumper, odometry.getPose().getHeading(AngleUnit.RADIANS)); + + telemetry.addData("Pose", odometry.getPose()); + telemetry.addData("Indexer", shooter.getIndexerStatus()); + if (shooter.isIndexerJammed()) { + telemetry.addLine("INDEXER JAMMED: release the right trigger and clear the balls"); + } + telemetry.update(); + } + + /** Called once when the OpMode stops. */ + public void stop() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().reset(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java new file mode 100644 index 0000000..bc093e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java @@ -0,0 +1,113 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; + +/** + * Thin wrapper around a {@link DcMotorEx}, shared by every subsystem that + * drives a motor (drivetrain, intake, shooter, indexer, ...). + * + * 
{@code
+ * GobildaMotor shooter = new GobildaMotor(
+ *     hardwareMap, "shooter", true, DcMotor.RunMode.RUN_USING_ENCODER, false);
+ * }
+ */ +public class GobildaMotor { + /** Power changes smaller than this are not re-sent to the hub. */ + private static final double POWER_MIN = 1e-3; + + private final String name; + private final DcMotorEx motor; + private double lastPower = Double.NaN; + + /** + * @param hardwareMap the OpMode's hardware map + * @param name the device name in the robot configuration + * @param reversed true to flip the motor's positive direction + * @param mode run mode to configure the motor with + * @param brake true to brake at zero power, false to float + */ + public GobildaMotor(HardwareMap hardwareMap, String name, boolean reversed, DcMotor.RunMode mode, boolean brake) { + this.name = name; + this.motor = hardwareMap.get(DcMotorEx.class, name); + motor.setDirection(reversed ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + motor.setMode(mode); + motor.setZeroPowerBehavior(brake ? DcMotor.ZeroPowerBehavior.BRAKE : DcMotor.ZeroPowerBehavior.FLOAT); + } + + /** Zeroes the encoder, then restores the previous run mode. */ + public GobildaMotor resetEncoder() { + DcMotor.RunMode previous = motor.getMode(); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(previous); + return this; + } + + /** + * Sets motor power, clamped to [-1, 1]. Skips the hardware write if unchanged. + */ + public void setPower(double power) { + power = Math.max(-1.0, Math.min(1.0, power)); + if (!Double.isNaN(lastPower) && Math.abs(power - lastPower) < POWER_MIN) { + return; + } + lastPower = power; + motor.setPower(power); + } + + /** + * Changes the run mode if it differs, and forgets the cached power so the + * next setPower reaches the motor in the new mode. + */ + public void setMode(DcMotor.RunMode mode) { + if (motor.getMode() == mode) { + return; + } + motor.setMode(mode); + lastPower = Double.NaN; + } + + /** Sets a target velocity in ticks/second (requires RUN_USING_ENCODER). */ + public void setVelocity(double ticksPerSecond) { + lastPower = Double.NaN; // power cache is no longer meaningful + motor.setVelocity(ticksPerSecond); + } + + public void stop() { + setPower(0.0); + } + + // ---------------------------------------------------------------- state + + public double getPower() { + return motor.getPower(); + } + + /** Encoder position in ticks. */ + public int getPosition() { + return motor.getCurrentPosition(); + } + + /** Velocity in ticks/second. */ + public double getVelocity() { + return motor.getVelocity(); + } + + /** Current draw in amps. Each call is a separate hub read. */ + public double getCurrentAmps() { + return motor.getCurrent(CurrentUnit.AMPS); + } + + public String getName() { + return name; + } + + /** Escape hatch for anything not wrapped here. */ + public DcMotorEx getRaw() { + return motor; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 4d1da42..cc5675a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -14,7 +14,7 @@ Sample opmodes exist in the FtcRobotController module. To locate these samples, find the FtcRobotController module in the "Project/Android" tab. Expand the following tree elements: - FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples +FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples ### Naming of Samples @@ -27,44 +27,44 @@ To summarize: A range of different samples classes will reside in the java/exter The class names will follow a naming convention which indicates the purpose of each class. The prefix of the name will be one of the following: -Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure - of a particular style of OpMode. These are bare bones examples. +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure +of a particular style of OpMode. These are bare bones examples. -Sensor: This is a Sample OpMode that shows how to use a specific sensor. - It is not intended to drive a functioning robot, it is simply showing the minimal code - required to read and display the sensor values. +Sensor: This is a Sample OpMode that shows how to use a specific sensor. +It is not intended to drive a functioning robot, it is simply showing the minimal code +required to read and display the sensor values. -Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. - It may be used to provide a common baseline driving OpMode, or - to demonstrate how a particular sensor or concept can be used to navigate. +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. +It may be used to provide a common baseline driving OpMode, or +to demonstrate how a particular sensor or concept can be used to navigate. -Concept: This is a sample OpMode that illustrates performing a specific function or concept. - These may be complex, but their operation should be explained clearly in the comments, - or the comments should reference an external doc, guide or tutorial. - Each OpMode should try to only demonstrate a single concept so they are easy to - locate based on their name. These OpModes may not produce a drivable robot. +Concept: This is a sample OpMode that illustrates performing a specific function or concept. +These may be complex, but their operation should be explained clearly in the comments, +or the comments should reference an external doc, guide or tutorial. +Each OpMode should try to only demonstrate a single concept so they are easy to +locate based on their name. These OpModes may not produce a drivable robot. After the prefix, other conventions will apply: -* Sensor class names are constructed as: Sensor - Company - Type -* Robot class names are constructed as: Robot - Mode - Action - OpModetype -* Concept class names are constructed as: Concept - Topic - OpModetype +- Sensor class names are constructed as: Sensor - Company - Type +- Robot class names are constructed as: Robot - Mode - Action - OpModetype +- Concept class names are constructed as: Concept - Topic - OpModetype Once you are familiar with the range of samples available, you can choose one to be the -basis for your own robot. In all cases, the desired sample(s) needs to be copied into +basis for your own robot. In all cases, the desired sample(s) needs to be copied into your TeamCode module to be used. This is done inside Android Studio directly, using the following steps: - 1) Locate the desired sample class in the Project/Android tree. +1. Locate the desired sample class in the Project/Android tree. - 2) Right click on the sample class and select "Copy" +2. Right click on the sample class and select "Copy" - 3) Expand the TeamCode/java folder +3. Expand the TeamCode/java folder - 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" +4. Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" - 5) You will be prompted for a class name for the copy. +5. You will be prompted for a class name for the copy. Choose something meaningful based on the purpose of this class. Start with a capital letter, and remember that there may be more similar classes later. @@ -80,20 +80,18 @@ Each OpMode sample class begins with several lines of code like the ones shown b ``` The name that will appear on the driver station's "opmode list" is defined by the code: - ``name="Template: Linear OpMode"`` +`name="Template: Linear OpMode"` You can change what appears between the quotes to better describe your opmode. The "group=" portion of the code can be used to help organize your list of OpModes. As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the - ``@Disabled`` annotation which has been included. +`@Disabled` annotation which has been included. This line can simply be deleted , or commented out, to make the OpMode visible. - - -## ADVANCED Multi-Team App management: Cloning the TeamCode Module +## ADVANCED Multi-Team App management: Cloning the TeamCode Module In some situations, you have multiple teams in your club and you want them to all share -a common code organization, with each being able to *see* the others code but each having +a common code organization, with each being able to _see_ the others code but each having their own team module with their own code that they maintain themselves. In this situation, you might wish to clone the TeamCode module, once for each of these teams. @@ -103,29 +101,30 @@ together with the FtcRobotController module (and the original TeamCode module). Selective Team phones can then be programmed by selecting the desired Module from the pulldown list prior to clicking to the green Run arrow. -Warning: This is not for the inexperienced Software developer. +Warning: This is not for the inexperienced Software developer. You will need to be comfortable with File manipulations and managing Android Studio Modules. These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. - + Also.. Make a full project backup before you start this :) To clone TeamCode, do the following: -Note: Some names start with "Team" and others start with "team". This is intentional. +Note: Some names start with "Team" and others start with "team". This is intentional. -1) Using your operating system file management tools, copy the whole "TeamCode" +1. Using your operating system file management tools, copy the whole "TeamCode" folder to a sibling folder with a corresponding new name, eg: "Team0417". -2) In the new Team0417 folder, delete the TeamCode.iml file. +2. In the new Team0417 folder, delete the TeamCode.iml file. -3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder - to a matching name with a lowercase 'team' eg: "team0417". +3. the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". -4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains - package="org.firstinspires.ftc.teamcode" +4. In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" to be - package="org.firstinspires.ftc.team0417" + package="org.firstinspires.ftc.team0417" + +5. Add: include ':Team0417' to the "/settings.gradle" file. + +6. Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" -5) Add: include ':Team0417' to the "/settings.gradle" file. - -6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java new file mode 100644 index 0000000..b67a75b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java @@ -0,0 +1,111 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +/** + * Stall protection for the indexer while it feeds the shooter. Pure logic with + * no hardware, so it can be unit tested. + * + *

+ * Measured on the prototype on 30 Sep: jammed on two balls, the indexer drew + * 8.4 to 11.2 A at 0 ticks/s, while heavily loaded but still moving it turned + * at 460 ticks/s or more. So high current at near-zero speed for + * {@link #STALL_TIME_S} means jammed. Each jam backs the indexer off for + * {@link #UNJAM_TIME_S} and then feeding resumes. After + * {@link #MAX_UNJAM_ATTEMPTS} back-offs in one feed, the next jam leaves the + * indexer off until the next feed starts, so a hard jam does not cook the + * motor. + */ +public class IndexerStallGuard { + public static final double STALL_CURRENT_AMPS = 7.0; + /** Ticks per second. */ + public static final double STALL_VELOCITY = 150; + public static final double STALL_TIME_S = 0.3; + public static final double UNJAM_TIME_S = 0.2; + public static final int MAX_UNJAM_ATTEMPTS = 3; + + public enum Phase { + FEEDING, UNJAMMING, JAMMED + } + + private final double feedPower; + private final double unjamPower; + + private Phase phase = Phase.FEEDING; + private int unjamAttempts; + private double phaseStartS; + private double stalledSinceS = Double.NaN; + + /** + * @param feedPower indexer power while feeding + * @param unjamPower indexer power while backing off a jam (negative) + */ + public IndexerStallGuard(double feedPower, double unjamPower) { + this.feedPower = feedPower; + this.unjamPower = unjamPower; + } + + /** Starts a new feed, clearing any jam and the attempt count. */ + public void start(double nowS) { + unjamAttempts = 0; + enter(Phase.FEEDING, nowS); + } + + /** + * @param nowS time in seconds + * @param currentAmps indexer current draw + * @param velocity indexer velocity in ticks/second + * @return the power to apply to the indexer + */ + public double update(double nowS, double currentAmps, double velocity) { + switch (phase) { + case FEEDING: + boolean stalled = currentAmps > STALL_CURRENT_AMPS && Math.abs(velocity) < STALL_VELOCITY; + if (!stalled) { + stalledSinceS = Double.NaN; + } else if (Double.isNaN(stalledSinceS)) { + stalledSinceS = nowS; + } else if (nowS - stalledSinceS >= STALL_TIME_S) { + if (unjamAttempts < MAX_UNJAM_ATTEMPTS) { + unjamAttempts++; + enter(Phase.UNJAMMING, nowS); + } else { + enter(Phase.JAMMED, nowS); + } + } + break; + case UNJAMMING: + if (nowS - phaseStartS >= UNJAM_TIME_S) { + enter(Phase.FEEDING, nowS); + } + break; + case JAMMED: + break; + } + return getPower(); + } + + private void enter(Phase newPhase, double nowS) { + phase = newPhase; + phaseStartS = nowS; + stalledSinceS = Double.NaN; + } + + /** The power the indexer should be at in the current phase. */ + public double getPower() { + switch (phase) { + case FEEDING: + return feedPower; + case UNJAMMING: + return unjamPower; + default: + return 0.0; + } + } + + public Phase getPhase() { + return phase; + } + + public int getUnjamAttempts() { + return unjamAttempts; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..948c25f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; + +/** + * Subsystem to control intake and indexer + */ +public class IntakeSubsystem extends SubsystemBase { + private final GobildaMotor intakeMotor; + + public enum IntakeState { + STOP, INTAKE, EJECT + } + + private IntakeState intakeState; + private IntakeState previousIntakeState = IntakeState.STOP; + + public IntakeSubsystem(HardwareMap hardwareMap) { + intakeMotor = new GobildaMotor(hardwareMap, "Intake", true, DcMotor.RunMode.RUN_USING_ENCODER, false); + } + + private void doIntakeState() { + if (previousIntakeState == intakeState) { + return; + } + + switch (intakeState) { + case STOP: + intakeMotor.stop(); + break; + case INTAKE: + intakeMotor.setPower(1.0); + break; + case EJECT: + intakeMotor.setPower(-1.0); + break; + } + } + + public void setIntakeState(IntakeState newIntakeState) { + previousIntakeState = intakeState; + intakeState = newIntakeState; + doIntakeState(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index 2f14fe6..edd9d74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -5,69 +5,75 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; /** - * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric - * driving using the Pinpoint's heading. + * Drivetrain subsystem for a four-motor mecanum base, with optional + * field-centric driving using the Pinpoint's heading. */ public class MecanumDriveSubsystem extends SubsystemBase { - private final DcMotor frontLeftDrive; - private final DcMotor frontRightDrive; - private final DcMotor backLeftDrive; - private final DcMotor backRightDrive; + private final GobildaMotor frontLeft; + private final GobildaMotor frontRight; + private final GobildaMotor backLeft; + private final GobildaMotor backRight; - public MecanumDriveSubsystem(HardwareMap hardwareMap) { - frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); - frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); - backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); - backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + public MecanumDriveSubsystem(HardwareMap hardwareMap) { + // Left motors are flipped so that a positive power on every motor drives + // straight. + frontLeft = createDriveMotor(hardwareMap, "DriveFL", true); + frontRight = createDriveMotor(hardwareMap, "DriveFR", false); + backLeft = createDriveMotor(hardwareMap, "DriveBL", true); + backRight = createDriveMotor(hardwareMap, "DriveBR", false); + } - // Left motors are flipped so that a positive power on every motor drives straight. - frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); - frontRightDrive.setDirection(DcMotor.Direction.FORWARD); - backLeftDrive.setDirection(DcMotor.Direction.REVERSE); - backRightDrive.setDirection(DcMotor.Direction.FORWARD); + private static GobildaMotor createDriveMotor(HardwareMap hardwareMap, String name, boolean reversed) { + return new GobildaMotor(hardwareMap, name, reversed, DcMotor.RunMode.RUN_USING_ENCODER, false); + } - frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } + /** + * Drives the robot given forward/right/rotate joystick components. + * + * @param forward forward power, [-1, 1] + * @param right strafe-right power, [-1, 1] + * @param rotate clockwise rotation power, [-1, 1] + * @param fieldCentric if true, forward/right are relative to the field + * instead of the robot + * @param headingRadians the robot's current field heading; only used when + * fieldCentric is true + */ + public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { + if (fieldCentric) { + double theta = Math.atan2(forward, right); + double r = Math.hypot(right, forward); - /** - * Drives the robot given forward/right/rotate joystick components. - * - * @param forward forward power, [-1, 1] - * @param right strafe-right power, [-1, 1] - * @param rotate clockwise rotation power, [-1, 1] - * @param fieldCentric if true, forward/right are relative to the field instead of the robot - * @param headingRadians the robot's current field heading; only used when fieldCentric is true - */ - public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { - if (fieldCentric) { - double theta = Math.atan2(forward, right); - double r = Math.hypot(right, forward); + theta = AngleUnit.normalizeRadians(theta - headingRadians); - theta = AngleUnit.normalizeRadians(theta - headingRadians); + forward = r * Math.sin(theta); + right = r * Math.cos(theta); + } - forward = r * Math.sin(theta); - right = r * Math.cos(theta); - } + double frontLeftPower = forward - right + rotate; + double frontRightPower = forward - right - rotate; + double backLeftPower = forward + right + rotate; + double backRightPower = forward + right - rotate; - double frontLeftPower = forward + right + rotate; - double frontRightPower = forward - right - rotate; - double backLeftPower = forward - right + rotate; - double backRightPower = forward + right - rotate; + // Scale all wheels down together so no power exceeds 1 and the direction + // of travel is preserved. + double maxPower = Math.max(1.0, + Math.max(Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)), + Math.max(Math.abs(backLeftPower), Math.abs(backRightPower)))); - double maxPower = 1.0; - maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); - maxPower = Math.max(maxPower, Math.abs(frontRightPower)); - maxPower = Math.max(maxPower, Math.abs(backLeftPower)); - maxPower = Math.max(maxPower, Math.abs(backRightPower)); + frontLeft.setPower(frontLeftPower / maxPower); + frontRight.setPower(frontRightPower / maxPower); + backLeft.setPower(backLeftPower / maxPower); + backRight.setPower(backRightPower / maxPower); + } - frontLeftDrive.setPower(frontLeftPower / maxPower); - frontRightDrive.setPower(frontRightPower / maxPower); - backLeftDrive.setPower(backLeftPower / maxPower); - backRightDrive.setPower(backRightPower / maxPower); - } + /** Cuts power to all four wheels. */ + public void stop() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 26f8ff5..6164537 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -13,38 +13,41 @@ * reports the robot's field pose. */ public class OdometrySubsystem extends SubsystemBase { - private final GoBildaPinpointDriver pinpoint; - - public OdometrySubsystem(HardwareMap hardwareMap) { - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); - } - - /** Called once from the OpMode's init(). Configures the Pinpoint and zeroes its pose. */ - public void init() { - pinpoint.setOffsets(0.0, -105.0, DistanceUnit.MM); - pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - pinpoint.setEncoderDirections( - GoBildaPinpointDriver.EncoderDirection.FORWARD, - GoBildaPinpointDriver.EncoderDirection.FORWARD); - - // Robot must be stationary during this call. - pinpoint.resetPosAndIMU(); - } - - @Override - public void periodic() { - pinpoint.update(); - } - - public Pose2D getPose() { - return pinpoint.getPosition(); - } - - /** Zeroes the reported heading in place, keeping the current position. */ - public void resetHeading() { - Pose2D current = pinpoint.getPosition(); - pinpoint.setPosition(new Pose2D( - DistanceUnit.MM, current.getX(DistanceUnit.MM), current.getY(DistanceUnit.MM), - AngleUnit.DEGREES, 0)); - } + private final GoBildaPinpointDriver pinpoint; + + public OdometrySubsystem(HardwareMap hardwareMap) { + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + } + + /** + * Called once from the OpMode's init(). Configures the Pinpoint and zeroes its + * pose. + */ + public void init() { + pinpoint.setOffsets(0.0, -105.0, DistanceUnit.MM); + pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + pinpoint.setEncoderDirections( + GoBildaPinpointDriver.EncoderDirection.FORWARD, + GoBildaPinpointDriver.EncoderDirection.FORWARD); + + // Robot must be stationary during this call. + pinpoint.resetPosAndIMU(); + } + + @Override + public void periodic() { + pinpoint.update(); + } + + public Pose2D getPose() { + return pinpoint.getPosition(); + } + + /** Zeroes the reported heading in place, keeping the current position. */ + public void resetHeading() { + Pose2D current = pinpoint.getPosition(); + pinpoint.setPosition(new Pose2D( + DistanceUnit.MM, current.getX(DistanceUnit.MM), current.getY(DistanceUnit.MM), + AngleUnit.DEGREES, 0)); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..9e4a693 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -0,0 +1,162 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; +import org.firstinspires.ftc.teamcode.subsystems.IndexerStallGuard.Phase; + +/** + * Controls the two-wheel shooter and the indexer that feeds balls into it. + * + *

+ * + * The "wait until at speed" step is non-blocking: it is handled in + * {@link #periodic()}, which the CommandScheduler calls every loop, so it never + * stalls the OpMode loop. + */ +public class ShooterSubsystem extends SubsystemBase { + // ---- Tuning ---------------------------------------------------------- + private static final double SHOOTER_VELOCITY = 4000; + private static final double INDEXER_POWER = 1.0; + private static final double EJECT_INDEXER_POWER = 0.6; + private static final double UNJAM_INDEXER_POWER = 0.6; + /** Current and velocity are separate hub reads, so sample them this often. */ + private static final double STALL_SAMPLE_PERIOD_S = 0.05; + + // ---- Hardware -------------------------------------------------------- + private final GobildaMotor indexerMotor; + private final GobildaMotor shooterFrontMotor; + private final GobildaMotor shooterBackMotor; + + public enum ShooterState { + STOP, IDLE, SHOOT, EJECT + } + + private ShooterState shooterState; + private ShooterState previousShooterState = ShooterState.STOP; + + // ---- Stall protection ------------------------------------------------ + private final IndexerStallGuard indexerStallGuard = new IndexerStallGuard(INDEXER_POWER, -UNJAM_INDEXER_POWER); + private final ElapsedTime clock = new ElapsedTime(); + private double lastStallSampleS = Double.NEGATIVE_INFINITY; + private double indexerCurrentAmps; + private double indexerVelocity; + + public ShooterSubsystem(HardwareMap hardwareMap) { + indexerMotor = new GobildaMotor(hardwareMap, "Indexer", false, DcMotor.RunMode.RUN_USING_ENCODER, true); + shooterFrontMotor = new GobildaMotor(hardwareMap, "ShooterF", false, DcMotor.RunMode.RUN_USING_ENCODER, false); + shooterBackMotor = new GobildaMotor(hardwareMap, "ShooterB", false, DcMotor.RunMode.RUN_USING_ENCODER, false); + + setShooterState(ShooterState.IDLE); + } + + private void doShooterState() { + if (previousShooterState == shooterState) { + return; + } + + switch (shooterState) { + case STOP: + indexerMotor.stop(); + setShooterPower(0.0); + break; + case IDLE: + indexerMotor.stop(); + setShooterMode(DcMotor.RunMode.RUN_USING_ENCODER); + setShooterPower(1.0); + break; + case SHOOT: + indexerStallGuard.start(clock.seconds()); + indexerMotor.setPower(indexerStallGuard.getPower()); + // RUN_USING_ENCODER holds power 1.0 to 85% of the motor's rated speed + // (about 5100 RPM on the 6000 RPM motors); open loop gives full voltage. + setShooterMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + setShooterPower(1.0); + break; + case EJECT: + indexerMotor.setPower(-1.0); + setShooterMode(DcMotor.RunMode.RUN_USING_ENCODER); + setShooterPower(-1.0); + break; + } + } + + public void setShooterState(ShooterState newShooterState) { + previousShooterState = shooterState; + shooterState = newShooterState; + doShooterState(); + } + + /** + * While shooting, watches the indexer for a jam and backs it off. See + * {@link IndexerStallGuard}. + */ + @Override + public void periodic() { + if (shooterState != ShooterState.SHOOT) { + return; + } + double now = clock.seconds(); + if (now - lastStallSampleS < STALL_SAMPLE_PERIOD_S) { + return; + } + lastStallSampleS = now; + + indexerCurrentAmps = indexerMotor.getCurrentAmps(); + indexerVelocity = indexerMotor.getVelocity(); + Phase before = indexerStallGuard.getPhase(); + indexerMotor.setPower(indexerStallGuard.update(now, indexerCurrentAmps, indexerVelocity)); + if (indexerStallGuard.getPhase() != before) { + RobotLog.dd("BioBuzz", "indexer %s -> %s at %.1f A, %.0f ticks/s, unjam %d/%d", + before, indexerStallGuard.getPhase(), indexerCurrentAmps, indexerVelocity, + indexerStallGuard.getUnjamAttempts(), IndexerStallGuard.MAX_UNJAM_ATTEMPTS); + } + } + + /** True while shooting with the indexer feeding, not backing off a jam. */ + public boolean isIndexerFeeding() { + return shooterState == ShooterState.SHOOT && indexerStallGuard.getPhase() == Phase.FEEDING; + } + + /** True once the indexer has given up on a jam; clears when shooting restarts. */ + public boolean isIndexerJammed() { + return shooterState == ShooterState.SHOOT && indexerStallGuard.getPhase() == Phase.JAMMED; + } + + /** Indexer feed state and the last current and velocity reading, for telemetry. */ + public String getIndexerStatus() { + if (shooterState != ShooterState.SHOOT) { + return shooterState.toString(); + } + return String.format("%s %.1f A %.0f ticks/s unjam %d/%d", + indexerStallGuard.getPhase(), indexerCurrentAmps, indexerVelocity, + indexerStallGuard.getUnjamAttempts(), IndexerStallGuard.MAX_UNJAM_ATTEMPTS); + } + + public boolean isAtSpeed() { + double minSpeed = SHOOTER_VELOCITY; + return Math.abs(shooterFrontMotor.getVelocity()) >= minSpeed + && Math.abs(shooterBackMotor.getVelocity()) >= minSpeed; + } + + private void setShooterMode(DcMotor.RunMode mode) { + shooterFrontMotor.setMode(mode); + shooterBackMotor.setMode(mode); + } + + private void setShooterPower(double ticksPerSecond) { + shooterFrontMotor.setPower(ticksPerSecond); + shooterBackMotor.setPower(ticksPerSecond); + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java new file mode 100644 index 0000000..4f87189 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import static org.junit.Assert.assertEquals; + +import org.firstinspires.ftc.teamcode.subsystems.IndexerStallGuard.Phase; +import org.junit.Before; +import org.junit.Test; + +/** Currents and speeds are from the prototype's two-ball jam on 30 Sep. */ +public class IndexerStallGuardTest { + private static final double FEED = 1.0; + private static final double UNJAM = -0.6; + private static final double STEP_S = 0.05; + + private IndexerStallGuard guard; + private double now; + + @Before + public void setUp() { + guard = new IndexerStallGuard(FEED, UNJAM); + now = 0; + guard.start(now); + } + + /** Feeds the same reading for a duration, returning the last power. */ + private double run(double seconds, double amps, double velocity) { + double power = guard.getPower(); + for (double end = now + seconds; now < end - 1e-9;) { + now += STEP_S; + power = guard.update(now, amps, velocity); + } + return power; + } + + @Test + public void feedsWhileRunningFreely() { + assertEquals(FEED, run(2.0, 1.7, 2200), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void heavyLoadThatStillMovesIsNotAStall() { + assertEquals(FEED, run(2.0, 8.7, 460), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void briefStallIsIgnored() { + run(0.2, 10.5, 0); + assertEquals(FEED, run(1.0, 7.5, 800), 0); + assertEquals(0, guard.getUnjamAttempts()); + } + + @Test + public void sustainedStallBacksOffThenResumes() { + assertEquals(UNJAM, run(0.4, 10.5, 0), 0); + assertEquals(Phase.UNJAMMING, guard.getPhase()); + assertEquals(1, guard.getUnjamAttempts()); + + assertEquals(FEED, run(IndexerStallGuard.UNJAM_TIME_S, 3.0, -300), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void givesUpAfterMaxAttemptsAndStaysOff() { + for (int i = 0; i < IndexerStallGuard.MAX_UNJAM_ATTEMPTS; i++) { + run(0.4, 10.5, 0); + run(IndexerStallGuard.UNJAM_TIME_S, 3.0, -300); + } + assertEquals(0.0, run(0.4, 10.5, 0), 0); + assertEquals(Phase.JAMMED, guard.getPhase()); + + // Stays off even once the motor goes quiet, until a new feed starts. + assertEquals(0.0, run(1.0, 0.0, 0), 0); + guard.start(now); + assertEquals(Phase.FEEDING, guard.getPhase()); + assertEquals(0, guard.getUnjamAttempts()); + assertEquals(FEED, guard.getPower(), 0); + } +}