diff --git a/build.gradle.kts b/build.gradle.kts index 4d8cf11..a9670e0 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -14,7 +14,8 @@ java { // Exclude FTC hardware files from PC compilation (they need Android SDK) tasks.withType { - exclude("**/FtcMotor.java") + exclude("**/FtcDCMotor.java") + exclude("**/FtcCRServo.java") exclude("**/FtcPotentiometer.java") exclude("**/opmodes/ChuteOpMode.java") } @@ -24,7 +25,8 @@ tasks.javadoc { include("**/subsystems/chute/**") // But exclude FTC hardware files (they need Android SDK) - exclude("**/FtcMotor.java") + exclude("**/FtcDCMotor.java") + exclude("**/FtcCRServo.java") exclude("**/FtcPotentiometer.java") exclude("**/opmodes/**") diff --git a/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java b/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java index 09476bd..8f0b43f 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ChuteOpMode.java @@ -2,19 +2,23 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.AnalogInput; import org.firstinspires.ftc.teamcode.subsystems.chute.Chute; -import org.firstinspires.ftc.teamcode.subsystems.chute.FtcMotor; +import org.firstinspires.ftc.teamcode.subsystems.chute.FtcCRServo; +import org.firstinspires.ftc.teamcode.subsystems.chute.FtcDCMotor; import org.firstinspires.ftc.teamcode.subsystems.chute.FtcPotentiometer; import org.firstinspires.ftc.teamcode.subsystems.chute.ChuteController; +import org.firstinspires.ftc.teamcode.subsystems.chute.MockMotor; /** * Example OpMode demonstrating chute control with real hardware. + * Supports either a DcMotor or CRServo as the actuator. * * Hardware Config: - * - Motor: "chute_motor" (DcMotor) + * - Motor: "chute_motor" (DcMotor) OR "chute_servo" (CRServo) * - Potentiometer: "chute_pot" (AnalogInput) */ @TeleOp(name = "Chute Control") @@ -22,6 +26,9 @@ public class ChuteOpMode extends LinearOpMode { private Chute chute; + // Set to true to use a CRServo instead of a DcMotor + private static final boolean USE_SERVO = false; + // Pot configuration - change this if your pot has different range private static final double POT_WRAP_AMOUNT = 2 * Math.PI; @@ -35,14 +42,20 @@ public class ChuteOpMode extends LinearOpMode { @Override public void runOpMode() { // Get hardware from config - DcMotor chuteMotor = hardwareMap.get(DcMotor.class, "chute_motor"); AnalogInput chutePot = hardwareMap.get(AnalogInput.class, "chute_pot"); - - // Create hardware adapters - FtcMotor motor = new FtcMotor(chuteMotor); FtcPotentiometer pot = new FtcPotentiometer(chutePot, POT_WRAP_AMOUNT); - // Create chute controller with real hardware + // Create motor adapter — either DcMotor or CRServo + MockMotor motor; + if (USE_SERVO) { + CRServo chuteServo = hardwareMap.get(CRServo.class, "chute_servo"); + motor = new FtcCRServo(chuteServo); + } else { + DcMotor chuteMotor = hardwareMap.get(DcMotor.class, "chute_motor"); + motor = new FtcDCMotor(chuteMotor); + } + + // Create chute controller with whichever actuator was selected ChuteController controller = new ChuteController(motor, pot, MAX); chute = new Chute(controller, motor, pot); diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java index 2e6da0c..d0464c4 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/Chute.java @@ -22,7 +22,14 @@ package org.firstinspires.ftc.teamcode.subsystems.chute; * *

Usage with Real Hardware

*
- * FtcMotor motor = new FtcMotor(hardwareMap.get(DcMotor.class, "chute_motor"));
+ * // With a DcMotor:
+ * FtcDCMotor motor = new FtcDCMotor(hardwareMap.get(DcMotor.class, "chute_motor"));
+ * FtcPotentiometer pot = new FtcPotentiometer(hardwareMap.get(AnalogInput.class, "chute_pot"));
+ * ChuteController controller = new ChuteController(motor, pot, 4 * Math.PI);
+ * Chute chute = new Chute(controller, motor, pot);
+ *
+ * // With a CRServo:
+ * FtcCRServo motor = new FtcCRServo(hardwareMap.get(CRServo.class, "chute_servo"));
  * FtcPotentiometer pot = new FtcPotentiometer(hardwareMap.get(AnalogInput.class, "chute_pot"));
  * ChuteController controller = new ChuteController(motor, pot, 4 * Math.PI);
  * Chute chute = new Chute(controller, motor, pot);
@@ -38,6 +45,7 @@ package org.firstinspires.ftc.teamcode.subsystems.chute;
  * 
  * 
  * @see ChuteController
+ * @see FtcCRServo
  * @see MockPotentiometer
  * @see FtcPotentiometer
  */
@@ -74,9 +82,9 @@ public class Chute {
      * Use this constructor in actual FTC OpModes with hardware from hardwareMap.
      * 
      * @param controller Pre-configured controller with real hardware
-     * @param motor FtcMotor adapter wrapping DcMotor
+     * @param motor FtcDCMotor adapter wrapping DcMotor
      * @param pot FtcPotentiometer adapter wrapping AnalogInput
-     * @see FtcMotor
+     * @see FtcDCMotor
      * @see FtcPotentiometer
      */
     public Chute(ChuteController controller, MockMotor motor, MockPotentiometer pot) {
diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcCRServo.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcCRServo.java
new file mode 100644
index 0000000..3a47eca
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcCRServo.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.subsystems.chute;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+
+/**
+ * Adapter that wraps a real FTC CRServo and makes it look like a MockMotor.
+ * Drop-in replacement for FtcDCMotor when the chute is driven by a continuous
+ * rotation servo instead of a DC motor.
+ *
+ * 

CRServo has no encoder, zero-power behavior, or run mode—just power + * control—so this adapter is simpler than {@link FtcDCMotor}. + * + *

Usage

+ *
+ * CRServo servo = hardwareMap.get(CRServo.class, "chute_servo");
+ * FtcCRServo motor = new FtcCRServo(servo);
+ * ChuteController controller = new ChuteController(motor, pot, maxExtension);
+ * 
+ * + * @see FtcDCMotor + */ +public class FtcCRServo extends MockMotor { + private final CRServo servo; + + /** + * Create servo adapter. + * @param servo The real FTC CRServo from hardwareMap + */ + public FtcCRServo(CRServo servo) { + super(); + this.servo = servo; + } + + @Override + public void setPower(double power) { + super.setPower(power); // Track power in parent + servo.setPower(power); // Send to real servo + } + + @Override + public void update(double dt) { + // Real servo updates itself — still call parent for mock position tracking + super.update(dt); + } +} diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcMotor.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcDCMotor.java similarity index 92% rename from src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcMotor.java rename to src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcDCMotor.java index 090c4cd..6cdfba4 100644 --- a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcMotor.java +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/FtcDCMotor.java @@ -6,14 +6,14 @@ import com.qualcomm.robotcore.hardware.DcMotor; * Adapter that wraps a real FTC DcMotor and makes it look like a MockMotor. * Use this to connect the chute controller to actual hardware. */ -public class FtcMotor extends MockMotor { +public class FtcDCMotor extends MockMotor { private final DcMotor motor; /** * Create motor adapter. * @param motor The real FTC motor from hardwareMap */ - public FtcMotor(DcMotor motor) { + public FtcDCMotor(DcMotor motor) { super(); this.motor = motor; diff --git a/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockCRServo.java b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockCRServo.java new file mode 100644 index 0000000..cb7aa54 --- /dev/null +++ b/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chute/MockCRServo.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.subsystems.chute; + +/** + * Mock continuous rotation servo for testing. + * Mirrors the override pattern of {@link FtcCRServo} without requiring the FTC SDK. + * + *

Extends MockMotor and overrides {@code setPower()} to track commanded values, + * letting tests verify that ChuteController correctly routes through a subclassed motor. + */ +public class MockCRServo extends MockMotor { + private double lastCommandedPower = 0.0; + private int setPowerCallCount = 0; + + @Override + public void setPower(double power) { + lastCommandedPower = power; + setPowerCallCount++; + super.setPower(power); + } + + /** + * @return The most recent power value passed to setPower + */ + public double getLastCommandedPower() { + return lastCommandedPower; + } + + /** + * @return Total number of times setPower has been called + */ + public int getSetPowerCallCount() { + return setPowerCallCount; + } +} \ No newline at end of file diff --git a/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java b/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java index 8d0de7a..9718229 100644 --- a/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java +++ b/src/test/java/org/firstinspires/ftc/teamcode/subsystems/chute/ChuteTest.java @@ -268,4 +268,138 @@ public class ChuteTest { assertEquals(3.0, chute.getPot().getWrapAmount(), EPSILON, "Pot should wrap at 3.0 radians"); } + + // ======================================================================== + // CRServo adapter tests + // + // FtcCRServo extends MockMotor and overrides setPower() to forward to real + // hardware. MockCRServo replicates that pattern without the FTC SDK, + // proving the full pipeline works when the motor is a subclass. + // ======================================================================== + + /** + * Verify a subclassed motor homes correctly through ChuteController. + * This is the same scenario as testHoming() but with the override path. + */ + @Test + public void testCRServoHoming() { + MockCRServo servo = new MockCRServo(); + MockPotentiometer pot = new MockPotentiometer(); + ChuteController controller = new ChuteController(servo, pot, 10.0); + Chute chute = new Chute(controller, servo, pot); + + servo.setPosition(0.5); + pot.setPosition(0.5); + + chute.home(); + assertTrue(servo.getSetPowerCallCount() > 0, "setPower should be called via override"); + + for (int i = 0; i < 200; i++) { + chute.update(DT); + if (chute.isHomed()) break; + } + + assertTrue(chute.isHomed(), "Should home with subclassed motor"); + assertEquals(0.0, servo.getPower(), EPSILON, "Motor should be stopped after homing"); + assertEquals(0.0, servo.getLastCommandedPower(), EPSILON, + "Override path should also see zero power"); + } + + /** + * Verify move-to-position works end-to-end through the override path. + */ + @Test + public void testCRServoMoveToPosition() { + MockCRServo servo = new MockCRServo(); + MockPotentiometer pot = new MockPotentiometer(); + ChuteController controller = new ChuteController(servo, pot, 10.0); + Chute chute = new Chute(controller, servo, pot); + + servo.setPosition(0.2); + pot.setPosition(0.2); + chute.home(); + for (int i = 0; i < 200; i++) { + chute.update(DT); + if (chute.isHomed()) break; + } + assertTrue(chute.isHomed(), "Should be homed"); + + int callsBefore = servo.getSetPowerCallCount(); + + chute.setTargetPosition(2.0); + for (int i = 0; i < 1000; i++) { + chute.update(DT); + if (chute.isAtTarget()) break; + } + + assertTrue(chute.isAtTarget(), "Should reach target with subclassed motor"); + assertTrue(Math.abs(chute.getPosition() - 2.0) < 0.3, + "Position should be close to 2.0, got: " + chute.getPosition()); + assertTrue(servo.getSetPowerCallCount() > callsBefore, + "setPower override should have been called during movement"); + } + + /** + * Verify multi-wrap tracking works through the override path. + * CRServos are common on linear actuators that travel multiple pot rotations. + */ + @Test + public void testCRServoMultipleRotations() { + MockCRServo servo = new MockCRServo(); + MockPotentiometer pot = new MockPotentiometer(); + ChuteController controller = new ChuteController(servo, pot, 25.0); + Chute chute = new Chute(controller, servo, pot); + + servo.setPosition(0.2); + pot.setPosition(0.2); + chute.home(); + for (int i = 0; i < 200; i++) { + chute.update(DT); + if (chute.isHomed()) break; + } + assertTrue(chute.isHomed(), "Should be homed"); + + double target = 3.5 * 2 * Math.PI; + chute.setTargetPosition(target); + for (int i = 0; i < 2000; i++) { + chute.update(DT); + if (chute.isAtTarget()) break; + } + + assertTrue(chute.getPosition() > 3.0 * 2 * Math.PI, + "Position should be > 3*2pi, got: " + chute.getPosition()); + assertTrue(Math.abs(chute.getPosition() - target) < 1.5, + "Position should be near target, expected: " + target + ", got: " + chute.getPosition()); + } + + /** + * Verify emergency stop propagates through the override path. + */ + @Test + public void testCRServoStop() { + MockCRServo servo = new MockCRServo(); + MockPotentiometer pot = new MockPotentiometer(); + ChuteController controller = new ChuteController(servo, pot, 10.0); + Chute chute = new Chute(controller, servo, pot); + + servo.setPosition(0.1); + pot.setPosition(0.1); + chute.home(); + for (int i = 0; i < 200; i++) { + chute.update(DT); + if (chute.isHomed()) break; + } + + chute.setTargetPosition(5.0); + for (int i = 0; i < 10; i++) { + chute.update(DT); + } + assertTrue(chute.isMoving(), "Should be moving"); + + chute.stop(); + + assertFalse(chute.isMoving(), "Should stop immediately"); + assertEquals(0.0, servo.getLastCommandedPower(), EPSILON, + "Override path should see zero power after stop"); + } } \ No newline at end of file