diff --git a/README.md b/README.md index 6fd496f..02f43f8 100644 --- a/README.md +++ b/README.md @@ -114,3 +114,32 @@ run the `TeamCode` configuration. Android Studio does the same `adb install` and launch shown above. If the Hub does not appear in the device dropdown, run `adb connect 192.168.43.1:5555` in a terminal first; Android Studio picks up devices from the same adb server. + +## TeleOp hardware and controls + +Before selecting **BioBuzz TeleOp**, configure the Control Hub with these exact +names: `front_left_drive`, `front_right_drive`, `back_left_drive`, +`back_right_drive`, `pinpoint`, `intake`, `indexer`, `shooter_left`, and +`shooter_right`. The intake and shooter motors must support `DcMotorEx` encoder +control. Motor directions, shooter speed and PIDF, and Pinpoint offsets are in +`TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`; confirm +them on the robot before a match. + +During INIT, D-pad left selects red and D-pad right selects blue. The selected +alliance appears in telemetry. The current code does not use it for camera +aiming yet. + +| Gamepad 1 input | Action | +| --- | --- | +| Left stick | Field-relative translation | +| Right stick X | Rotation | +| Left bumper (hold) | Robot-relative translation | +| A | Zero heading | +| Right trigger (hold) | Run intake and indexer | +| Left trigger (hold) | Reverse intake and indexer | +| Right bumper | Toggle shooter spin-up | +| X (hold) | Feed with the indexer when both flywheels are at speed | + +Left trigger takes priority over right trigger, and either trigger takes +priority over X. Test motor directions, Pinpoint heading, and shooter RPM on +the robot before firing. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java new file mode 100644 index 0000000..cff46c9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode; + +public enum Alliance { + RED, + BLUE +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java new file mode 100644 index 0000000..8bbf5db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** Hardware names, directions and tuning values. One home so drivers and Pedro Pathing can find them. */ +public final class Constants { + private Constants() {} + + // Robot Controller configuration names. + public static final String FRONT_LEFT_DRIVE = "front_left_drive"; + public static final String FRONT_RIGHT_DRIVE = "front_right_drive"; + public static final String BACK_LEFT_DRIVE = "back_left_drive"; + public static final String BACK_RIGHT_DRIVE = "back_right_drive"; + public static final String INTAKE = "intake"; + public static final String INDEXER = "indexer"; + public static final String SHOOTER_LEFT = "shooter_left"; + public static final String SHOOTER_RIGHT = "shooter_right"; + public static final String PINPOINT = "pinpoint"; + public static final String WEBCAM = "Webcam 1"; + + // Motor directions. Left side reversed so positive power on every drive motor goes forward. + public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction INDEXER_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction SHOOTER_LEFT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction SHOOTER_RIGHT_DIRECTION = DcMotorSimple.Direction.REVERSE; + + // Pinpoint pod offsets from the tracking point, mm. X pod: left of centre positive. + // Y pod: forward of centre positive. Measure on the robot per the goBILDA setup guide. + public static final double PINPOINT_X_OFFSET_MM = 0.0; + public static final double PINPOINT_Y_OFFSET_MM = -105.0; + + // Intake and indexer open-loop powers. + public static final double INTAKE_POWER = 1.0; + public static final double INDEXER_POWER = 1.0; + + // Shooter. 1:1 Yellow Jacket, 28 ticks per rev, 6000 RPM free speed. + public static final double SHOOTER_TICKS_PER_REV = 28.0; + public static final double SHOOTER_SETPOINT_RPM = 3500.0; // tune on the robot + public static final double SHOOTER_TOLERANCE_RPM = 100.0; + // Velocity PIDF starting point: F = 32767 / max ticks per second, P = 0.1 F, I = 0.1 P, D = 0. + public static final double SHOOTER_P = 1.17; + public static final double SHOOTER_I = 0.117; + public static final double SHOOTER_D = 0.0; + public static final double SHOOTER_F = 11.7; + + // Driver controls. + public static final double TRIGGER_THRESHOLD = 0.2; + + // Aim assist: rotation = -AIM_KP * bearingDeg, clamped. Bearing is positive to the left. + public static final double AIM_KP = 0.02; + public static final double AIM_MAX_ROTATE = 0.5; + public static final double AIM_DEADBAND_DEG = 1.0; +} 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..63d923a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java @@ -13,6 +13,11 @@ public void init() { robot.init(); } + @Override + public void init_loop() { + robot.initLoop(); + } + @Override public void loop() { robot.periodic(); 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..8cbdbfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -1,36 +1,46 @@ package org.firstinspires.ftc.teamcode; import com.arcrobotics.ftclib.command.CommandScheduler; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem; public class Robot { private final Telemetry telemetry; - private final Gamepad driver; + private final GamepadEx driver; + private Alliance alliance = Alliance.RED; public final MecanumDriveSubsystem drive; public final OdometrySubsystem odometry; + public final IntakeSubsystem intake; + public final IndexerSubsystem indexer; + public final ShooterSubsystem shooter; - public Robot( - HardwareMap hardwareMap, - Telemetry telemetry, - Gamepad driver - ) { + public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; - this.driver = driver; + this.driver = new GamepadEx(driverGamepad); + + for (LynxModule hub : hardwareMap.getAll(LynxModule.class)) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } CommandScheduler.getInstance().reset(); - // Construct subsystems. drive = new MecanumDriveSubsystem(hardwareMap); odometry = new OdometrySubsystem(hardwareMap); - // intake = new IntakeSubsystem(hardwareMap); - // outtake = new OuttakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); + indexer = new IndexerSubsystem(hardwareMap); + shooter = new ShooterSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -38,24 +48,69 @@ public void init() { odometry.init(); } + /** Select the alliance before START with D-pad left or right. */ + public void initLoop() { + driver.readButtons(); + CommandScheduler.getInstance().run(); + + if (driver.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { + alliance = Alliance.RED; + } else if (driver.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { + alliance = Alliance.BLUE; + } + + telemetry.addData("Alliance (D-pad left/right)", alliance); + telemetry.update(); + } + /** Called repeatedly while the OpMode is running. */ public void periodic() { + driver.readButtons(); CommandScheduler.getInstance().run(); - if (driver.a) { + if (driver.wasJustPressed(GamepadKeys.Button.A)) { odometry.resetHeading(); } // 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)); + drive.drive(driver.getLeftY(), driver.getLeftX(), driver.getRightX(), + !driver.isDown(GamepadKeys.Button.LEFT_BUMPER), + odometry.getPose().getHeading(AngleUnit.RADIANS)); + + if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { + shooter.toggle(); + } + + double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); + if (leftTrigger > Constants.TRIGGER_THRESHOLD) { + intake.reverse(); + indexer.reverse(); + } else if (rightTrigger > Constants.TRIGGER_THRESHOLD) { + intake.run(); + indexer.feed(); + } else if (driver.isDown(GamepadKeys.Button.X) && shooter.atSpeed()) { + intake.stop(); + indexer.feed(); + } else { + intake.stop(); + indexer.stop(); + } + telemetry.addData("Alliance", alliance); telemetry.addData("Pose", odometry.getPose()); + telemetry.addData("Shooter", "%s target %.0f RPM L %.0f R %.0f %s", + shooter.isRunning() ? "ON" : "off", Constants.SHOOTER_SETPOINT_RPM, + shooter.getLeftRpm(), shooter.getRightRpm(), shooter.atSpeed() ? "AT SPEED" : ""); telemetry.update(); } /** Called once when the OpMode stops. */ public void stop() { + drive.stop(); + intake.stop(); + indexer.stop(); + shooter.idle(); CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().reset(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java new file mode 100644 index 0000000..54feb3b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode; + +/** Encoder velocity conversions for the shooter flywheels. */ +public final class ShooterMath { + private ShooterMath() {} + + public static double rpmToTicksPerSecond(double rpm, double ticksPerRev) { + return rpm * ticksPerRev / 60.0; + } + + public static double ticksPerSecondToRpm(double ticksPerSecond, double ticksPerRev) { + return ticksPerSecond * 60.0 / ticksPerRev; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java new file mode 100644 index 0000000..9983b41 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; + +/** Feeds balls to the shooter and brakes when stopped. */ +public class IndexerSubsystem extends SubsystemBase { + private final DcMotorEx motor; + + public IndexerSubsystem(HardwareMap hardwareMap) { + motor = hardwareMap.get(DcMotorEx.class, Constants.INDEXER); + motor.setDirection(Constants.INDEXER_DIRECTION); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void feed() { + motor.setPower(Constants.INDEXER_POWER); + } + + public void reverse() { + motor.setPower(-Constants.INDEXER_POWER); + } + + public void stop() { + motor.setPower(0); + } +} 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..5e03ad2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; + +/** Front intake roller, driven open loop. */ +public class IntakeSubsystem extends SubsystemBase { + private final DcMotorEx motor; + + public IntakeSubsystem(HardwareMap hardwareMap) { + motor = hardwareMap.get(DcMotorEx.class, Constants.INTAKE); + motor.setDirection(Constants.INTAKE_DIRECTION); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + public void intake(double power) { + motor.setPower(power); + } + + public void run() { + intake(Constants.INTAKE_POWER); + } + + public void reverse() { + intake(-Constants.INTAKE_POWER); + } + + public void stop() { + intake(0); + } +} 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..68d296c 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,6 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Constants; /** * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric @@ -17,16 +18,16 @@ public class MecanumDriveSubsystem extends SubsystemBase { private final DcMotor backRightDrive; 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"); + frontLeftDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_LEFT_DRIVE); + frontRightDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_RIGHT_DRIVE); + backLeftDrive = hardwareMap.get(DcMotor.class, Constants.BACK_LEFT_DRIVE); + backRightDrive = hardwareMap.get(DcMotor.class, Constants.BACK_RIGHT_DRIVE); // 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); + frontLeftDrive.setDirection(Constants.FRONT_LEFT_DIRECTION); + frontRightDrive.setDirection(Constants.FRONT_RIGHT_DIRECTION); + backLeftDrive.setDirection(Constants.BACK_LEFT_DIRECTION); + backRightDrive.setDirection(Constants.BACK_RIGHT_DIRECTION); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -70,4 +71,12 @@ public void drive(double forward, double right, double rotate, boolean fieldCent backLeftDrive.setPower(backLeftPower / maxPower); backRightDrive.setPower(backRightPower / maxPower); } + + /** Stops all drive motors when the OpMode ends. */ + public void stop() { + frontLeftDrive.setPower(0); + frontRightDrive.setPower(0); + backLeftDrive.setPower(0); + backRightDrive.setPower(0); + } } 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..7de73c1 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 @@ -7,6 +7,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Constants; /** * Wraps a goBILDA Pinpoint Odometry Computer reading two dead-wheel pods, and @@ -16,12 +17,12 @@ public class OdometrySubsystem extends SubsystemBase { private final GoBildaPinpointDriver pinpoint; public OdometrySubsystem(HardwareMap hardwareMap) { - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, Constants.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.setOffsets(Constants.PINPOINT_X_OFFSET_MM, Constants.PINPOINT_Y_OFFSET_MM, DistanceUnit.MM); pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); pinpoint.setEncoderDirections( GoBildaPinpointDriver.EncoderDirection.FORWARD, 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..932ce94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; +import org.firstinspires.ftc.teamcode.ShooterMath; + +/** Twin flywheels controlled by the hubs' velocity controllers. */ +public class ShooterSubsystem extends SubsystemBase { + private final DcMotorEx left; + private final DcMotorEx right; + private boolean running; + + public ShooterSubsystem(HardwareMap hardwareMap) { + left = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_LEFT); + right = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_RIGHT); + left.setDirection(Constants.SHOOTER_LEFT_DIRECTION); + right.setDirection(Constants.SHOOTER_RIGHT_DIRECTION); + + for (DcMotorEx motor : new DcMotorEx[]{left, right}) { + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + motor.setVelocityPIDFCoefficients( + Constants.SHOOTER_P, Constants.SHOOTER_I, + Constants.SHOOTER_D, Constants.SHOOTER_F); + } + } + + public void spinUp() { + double ticksPerSecond = ShooterMath.rpmToTicksPerSecond( + Constants.SHOOTER_SETPOINT_RPM, Constants.SHOOTER_TICKS_PER_REV); + left.setVelocity(ticksPerSecond); + right.setVelocity(ticksPerSecond); + running = true; + } + + public void idle() { + left.setVelocity(0); + right.setVelocity(0); + running = false; + } + + public void toggle() { + if (running) { + idle(); + } else { + spinUp(); + } + } + + public boolean isRunning() { + return running; + } + + public boolean atSpeed() { + return running + && Math.abs(getLeftRpm() - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(getRightRpm() - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM; + } + + public double getLeftRpm() { + return ShooterMath.ticksPerSecondToRpm(left.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + } + + public double getRightRpm() { + return ShooterMath.ticksPerSecondToRpm(right.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java new file mode 100644 index 0000000..812a21d --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode; + +import static org.junit.Assert.assertEquals; + +import org.junit.Test; + +public class ShooterMathTest { + @Test + public void freeSpeedOfOneToOneMotorIs2800TicksPerSecond() { + assertEquals(2800.0, ShooterMath.rpmToTicksPerSecond(6000, 28), 1e-9); + } + + @Test + public void conversionsRoundTrip() { + double rpm = 3500; + double ticksPerSecond = ShooterMath.rpmToTicksPerSecond(rpm, 28); + assertEquals(rpm, ShooterMath.ticksPerSecondToRpm(ticksPerSecond, 28), 1e-9); + } +}