feat: Add template system to weevil new command
Implements template-based project creation allowing teams to start with
professional example code instead of empty projects.
Features:
- Two templates: 'basic' (minimal) and 'testing' (45-test showcase)
- Template variable substitution ({{PROJECT_NAME}}, etc.)
- Template validation with helpful error messages
- `weevil new --list-templates` command
- Template files embedded in binary at compile time
Technical details:
- Templates stored in templates/basic/ and templates/testing/
- Files ending in .template have variables replaced
- Uses include_dir! macro to embed templates in binary
- Returns file count for user feedback
Testing template includes:
- 3 complete subsystems (MotorCycler, WallApproach, TurnController)
- Hardware abstraction layer with mock implementations
- 45 comprehensive tests (unit, integration, system)
- Professional documentation (DESIGN_AND_TEST_PLAN.md, etc.)
Usage:
weevil new my-robot # basic template
weevil new my-robot --template testing # testing showcase
weevil new --list-templates # show available templates
This enables FTC teams to learn from working code and best practices
rather than starting from scratch.
This commit is contained in:
Eric Ratliff
@@ -0,0 +1,19 @@
|
||||
package robot.hardware;
|
||||
|
||||
/**
|
||||
* Interface for distance sensors (ultrasonic, time-of-flight, etc.).
|
||||
* This abstraction allows testing distance-based behaviors without physical sensors.
|
||||
*/
|
||||
public interface DistanceSensor {
|
||||
/**
|
||||
* Get the current distance reading in centimeters.
|
||||
* @return distance in cm, or -1 if sensor error
|
||||
*/
|
||||
double getDistanceCm();
|
||||
|
||||
/**
|
||||
* Check if the sensor has valid data.
|
||||
* @return true if sensor is working and has valid reading
|
||||
*/
|
||||
boolean isValid();
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package robot.hardware;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* FTC implementation of DistanceSensor interface.
|
||||
* This file will be EXCLUDED from test compilation.
|
||||
*/
|
||||
public class FtcDistanceSensor implements robot.hardware.DistanceSensor {
|
||||
private final DistanceSensor sensor;
|
||||
|
||||
public FtcDistanceSensor(DistanceSensor sensor) {
|
||||
this.sensor = sensor;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getDistanceCm() {
|
||||
return sensor.getDistance(DistanceUnit.CM);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isValid() {
|
||||
double dist = getDistanceCm();
|
||||
return dist > 0 && dist < 8190; // Valid range for REV sensors
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
package robot.hardware;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||
|
||||
/**
|
||||
* FTC implementation of GyroSensor using REV Hub IMU.
|
||||
* This file will be EXCLUDED from test compilation.
|
||||
*/
|
||||
public class FtcGyroSensor implements GyroSensor {
|
||||
private final IMU imu;
|
||||
|
||||
public FtcGyroSensor(IMU imu) {
|
||||
this.imu = imu;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getHeading() {
|
||||
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
|
||||
double heading = angles.getYaw(AngleUnit.DEGREES);
|
||||
|
||||
// Normalize to 0-359
|
||||
while (heading < 0) heading += 360;
|
||||
while (heading >= 360) heading -= 360;
|
||||
|
||||
return heading;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void reset() {
|
||||
imu.resetYaw();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isCalibrated() {
|
||||
return imu.getRobotYawPitchRollAngles() != null;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
package robot.hardware;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
/**
|
||||
* FTC SDK implementation of MotorController.
|
||||
* This wraps the FTC DcMotor class and will only be available when deployed to the robot.
|
||||
*/
|
||||
public class FtcMotorController implements MotorController {
|
||||
private final DcMotor motor;
|
||||
|
||||
public FtcMotorController(DcMotor motor) {
|
||||
this.motor = motor;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setPower(double power) {
|
||||
motor.setPower(power);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getPower() {
|
||||
return motor.getPower();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
package robot.hardware;
|
||||
|
||||
/**
|
||||
* Interface for gyroscope/IMU sensors.
|
||||
* Provides heading information for navigation and autonomous driving.
|
||||
*/
|
||||
public interface GyroSensor {
|
||||
/**
|
||||
* Get the current heading in degrees.
|
||||
* @return heading from 0-359 degrees (0 = initial orientation)
|
||||
*/
|
||||
double getHeading();
|
||||
|
||||
/**
|
||||
* Reset the heading to zero.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* Check if the gyro is calibrated and ready.
|
||||
* @return true if gyro is working properly
|
||||
*/
|
||||
boolean isCalibrated();
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package robot.hardware;
|
||||
|
||||
/**
|
||||
* Interface for motor control.
|
||||
* This abstraction allows us to test our logic without actual hardware.
|
||||
*/
|
||||
public interface MotorController {
|
||||
/**
|
||||
* Set the motor power.
|
||||
* @param power Power level from -1.0 (full reverse) to 1.0 (full forward)
|
||||
*/
|
||||
void setPower(double power);
|
||||
|
||||
/**
|
||||
* Get the current motor power setting.
|
||||
* @return Current power level
|
||||
*/
|
||||
double getPower();
|
||||
|
||||
/**
|
||||
* Check if the motor is currently running.
|
||||
* @return true if power is non-zero
|
||||
*/
|
||||
default boolean isRunning() {
|
||||
return Math.abs(getPower()) > 0.001;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
package robot.opmodes;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import robot.hardware.FtcMotorController;
|
||||
import robot.subsystems.MotorCycler;
|
||||
|
||||
/**
|
||||
* Simple TeleOp mode that cycles a motor on/off.
|
||||
*
|
||||
* This demonstrates:
|
||||
* - Clean separation between hardware abstraction and logic
|
||||
* - Testable subsystems (MotorCycler can be tested without FTC SDK)
|
||||
* - Simple, readable OpMode code
|
||||
*/
|
||||
@TeleOp(name = "Motor Cycle Demo", group = "Demo")
|
||||
public class MotorCycleOpMode extends OpMode {
|
||||
private MotorCycler motorCycler;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
// Get the motor from the hardware map
|
||||
DcMotor motor = hardwareMap.get(DcMotor.class, "motor");
|
||||
|
||||
// Wrap it in our abstraction
|
||||
FtcMotorController motorController = new FtcMotorController(motor);
|
||||
|
||||
// Create the cycler: 2 seconds on, 1 second off
|
||||
motorCycler = new MotorCycler(motorController, 2000, 1000, 0.5);
|
||||
motorCycler.init();
|
||||
|
||||
telemetry.addData("Status", "Initialized - Ready to cycle motor");
|
||||
telemetry.addData("Pattern", "2s ON, 1s OFF");
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
// Update the cycler with current time
|
||||
motorCycler.update(System.currentTimeMillis());
|
||||
|
||||
// Display status
|
||||
telemetry.addData("Motor State", motorCycler.getState());
|
||||
telemetry.addData("Time in State", "%.1f seconds",
|
||||
motorCycler.getTimeInState(System.currentTimeMillis()) / 1000.0);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stop() {
|
||||
motorCycler.stop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import robot.hardware.MotorController;
|
||||
|
||||
/**
|
||||
* Subsystem that cycles a motor on and off with specific timing.
|
||||
* This demonstrates clean separation between logic and hardware.
|
||||
*/
|
||||
public class MotorCycler {
|
||||
private final MotorController motor;
|
||||
private final long onDurationMs;
|
||||
private final long offDurationMs;
|
||||
private final double motorPower;
|
||||
|
||||
private MotorCycleState state;
|
||||
private long stateStartTime;
|
||||
|
||||
public enum MotorCycleState {
|
||||
ON, OFF
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a motor cycler with custom timing.
|
||||
* @param motor The motor to control
|
||||
* @param onDurationMs How long to run the motor (milliseconds)
|
||||
* @param offDurationMs How long to pause between runs (milliseconds)
|
||||
* @param motorPower Power level to use when on (0.0 to 1.0)
|
||||
*/
|
||||
public MotorCycler(MotorController motor, long onDurationMs, long offDurationMs, double motorPower) {
|
||||
this.motor = motor;
|
||||
this.onDurationMs = onDurationMs;
|
||||
this.offDurationMs = offDurationMs;
|
||||
this.motorPower = motorPower;
|
||||
this.state = MotorCycleState.OFF;
|
||||
this.stateStartTime = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a motor cycler with default power (0.5).
|
||||
*/
|
||||
public MotorCycler(MotorController motor, long onDurationMs, long offDurationMs) {
|
||||
this(motor, onDurationMs, offDurationMs, 0.5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the cycler (call once at startup).
|
||||
*/
|
||||
public void init() {
|
||||
state = MotorCycleState.OFF;
|
||||
stateStartTime = System.currentTimeMillis();
|
||||
motor.setPower(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the motor state based on elapsed time.
|
||||
* Call this repeatedly in your main loop.
|
||||
* @param currentTimeMs Current time in milliseconds
|
||||
*/
|
||||
public void update(long currentTimeMs) {
|
||||
long elapsed = currentTimeMs - stateStartTime;
|
||||
|
||||
switch (state) {
|
||||
case OFF:
|
||||
if (elapsed >= offDurationMs) {
|
||||
// Time to turn on
|
||||
motor.setPower(motorPower);
|
||||
state = MotorCycleState.ON;
|
||||
stateStartTime = currentTimeMs;
|
||||
}
|
||||
break;
|
||||
|
||||
case ON:
|
||||
if (elapsed >= onDurationMs) {
|
||||
// Time to turn off
|
||||
motor.setPower(0.0);
|
||||
state = MotorCycleState.OFF;
|
||||
stateStartTime = currentTimeMs;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the motor and reset to initial state.
|
||||
*/
|
||||
public void stop() {
|
||||
motor.setPower(0.0);
|
||||
state = MotorCycleState.OFF;
|
||||
stateStartTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the current cycle state.
|
||||
*/
|
||||
public MotorCycleState getState() {
|
||||
return state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get how long we've been in the current state (ms).
|
||||
*/
|
||||
public long getTimeInState(long currentTimeMs) {
|
||||
return currentTimeMs - stateStartTime;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import robot.hardware.GyroSensor;
|
||||
import robot.hardware.MotorController;
|
||||
|
||||
/**
|
||||
* Subsystem that turns the robot to a target heading using gyro feedback.
|
||||
*
|
||||
* This demonstrates closed-loop control:
|
||||
* - Proportional control (turn faster when far from target)
|
||||
* - Threshold detection (when close enough to target)
|
||||
* - Direction selection (shortest path to target)
|
||||
*
|
||||
* Pure Java - fully testable without hardware!
|
||||
*/
|
||||
public class TurnController {
|
||||
private final GyroSensor gyro;
|
||||
private final MotorController leftMotor;
|
||||
private final MotorController rightMotor;
|
||||
|
||||
// Control parameters
|
||||
private final double HEADING_TOLERANCE = 2.0; // Within 2 degrees = success
|
||||
private final double MIN_TURN_POWER = 0.15; // Minimum power to overcome friction
|
||||
private final double MAX_TURN_POWER = 0.5; // Maximum turn speed
|
||||
private final double KP = 0.02; // Proportional gain
|
||||
|
||||
// State
|
||||
private double targetHeading = 0.0;
|
||||
private TurnState state = TurnState.IDLE;
|
||||
|
||||
public enum TurnState {
|
||||
IDLE, // Not turning
|
||||
TURNING, // Actively turning to target
|
||||
COMPLETE // Reached target heading
|
||||
}
|
||||
|
||||
public TurnController(GyroSensor gyro, MotorController leftMotor, MotorController rightMotor) {
|
||||
this.gyro = gyro;
|
||||
this.leftMotor = leftMotor;
|
||||
this.rightMotor = rightMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start turning to a target heading.
|
||||
*
|
||||
* @param targetDegrees target heading (0-359)
|
||||
*/
|
||||
public void turnTo(double targetDegrees) {
|
||||
this.targetHeading = normalizeHeading(targetDegrees);
|
||||
this.state = TurnState.TURNING;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the turn controller.
|
||||
* Call this repeatedly in your main loop.
|
||||
*/
|
||||
public void update() {
|
||||
if (state != TurnState.TURNING) {
|
||||
return; // Not actively turning
|
||||
}
|
||||
|
||||
if (!gyro.isCalibrated()) {
|
||||
stop();
|
||||
state = TurnState.IDLE;
|
||||
return;
|
||||
}
|
||||
|
||||
double currentHeading = gyro.getHeading();
|
||||
double error = calculateShortestError(currentHeading, targetHeading);
|
||||
|
||||
// Check if we've reached the target
|
||||
if (Math.abs(error) <= HEADING_TOLERANCE) {
|
||||
stop();
|
||||
state = TurnState.COMPLETE;
|
||||
return;
|
||||
}
|
||||
|
||||
// Proportional control: turn power proportional to error
|
||||
double turnPower = error * KP;
|
||||
|
||||
// Clamp to min/max power
|
||||
if (Math.abs(turnPower) < MIN_TURN_POWER) {
|
||||
turnPower = Math.signum(turnPower) * MIN_TURN_POWER;
|
||||
}
|
||||
if (Math.abs(turnPower) > MAX_TURN_POWER) {
|
||||
turnPower = Math.signum(turnPower) * MAX_TURN_POWER;
|
||||
}
|
||||
|
||||
// Apply power: positive error = turn right
|
||||
leftMotor.setPower(turnPower);
|
||||
rightMotor.setPower(-turnPower);
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop turning.
|
||||
*/
|
||||
public void stop() {
|
||||
leftMotor.setPower(0.0);
|
||||
rightMotor.setPower(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the shortest angular error between two headings.
|
||||
* Returns positive for clockwise, negative for counter-clockwise.
|
||||
*
|
||||
* Example: current=10, target=350 → error=-20 (turn left 20°)
|
||||
* current=350, target=10 → error=+20 (turn right 20°)
|
||||
*/
|
||||
private double calculateShortestError(double current, double target) {
|
||||
double error = target - current;
|
||||
|
||||
// Normalize to -180 to +180
|
||||
while (error > 180) error -= 360;
|
||||
while (error < -180) error += 360;
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize heading to 0-359 range.
|
||||
*/
|
||||
private double normalizeHeading(double degrees) {
|
||||
while (degrees < 0) degrees += 360;
|
||||
while (degrees >= 360) degrees -= 360;
|
||||
return degrees;
|
||||
}
|
||||
|
||||
// Getters for testing
|
||||
|
||||
public TurnState getState() {
|
||||
return state;
|
||||
}
|
||||
|
||||
public double getTargetHeading() {
|
||||
return targetHeading;
|
||||
}
|
||||
|
||||
public double getCurrentHeading() {
|
||||
return gyro.getHeading();
|
||||
}
|
||||
|
||||
public double getHeadingError() {
|
||||
return calculateShortestError(gyro.getHeading(), targetHeading);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import robot.hardware.DistanceSensor;
|
||||
import robot.hardware.MotorController;
|
||||
|
||||
/**
|
||||
* Subsystem that safely approaches a wall using distance sensor feedback.
|
||||
*
|
||||
* This demonstrates a real robotics control problem:
|
||||
* - Drive fast when far away
|
||||
* - Slow down as you get closer
|
||||
* - Stop before hitting the wall
|
||||
* - Handle sensor failures gracefully
|
||||
*
|
||||
* This is PURE JAVA - no FTC dependencies!
|
||||
* Can be tested instantly on Windows without a robot.
|
||||
*/
|
||||
public class WallApproach {
|
||||
// Hardware interfaces (not FTC classes!)
|
||||
private final DistanceSensor sensor;
|
||||
private final MotorController leftMotor;
|
||||
private final MotorController rightMotor;
|
||||
|
||||
// Configuration constants
|
||||
private final double STOP_DISTANCE_CM = 10.0; // Stop 10cm from wall
|
||||
private final double SLOW_DISTANCE_CM = 30.0; // Start slowing at 30cm
|
||||
private final double FAST_SPEED = 0.6; // Full speed when far
|
||||
private final double SLOW_SPEED = 0.2; // Slow speed when near
|
||||
|
||||
// State tracking
|
||||
private WallApproachState state = WallApproachState.INIT;
|
||||
private boolean sensorError = false;
|
||||
|
||||
public enum WallApproachState {
|
||||
INIT, // Not started
|
||||
APPROACHING, // Driving toward wall
|
||||
SLOWING, // Close to wall, slowing down
|
||||
STOPPED, // At target distance
|
||||
ERROR // Sensor failure
|
||||
}
|
||||
|
||||
public WallApproach(DistanceSensor sensor, MotorController leftMotor, MotorController rightMotor) {
|
||||
this.sensor = sensor;
|
||||
this.leftMotor = leftMotor;
|
||||
this.rightMotor = rightMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the approach sequence.
|
||||
*/
|
||||
public void start() {
|
||||
state = WallApproachState.APPROACHING;
|
||||
sensorError = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the approach logic.
|
||||
* Call this repeatedly in your main loop.
|
||||
*/
|
||||
public void update() {
|
||||
// Check for sensor errors
|
||||
if (!sensor.isValid()) {
|
||||
state = WallApproachState.ERROR;
|
||||
sensorError = true;
|
||||
stop();
|
||||
return;
|
||||
}
|
||||
|
||||
double distance = sensor.getDistanceCm();
|
||||
|
||||
// State machine logic
|
||||
switch (state) {
|
||||
case INIT:
|
||||
// Do nothing until started
|
||||
break;
|
||||
|
||||
case APPROACHING:
|
||||
if (distance <= STOP_DISTANCE_CM) {
|
||||
// Too close - stop immediately!
|
||||
stop();
|
||||
state = WallApproachState.STOPPED;
|
||||
} else if (distance <= SLOW_DISTANCE_CM) {
|
||||
// Getting close - slow down
|
||||
setMotors(SLOW_SPEED);
|
||||
state = WallApproachState.SLOWING;
|
||||
} else {
|
||||
// Far away - drive fast
|
||||
setMotors(FAST_SPEED);
|
||||
}
|
||||
break;
|
||||
|
||||
case SLOWING:
|
||||
if (distance <= STOP_DISTANCE_CM) {
|
||||
// Reached target distance
|
||||
stop();
|
||||
state = WallApproachState.STOPPED;
|
||||
} else if (distance > SLOW_DISTANCE_CM) {
|
||||
// Drifted backward? Speed up again
|
||||
setMotors(FAST_SPEED);
|
||||
state = WallApproachState.APPROACHING;
|
||||
} else {
|
||||
// Continue at slow speed
|
||||
setMotors(SLOW_SPEED);
|
||||
}
|
||||
break;
|
||||
|
||||
case STOPPED:
|
||||
// Stay stopped
|
||||
stop();
|
||||
break;
|
||||
|
||||
case ERROR:
|
||||
// Stay stopped in error state
|
||||
stop();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Emergency stop.
|
||||
*/
|
||||
public void stop() {
|
||||
leftMotor.setPower(0.0);
|
||||
rightMotor.setPower(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set both motors to the same speed.
|
||||
*/
|
||||
private void setMotors(double power) {
|
||||
leftMotor.setPower(power);
|
||||
rightMotor.setPower(power);
|
||||
}
|
||||
|
||||
// Getters for testing
|
||||
|
||||
public WallApproachState getState() {
|
||||
return state;
|
||||
}
|
||||
|
||||
public boolean hasSensorError() {
|
||||
return sensorError;
|
||||
}
|
||||
|
||||
public double getCurrentDistance() {
|
||||
return sensor.isValid() ? sensor.getDistanceCm() : -1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
package robot.hardware;
|
||||
|
||||
import java.util.Random;
|
||||
|
||||
/**
|
||||
* Mock implementation of DistanceSensor for testing.
|
||||
*
|
||||
* This mock can simulate:
|
||||
* - Setting specific distances
|
||||
* - Sensor noise/jitter
|
||||
* - Sensor failures
|
||||
* - Gradual distance changes (approaching/retreating)
|
||||
*
|
||||
* Example usage in tests:
|
||||
* MockDistanceSensor sensor = new MockDistanceSensor();
|
||||
* sensor.setDistance(50.0); // Robot is 50cm from wall
|
||||
* sensor.addNoise(2.0); // Add ±2cm random noise
|
||||
*/
|
||||
public class MockDistanceSensor implements DistanceSensor {
|
||||
private double distance = 100.0; // Default: far away
|
||||
private double noiseLevel = 0.0; // Standard deviation of noise
|
||||
private boolean valid = true;
|
||||
private Random random = new Random(12345); // Seeded for reproducible tests
|
||||
|
||||
/**
|
||||
* Set the distance reading.
|
||||
* @param distanceCm distance in centimeters
|
||||
*/
|
||||
public void setDistance(double distanceCm) {
|
||||
this.distance = distanceCm;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add Gaussian noise to the readings.
|
||||
* Simulates real-world sensor jitter.
|
||||
*
|
||||
* @param stdDev standard deviation of noise in cm
|
||||
*/
|
||||
public void setNoise(double stdDev) {
|
||||
this.noiseLevel = stdDev;
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate sensor failure/disconnection.
|
||||
*/
|
||||
public void simulateFailure() {
|
||||
this.valid = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Restore sensor to working state.
|
||||
*/
|
||||
public void restore() {
|
||||
this.valid = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate gradual approach (like robot driving toward wall).
|
||||
* @param deltaCm how much closer to get (negative = moving away)
|
||||
*/
|
||||
public void approach(double deltaCm) {
|
||||
this.distance = Math.max(0, this.distance - deltaCm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getDistanceCm() {
|
||||
if (!valid) {
|
||||
return -1; // Error value
|
||||
}
|
||||
|
||||
// Add random noise if configured
|
||||
double noise = 0;
|
||||
if (noiseLevel > 0) {
|
||||
noise = random.nextGaussian() * noiseLevel;
|
||||
}
|
||||
|
||||
return distance + noise;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isValid() {
|
||||
return valid && distance >= 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
package robot.hardware;
|
||||
|
||||
/**
|
||||
* Mock implementation of GyroSensor for testing.
|
||||
*
|
||||
* This mock can simulate:
|
||||
* - Precise heading control for testing turns
|
||||
* - Gyro drift over time (realistic behavior)
|
||||
* - Calibration states
|
||||
* - Rotation simulation
|
||||
*
|
||||
* Example usage:
|
||||
* MockGyroSensor gyro = new MockGyroSensor();
|
||||
* gyro.setHeading(90); // Robot facing 90 degrees
|
||||
* gyro.rotate(45); // Robot turns 45 more degrees
|
||||
*/
|
||||
public class MockGyroSensor implements GyroSensor {
|
||||
private double heading = 0.0;
|
||||
private boolean calibrated = true;
|
||||
private double driftPerSecond = 0.0; // Degrees of drift per second
|
||||
private long lastUpdateTime = System.currentTimeMillis();
|
||||
|
||||
/**
|
||||
* Set the gyro heading directly.
|
||||
* Useful for setting up test scenarios.
|
||||
*
|
||||
* @param degrees heading in degrees (will be normalized to 0-359)
|
||||
*/
|
||||
public void setHeading(double degrees) {
|
||||
this.heading = normalizeHeading(degrees);
|
||||
this.lastUpdateTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate the robot rotating.
|
||||
* Positive = clockwise, negative = counter-clockwise.
|
||||
*
|
||||
* @param degrees how many degrees to rotate
|
||||
*/
|
||||
public void rotate(double degrees) {
|
||||
this.heading = normalizeHeading(this.heading + degrees);
|
||||
this.lastUpdateTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate gyro drift (realistic behavior).
|
||||
* Real gyros drift slightly over time.
|
||||
*
|
||||
* @param degreesPerSecond how much the gyro drifts per second
|
||||
*/
|
||||
public void setDrift(double degreesPerSecond) {
|
||||
this.driftPerSecond = degreesPerSecond;
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate uncalibrated state.
|
||||
*/
|
||||
public void setUncalibrated() {
|
||||
this.calibrated = false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getHeading() {
|
||||
if (!calibrated) {
|
||||
return 0.0; // Return zero if not calibrated
|
||||
}
|
||||
|
||||
// Apply drift based on time elapsed
|
||||
long now = System.currentTimeMillis();
|
||||
double elapsedSeconds = (now - lastUpdateTime) / 1000.0;
|
||||
double drift = driftPerSecond * elapsedSeconds;
|
||||
|
||||
lastUpdateTime = now;
|
||||
heading = normalizeHeading(heading + drift);
|
||||
|
||||
return heading;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void reset() {
|
||||
this.heading = 0.0;
|
||||
this.lastUpdateTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isCalibrated() {
|
||||
return calibrated;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize heading to 0-359 range.
|
||||
*/
|
||||
private double normalizeHeading(double degrees) {
|
||||
while (degrees < 0) degrees += 360;
|
||||
while (degrees >= 360) degrees -= 360;
|
||||
return degrees;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
package robot.hardware;
|
||||
|
||||
/**
|
||||
* Mock implementation of MotorController for testing.
|
||||
* Tracks power settings without requiring actual hardware.
|
||||
*/
|
||||
public class MockMotorController implements MotorController {
|
||||
private double power = 0.0;
|
||||
private int powerSetCount = 0;
|
||||
|
||||
@Override
|
||||
public void setPower(double power) {
|
||||
this.power = power;
|
||||
this.powerSetCount++;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getPower() {
|
||||
return power;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get how many times setPower was called (useful for testing).
|
||||
*/
|
||||
public int getPowerSetCount() {
|
||||
return powerSetCount;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the mock to initial state.
|
||||
*/
|
||||
public void reset() {
|
||||
power = 0.0;
|
||||
powerSetCount = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,310 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.DisplayName;
|
||||
import robot.hardware.*;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
||||
/**
|
||||
* INTEGRATION TEST: Complete autonomous sequence.
|
||||
*
|
||||
* This test demonstrates SYSTEM-LEVEL testing without a robot!
|
||||
*
|
||||
* Scenario:
|
||||
* 1. Start 100cm from wall
|
||||
* 2. Drive straight toward wall
|
||||
* 3. Stop at 10cm from wall
|
||||
* 4. Turn 90° right
|
||||
* 5. Drive forward again
|
||||
* 6. Turn back to original heading
|
||||
*
|
||||
* This tests:
|
||||
* - Multiple subsystems working together
|
||||
* - State transitions
|
||||
* - Sensor coordination
|
||||
* - Complete mission simulation
|
||||
*
|
||||
* ALL WITHOUT A PHYSICAL ROBOT!
|
||||
*/
|
||||
@DisplayName("Autonomous Sequence Integration Test")
|
||||
class AutonomousIntegrationTest {
|
||||
|
||||
// Mock hardware
|
||||
private MockDistanceSensor distanceSensor;
|
||||
private MockGyroSensor gyro;
|
||||
private MockMotorController leftMotor;
|
||||
private MockMotorController rightMotor;
|
||||
|
||||
// Subsystems
|
||||
private WallApproach wallApproach;
|
||||
private TurnController turnController;
|
||||
|
||||
@BeforeEach
|
||||
void setUp() {
|
||||
// Create mock hardware (no FTC SDK needed!)
|
||||
distanceSensor = new MockDistanceSensor();
|
||||
gyro = new MockGyroSensor();
|
||||
leftMotor = new MockMotorController();
|
||||
rightMotor = new MockMotorController();
|
||||
|
||||
// Create subsystems
|
||||
wallApproach = new WallApproach(distanceSensor, leftMotor, rightMotor);
|
||||
turnController = new TurnController(gyro, leftMotor, rightMotor);
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Full autonomous mission simulation")
|
||||
void testCompleteAutonomousMission() {
|
||||
System.out.println("=== Starting Autonomous Mission ===");
|
||||
|
||||
// ========== PHASE 1: Approach Wall ==========
|
||||
System.out.println("\n--- Phase 1: Approaching Wall ---");
|
||||
|
||||
distanceSensor.setDistance(100.0); // Start 100cm away
|
||||
gyro.setHeading(0); // Facing forward
|
||||
|
||||
wallApproach.start();
|
||||
|
||||
int phaseUpdates = 0;
|
||||
while (wallApproach.getState() != WallApproach.WallApproachState.STOPPED && phaseUpdates < 200) {
|
||||
wallApproach.update();
|
||||
|
||||
// Simulate robot actually moving
|
||||
// Movement speed proportional to motor power
|
||||
double moveSpeed = leftMotor.getPower() * 2.0; // 2cm per update at full power
|
||||
distanceSensor.approach(moveSpeed);
|
||||
|
||||
phaseUpdates++;
|
||||
|
||||
// Log state changes
|
||||
if (phaseUpdates % 20 == 0) {
|
||||
System.out.printf(" Update %d: State=%s, Distance=%.1fcm, Power=%.2f\n",
|
||||
phaseUpdates,
|
||||
wallApproach.getState(),
|
||||
distanceSensor.getDistanceCm(),
|
||||
leftMotor.getPower());
|
||||
}
|
||||
}
|
||||
|
||||
// Verify Phase 1 completed successfully
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Phase 1: Should successfully stop at wall");
|
||||
assertTrue(distanceSensor.getDistanceCm() <= 12.0,
|
||||
"Phase 1: Should be close to target distance");
|
||||
System.out.printf("Phase 1 Complete: Stopped at %.1fcm in %d updates\n",
|
||||
distanceSensor.getDistanceCm(), phaseUpdates);
|
||||
|
||||
// ========== PHASE 2: Turn 90° Right ==========
|
||||
System.out.println("\n--- Phase 2: Turning 90° Right ---");
|
||||
|
||||
turnController.turnTo(90);
|
||||
|
||||
phaseUpdates = 0;
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING && phaseUpdates < 200) {
|
||||
turnController.update();
|
||||
|
||||
// Simulate robot actually turning
|
||||
double turnSpeed = leftMotor.getPower() * 2.0; // 2° per update at full power
|
||||
gyro.rotate(turnSpeed);
|
||||
|
||||
phaseUpdates++;
|
||||
|
||||
if (phaseUpdates % 10 == 0) {
|
||||
System.out.printf(" Update %d: Heading=%.1f°, Error=%.1f°, Power=%.2f\n",
|
||||
phaseUpdates,
|
||||
gyro.getHeading(),
|
||||
turnController.getHeadingError(),
|
||||
leftMotor.getPower());
|
||||
}
|
||||
}
|
||||
|
||||
// Verify Phase 2 completed successfully
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState(),
|
||||
"Phase 2: Turn should complete");
|
||||
assertTrue(Math.abs(gyro.getHeading() - 90) <= 2.0,
|
||||
"Phase 2: Should be facing 90°");
|
||||
System.out.printf("Phase 2 Complete: Turned to %.1f° in %d updates\n",
|
||||
gyro.getHeading(), phaseUpdates);
|
||||
|
||||
// ========== PHASE 3: Drive Forward (new direction) ==========
|
||||
System.out.println("\n--- Phase 3: Driving Forward (after turn) ---");
|
||||
|
||||
// Reset distance sensor for new direction
|
||||
distanceSensor.setDistance(80.0);
|
||||
wallApproach.start();
|
||||
|
||||
phaseUpdates = 0;
|
||||
while (wallApproach.getState() != WallApproach.WallApproachState.STOPPED && phaseUpdates < 200) {
|
||||
wallApproach.update();
|
||||
|
||||
double moveSpeed = leftMotor.getPower() * 2.0;
|
||||
distanceSensor.approach(moveSpeed);
|
||||
|
||||
phaseUpdates++;
|
||||
}
|
||||
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Phase 3: Should stop at wall in new direction");
|
||||
System.out.printf("Phase 3 Complete: Stopped at %.1fcm\n",
|
||||
distanceSensor.getDistanceCm());
|
||||
|
||||
// ========== PHASE 4: Turn back to original heading ==========
|
||||
System.out.println("\n--- Phase 4: Returning to Original Heading ---");
|
||||
|
||||
turnController.turnTo(0); // Turn back to 0°
|
||||
|
||||
phaseUpdates = 0;
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING && phaseUpdates < 200) {
|
||||
turnController.update();
|
||||
double turnSpeed = leftMotor.getPower() * 2.0;
|
||||
gyro.rotate(turnSpeed);
|
||||
phaseUpdates++;
|
||||
}
|
||||
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState(),
|
||||
"Phase 4: Should complete return turn");
|
||||
assertTrue(Math.abs(gyro.getHeading()) <= 2.0,
|
||||
"Phase 4: Should be back to original heading");
|
||||
System.out.printf("Phase 4 Complete: Returned to %.1f°\n", gyro.getHeading());
|
||||
|
||||
System.out.println("\n=== Mission Complete! ===");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Mission handles sensor failure gracefully")
|
||||
void testMissionWithSensorFailure() {
|
||||
System.out.println("=== Testing Mission with Sensor Failure ===");
|
||||
|
||||
// Start approaching wall
|
||||
distanceSensor.setDistance(50.0);
|
||||
gyro.setHeading(0);
|
||||
wallApproach.start();
|
||||
|
||||
// Run for a bit
|
||||
for (int i = 0; i < 10; i++) {
|
||||
wallApproach.update();
|
||||
distanceSensor.approach(leftMotor.getPower() * 2.0);
|
||||
}
|
||||
|
||||
// SENSOR FAILS!
|
||||
System.out.println("--- Simulating sensor failure ---");
|
||||
distanceSensor.simulateFailure();
|
||||
wallApproach.update();
|
||||
|
||||
// System should detect failure and stop
|
||||
assertEquals(WallApproach.WallApproachState.ERROR, wallApproach.getState(),
|
||||
"Should enter error state on sensor failure");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Should stop motors on sensor failure");
|
||||
assertTrue(wallApproach.hasSensorError(),
|
||||
"Should report sensor error");
|
||||
|
||||
System.out.println("Mission safely aborted due to sensor failure");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Mission handles unexpected obstacles")
|
||||
void testMissionWithObstacle() {
|
||||
System.out.println("=== Testing Mission with Unexpected Obstacle ===");
|
||||
|
||||
// Start normal approach
|
||||
distanceSensor.setDistance(100.0);
|
||||
wallApproach.start();
|
||||
|
||||
// Robot driving toward wall
|
||||
for (int i = 0; i < 20; i++) {
|
||||
wallApproach.update();
|
||||
distanceSensor.approach(leftMotor.getPower() * 2.0);
|
||||
}
|
||||
|
||||
// UNEXPECTED OBSTACLE APPEARS!
|
||||
System.out.println("--- Obstacle detected! ---");
|
||||
distanceSensor.setDistance(8.0); // Suddenly very close!
|
||||
wallApproach.update();
|
||||
|
||||
// Should immediately stop
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Should immediately stop when obstacle detected");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should stop");
|
||||
|
||||
System.out.println("Emergency stop successful");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Multi-waypoint navigation")
|
||||
void testMultiWaypointNavigation() {
|
||||
System.out.println("=== Testing Multi-Waypoint Navigation ===");
|
||||
|
||||
// Simulate driving to multiple waypoints:
|
||||
// 1. Drive forward, turn 90° right
|
||||
// 2. Drive forward, turn 90° right
|
||||
// 3. Drive forward, turn 90° right
|
||||
// 4. Drive forward, turn 90° right
|
||||
// = Square pattern!
|
||||
|
||||
gyro.setHeading(0);
|
||||
|
||||
for (int waypoint = 1; waypoint <= 4; waypoint++) {
|
||||
System.out.printf("\n--- Waypoint %d ---\n", waypoint);
|
||||
|
||||
// Drive forward
|
||||
distanceSensor.setDistance(50.0);
|
||||
wallApproach.start();
|
||||
|
||||
while (wallApproach.getState() != WallApproach.WallApproachState.STOPPED) {
|
||||
wallApproach.update();
|
||||
distanceSensor.approach(leftMotor.getPower() * 2.0);
|
||||
}
|
||||
|
||||
System.out.printf("Reached waypoint %d\n", waypoint);
|
||||
|
||||
// Turn 90° right
|
||||
double targetHeading = (waypoint * 90) % 360;
|
||||
turnController.turnTo(targetHeading);
|
||||
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING) {
|
||||
turnController.update();
|
||||
gyro.rotate(leftMotor.getPower() * 2.0);
|
||||
}
|
||||
|
||||
System.out.printf("Turned to %.0f°\n", gyro.getHeading());
|
||||
}
|
||||
|
||||
// Should complete the square and face original direction
|
||||
assertTrue(Math.abs(gyro.getHeading()) <= 2.0 ||
|
||||
Math.abs(gyro.getHeading() - 360) <= 2.0,
|
||||
"Should complete square and face original direction");
|
||||
|
||||
System.out.println("\nSquare pattern complete!");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Concurrent sensor updates")
|
||||
void testConcurrentSensorUpdates() {
|
||||
// Test that system handles sensors updating at different rates
|
||||
|
||||
distanceSensor.setDistance(50.0);
|
||||
gyro.setHeading(0);
|
||||
|
||||
wallApproach.start();
|
||||
|
||||
// Simulate 100 updates where sensors might not always have new data
|
||||
for (int i = 0; i < 100; i++) {
|
||||
// Distance sensor updates every cycle
|
||||
wallApproach.update();
|
||||
distanceSensor.approach(leftMotor.getPower() * 1.0);
|
||||
|
||||
// Gyro might update less frequently (every 3 cycles)
|
||||
if (i % 3 == 0) {
|
||||
gyro.rotate(0.1); // Slight drift
|
||||
}
|
||||
|
||||
// System should remain stable
|
||||
assertNotEquals(WallApproach.WallApproachState.ERROR, wallApproach.getState(),
|
||||
"System should remain stable with varying sensor update rates");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,160 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import robot.hardware.MockMotorController;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
||||
/**
|
||||
* Tests for MotorCycler subsystem.
|
||||
* These tests run on the PC without requiring FTC SDK or Android.
|
||||
*/
|
||||
class MotorCyclerTest {
|
||||
private MockMotorController motor;
|
||||
private MotorCycler cycler;
|
||||
|
||||
@BeforeEach
|
||||
void setUp() {
|
||||
motor = new MockMotorController();
|
||||
// Create cycler: 100ms on, 50ms off, 0.75 power
|
||||
cycler = new MotorCycler(motor, 100, 50, 0.75);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInitialization() {
|
||||
cycler.init();
|
||||
assertEquals(0.0, motor.getPower(), 0.001,
|
||||
"Motor should be off after init");
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState(),
|
||||
"Should start in OFF state");
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFirstCycle_TurnsOnAfterOffPeriod() {
|
||||
cycler.init();
|
||||
long startTime = System.currentTimeMillis();
|
||||
|
||||
// Should stay off during the off period
|
||||
cycler.update(startTime + 25);
|
||||
assertEquals(0.0, motor.getPower(), 0.001);
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState());
|
||||
|
||||
// Should turn on after off period completes
|
||||
cycler.update(startTime + 50);
|
||||
assertEquals(0.75, motor.getPower(), 0.001,
|
||||
"Motor should turn on after off period");
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCycleFromOnToOff() {
|
||||
cycler.init();
|
||||
long startTime = System.currentTimeMillis();
|
||||
|
||||
// Skip to motor being on
|
||||
cycler.update(startTime + 50);
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
|
||||
// Should stay on during on period
|
||||
cycler.update(startTime + 100);
|
||||
assertEquals(0.75, motor.getPower(), 0.001);
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
|
||||
// Should turn off after on period completes
|
||||
cycler.update(startTime + 150);
|
||||
assertEquals(0.0, motor.getPower(), 0.001,
|
||||
"Motor should turn off after on period");
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFullCycle() {
|
||||
cycler.init();
|
||||
long time = System.currentTimeMillis();
|
||||
|
||||
// Start OFF
|
||||
cycler.update(time);
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState());
|
||||
assertEquals(0.0, motor.getPower(), 0.001);
|
||||
|
||||
// After 50ms: turn ON
|
||||
time += 50;
|
||||
cycler.update(time);
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
assertEquals(0.75, motor.getPower(), 0.001);
|
||||
|
||||
// After another 100ms: turn OFF
|
||||
time += 100;
|
||||
cycler.update(time);
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState());
|
||||
assertEquals(0.0, motor.getPower(), 0.001);
|
||||
|
||||
// After another 50ms: turn ON again
|
||||
time += 50;
|
||||
cycler.update(time);
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
assertEquals(0.75, motor.getPower(), 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTimeInState() {
|
||||
cycler.init();
|
||||
long startTime = System.currentTimeMillis();
|
||||
|
||||
// Check time in initial OFF state
|
||||
assertEquals(0, cycler.getTimeInState(startTime));
|
||||
assertEquals(25, cycler.getTimeInState(startTime + 25));
|
||||
|
||||
// Transition to ON
|
||||
cycler.update(startTime + 50);
|
||||
assertEquals(0, cycler.getTimeInState(startTime + 50));
|
||||
assertEquals(30, cycler.getTimeInState(startTime + 80));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStop() {
|
||||
cycler.init();
|
||||
long time = System.currentTimeMillis();
|
||||
|
||||
// Get motor running
|
||||
cycler.update(time + 50);
|
||||
assertEquals(MotorCycler.MotorCycleState.ON, cycler.getState());
|
||||
assertEquals(0.75, motor.getPower(), 0.001);
|
||||
|
||||
// Stop should turn off motor and reset to OFF state
|
||||
cycler.stop();
|
||||
assertEquals(MotorCycler.MotorCycleState.OFF, cycler.getState());
|
||||
assertEquals(0.0, motor.getPower(), 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDefaultPower() {
|
||||
// Create cycler with default power
|
||||
MotorCycler defaultCycler = new MotorCycler(motor, 100, 50);
|
||||
defaultCycler.init();
|
||||
long time = System.currentTimeMillis();
|
||||
|
||||
// Skip to ON state
|
||||
defaultCycler.update(time + 50);
|
||||
|
||||
// Should use default power of 0.5
|
||||
assertEquals(0.5, motor.getPower(), 0.001,
|
||||
"Default power should be 0.5");
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMotorControllerIsRunning() {
|
||||
motor.setPower(0.0);
|
||||
assertFalse(motor.isRunning(), "Motor with 0 power should not be running");
|
||||
|
||||
motor.setPower(0.5);
|
||||
assertTrue(motor.isRunning(), "Motor with positive power should be running");
|
||||
|
||||
motor.setPower(-0.3);
|
||||
assertTrue(motor.isRunning(), "Motor with negative power should be running");
|
||||
|
||||
motor.setPower(0.0001);
|
||||
assertFalse(motor.isRunning(), "Motor with tiny power should not be running");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,383 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.DisplayName;
|
||||
import robot.hardware.MockGyroSensor;
|
||||
import robot.hardware.MockMotorController;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
||||
/**
|
||||
* Comprehensive tests for TurnController subsystem.
|
||||
*
|
||||
* Tests cover:
|
||||
* - Basic turn mechanics
|
||||
* - Shortest path selection (clockwise vs counter-clockwise)
|
||||
* - Proportional control behavior
|
||||
* - Gyro drift handling
|
||||
* - 360-degree wraparound cases
|
||||
*/
|
||||
@DisplayName("Turn Controller Subsystem Tests")
|
||||
class TurnControllerTest {
|
||||
|
||||
private MockGyroSensor gyro;
|
||||
private MockMotorController leftMotor;
|
||||
private MockMotorController rightMotor;
|
||||
private TurnController turnController;
|
||||
|
||||
@BeforeEach
|
||||
void setUp() {
|
||||
gyro = new MockGyroSensor();
|
||||
leftMotor = new MockMotorController();
|
||||
rightMotor = new MockMotorController();
|
||||
turnController = new TurnController(gyro, leftMotor, rightMotor);
|
||||
}
|
||||
|
||||
// ========== UNIT TESTS: Basic Functionality ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Initial state is IDLE")
|
||||
void testInitialState() {
|
||||
assertEquals(TurnController.TurnState.IDLE, turnController.getState(),
|
||||
"Turn controller should start in IDLE state");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: turnTo() starts turning")
|
||||
void testTurnToStartsTurning() {
|
||||
turnController.turnTo(90);
|
||||
|
||||
assertEquals(TurnController.TurnState.TURNING, turnController.getState(),
|
||||
"Should enter TURNING state after turnTo()");
|
||||
assertEquals(90, turnController.getTargetHeading(), 0.001,
|
||||
"Target heading should be set");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Completes when within tolerance")
|
||||
void testCompletesWithinTolerance() {
|
||||
gyro.setHeading(88.5); // Very close to 90
|
||||
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
|
||||
// Should complete (within 2 degree tolerance)
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState(),
|
||||
"Should complete when within 2 degrees of target");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should stop when turn complete");
|
||||
}
|
||||
|
||||
// ========== SHORTEST PATH TESTS ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Path: Simple clockwise turn (0° → 90°)")
|
||||
void testSimpleClockwiseTurn() {
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
|
||||
// Should turn right (positive power on left motor)
|
||||
assertTrue(leftMotor.getPower() > 0,
|
||||
"Left motor should be positive for clockwise turn");
|
||||
assertTrue(rightMotor.getPower() < 0,
|
||||
"Right motor should be negative for clockwise turn");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Path: Simple counter-clockwise turn (90° → 0°)")
|
||||
void testSimpleCounterClockwiseTurn() {
|
||||
gyro.setHeading(90);
|
||||
turnController.turnTo(0);
|
||||
turnController.update();
|
||||
|
||||
// Should turn left (negative power on left motor)
|
||||
assertTrue(leftMotor.getPower() < 0,
|
||||
"Left motor should be negative for counter-clockwise turn");
|
||||
assertTrue(rightMotor.getPower() > 0,
|
||||
"Right motor should be positive for counter-clockwise turn");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Path: Wraparound clockwise (350° → 10°)")
|
||||
void testWraparoundClockwise() {
|
||||
// Currently at 350°, want to turn to 10°
|
||||
// Shortest path is clockwise through 0° (20° turn)
|
||||
gyro.setHeading(350);
|
||||
turnController.turnTo(10);
|
||||
|
||||
double error = turnController.getHeadingError();
|
||||
|
||||
// Error should be positive (clockwise)
|
||||
assertTrue(error > 0,
|
||||
"Should choose clockwise path (positive error)");
|
||||
assertEquals(20, error, 0.001,
|
||||
"Shortest path from 350° to 10° is 20° clockwise");
|
||||
|
||||
turnController.update();
|
||||
assertTrue(leftMotor.getPower() > 0,
|
||||
"Should turn clockwise");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Path: Wraparound counter-clockwise (10° → 350°)")
|
||||
void testWraparoundCounterClockwise() {
|
||||
// Currently at 10°, want to turn to 350°
|
||||
// Shortest path is counter-clockwise through 0° (20° turn)
|
||||
gyro.setHeading(10);
|
||||
turnController.turnTo(350);
|
||||
|
||||
double error = turnController.getHeadingError();
|
||||
|
||||
// Error should be negative (counter-clockwise)
|
||||
assertTrue(error < 0,
|
||||
"Should choose counter-clockwise path (negative error)");
|
||||
assertEquals(-20, error, 0.001,
|
||||
"Shortest path from 10° to 350° is 20° counter-clockwise");
|
||||
|
||||
turnController.update();
|
||||
assertTrue(leftMotor.getPower() < 0,
|
||||
"Should turn counter-clockwise");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Path: Exactly opposite heading (180° ambiguous)")
|
||||
void testOppositeHeading() {
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(180);
|
||||
|
||||
double error = turnController.getHeadingError();
|
||||
|
||||
// Either direction is valid, should pick one consistently
|
||||
assertEquals(180, Math.abs(error), 0.001,
|
||||
"180° turn should be exactly 180° either direction");
|
||||
}
|
||||
|
||||
// ========== PROPORTIONAL CONTROL TESTS ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Control: Turn power proportional to error")
|
||||
void testProportionalControl() {
|
||||
// Large error should produce large turn power
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
double largePower = Math.abs(leftMotor.getPower());
|
||||
|
||||
// Small error should produce small turn power
|
||||
leftMotor.setPower(0); // Reset
|
||||
gyro.setHeading(85);
|
||||
turnController.update();
|
||||
double smallPower = Math.abs(leftMotor.getPower());
|
||||
|
||||
assertTrue(largePower > smallPower,
|
||||
"Larger heading error should produce larger turn power");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Control: Minimum power enforced")
|
||||
void testMinimumPower() {
|
||||
// Very small error (but not within tolerance)
|
||||
gyro.setHeading(88); // 2° away, at tolerance threshold
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
|
||||
// If not complete, should use minimum power (0.15)
|
||||
if (turnController.getState() == TurnController.TurnState.TURNING) {
|
||||
assertTrue(Math.abs(leftMotor.getPower()) >= 0.15,
|
||||
"Should enforce minimum turn power to overcome friction");
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Control: Maximum power capped")
|
||||
void testMaximumPower() {
|
||||
// Very large error
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(179); // Almost opposite
|
||||
turnController.update();
|
||||
|
||||
// Power should be capped at 0.5
|
||||
assertTrue(Math.abs(leftMotor.getPower()) <= 0.5,
|
||||
"Turn power should be capped at maximum");
|
||||
}
|
||||
|
||||
// ========== SYSTEM TESTS: Complete Turns ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("System: Complete 90° turn")
|
||||
void testComplete90DegreeTurn() {
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(90);
|
||||
|
||||
// Simulate turning (gyro updates as robot turns)
|
||||
int maxIterations = 200; // Increased from 100
|
||||
int iteration = 0;
|
||||
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING && iteration < maxIterations) {
|
||||
turnController.update();
|
||||
|
||||
// Simulate robot actually turning
|
||||
// Turn speed proportional to motor power
|
||||
double turnSpeed = leftMotor.getPower() * 3.0; // Increased from 2.0 for faster simulation
|
||||
gyro.rotate(turnSpeed);
|
||||
|
||||
iteration++;
|
||||
}
|
||||
|
||||
// Should complete
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState(),
|
||||
"Turn should complete");
|
||||
assertTrue(Math.abs(gyro.getHeading() - 90) <= 2.0,
|
||||
"Should be within 2° of target");
|
||||
assertTrue(iteration < maxIterations,
|
||||
"Should complete in reasonable time");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("System: Complete wraparound turn")
|
||||
void testCompleteWraparoundTurn() {
|
||||
gyro.setHeading(350);
|
||||
turnController.turnTo(10);
|
||||
|
||||
int maxIterations = 100;
|
||||
int iteration = 0;
|
||||
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING && iteration < maxIterations) {
|
||||
turnController.update();
|
||||
double turnSpeed = leftMotor.getPower() * 2.0;
|
||||
gyro.rotate(turnSpeed);
|
||||
iteration++;
|
||||
}
|
||||
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState());
|
||||
|
||||
// Should be at ~10°
|
||||
double finalHeading = gyro.getHeading();
|
||||
assertTrue(Math.abs(finalHeading - 10) <= 2.0,
|
||||
"Should complete wraparound turn accurately");
|
||||
}
|
||||
|
||||
// ========== EDGE CASE TESTS ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Handles uncalibrated gyro")
|
||||
void testUncalibratedGyro() {
|
||||
gyro.setUncalibrated();
|
||||
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
|
||||
// Should stop and return to idle
|
||||
assertEquals(TurnController.TurnState.IDLE, turnController.getState(),
|
||||
"Should not turn with uncalibrated gyro");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should stop with uncalibrated gyro");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Handles gyro drift during turn")
|
||||
void testGyrodrift() {
|
||||
gyro.setHeading(0);
|
||||
gyro.setDrift(0.5); // 0.5° per second drift
|
||||
|
||||
turnController.turnTo(90);
|
||||
|
||||
// Simulate turn with drift
|
||||
for (int i = 0; i < 50; i++) {
|
||||
turnController.update();
|
||||
double turnSpeed = leftMotor.getPower() * 2.0;
|
||||
gyro.rotate(turnSpeed);
|
||||
|
||||
// Drift adds a bit each update
|
||||
try {
|
||||
Thread.sleep(10); // 10ms per update
|
||||
} catch (InterruptedException e) {
|
||||
// Ignore
|
||||
}
|
||||
}
|
||||
|
||||
// Should still complete despite drift
|
||||
// (Controller will compensate for drift)
|
||||
assertTrue(turnController.getState() == TurnController.TurnState.COMPLETE ||
|
||||
turnController.getState() == TurnController.TurnState.TURNING,
|
||||
"Should handle drift gracefully");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Multiple turns in sequence")
|
||||
void testSequentialTurns() {
|
||||
gyro.setHeading(0);
|
||||
|
||||
// First turn: 0 → 90
|
||||
turnController.turnTo(90);
|
||||
simulateTurn();
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState());
|
||||
|
||||
// Second turn: 90 → 180
|
||||
turnController.turnTo(180);
|
||||
simulateTurn();
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState());
|
||||
|
||||
// Third turn: 180 → 0 (shortest is through 270)
|
||||
turnController.turnTo(0);
|
||||
simulateTurn();
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState());
|
||||
|
||||
assertTrue(Math.abs(gyro.getHeading()) <= 2.0,
|
||||
"Should complete all turns accurately");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Manual stop during turn")
|
||||
void testManualStopDuringTurn() {
|
||||
gyro.setHeading(0);
|
||||
turnController.turnTo(90);
|
||||
turnController.update();
|
||||
|
||||
// Motors should be running
|
||||
assertTrue(Math.abs(leftMotor.getPower()) > 0);
|
||||
|
||||
// Manual stop
|
||||
turnController.stop();
|
||||
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Stop should immediately halt motors");
|
||||
assertEquals(0.0, rightMotor.getPower(), 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Turn to current heading (no-op)")
|
||||
void testTurnToCurrentHeading() {
|
||||
gyro.setHeading(45);
|
||||
turnController.turnTo(45);
|
||||
turnController.update();
|
||||
|
||||
// Should immediately complete
|
||||
assertEquals(TurnController.TurnState.COMPLETE, turnController.getState(),
|
||||
"Turning to current heading should immediately complete");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"No motor power needed");
|
||||
}
|
||||
|
||||
// ========== HELPER METHODS ==========
|
||||
|
||||
/**
|
||||
* Helper to simulate a turn completing.
|
||||
*/
|
||||
private void simulateTurn() {
|
||||
int maxIterations = 300; // Increased from 200
|
||||
int iteration = 0;
|
||||
|
||||
while (turnController.getState() == TurnController.TurnState.TURNING && iteration < maxIterations) {
|
||||
turnController.update();
|
||||
double turnSpeed = leftMotor.getPower() * 3.0; // Match testComplete90DegreeTurn
|
||||
gyro.rotate(turnSpeed);
|
||||
iteration++;
|
||||
}
|
||||
|
||||
assertTrue(iteration < maxIterations,
|
||||
"Turn should complete in reasonable time");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,352 @@
|
||||
package robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.DisplayName;
|
||||
import robot.hardware.MockDistanceSensor;
|
||||
import robot.hardware.MockMotorController;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
||||
/**
|
||||
* Comprehensive tests for WallApproach subsystem.
|
||||
*
|
||||
* These tests demonstrate:
|
||||
* - Unit testing (individual behaviors)
|
||||
* - System testing (complete scenarios)
|
||||
* - Edge case testing (sensor failures, noise)
|
||||
* - State machine testing
|
||||
*
|
||||
* All tests run on Windows JRE - no robot needed!
|
||||
*/
|
||||
@DisplayName("Wall Approach Subsystem Tests")
|
||||
class WallApproachTest {
|
||||
|
||||
private MockDistanceSensor sensor;
|
||||
private MockMotorController leftMotor;
|
||||
private MockMotorController rightMotor;
|
||||
private WallApproach wallApproach;
|
||||
|
||||
@BeforeEach
|
||||
void setUp() {
|
||||
// Create mock hardware (no FTC SDK needed!)
|
||||
sensor = new MockDistanceSensor();
|
||||
leftMotor = new MockMotorController();
|
||||
rightMotor = new MockMotorController();
|
||||
|
||||
// Create the subsystem we're testing
|
||||
wallApproach = new WallApproach(sensor, leftMotor, rightMotor);
|
||||
}
|
||||
|
||||
// ========== UNIT TESTS: Individual Behaviors ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Initial state should be INIT")
|
||||
void testInitialState() {
|
||||
assertEquals(WallApproach.WallApproachState.INIT, wallApproach.getState(),
|
||||
"Wall approach should start in INIT state");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Starting approach transitions to APPROACHING state")
|
||||
void testStartTransition() {
|
||||
sensor.setDistance(100.0); // Far from wall
|
||||
|
||||
wallApproach.start();
|
||||
|
||||
assertEquals(WallApproach.WallApproachState.APPROACHING, wallApproach.getState(),
|
||||
"After start(), should be in APPROACHING state");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Drives at full speed when far from wall")
|
||||
void testFullSpeedWhenFar() {
|
||||
sensor.setDistance(100.0); // 100cm = far away
|
||||
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Should drive at full speed (0.6)
|
||||
assertEquals(0.6, leftMotor.getPower(), 0.001,
|
||||
"Left motor should be at full speed when far from wall");
|
||||
assertEquals(0.6, rightMotor.getPower(), 0.001,
|
||||
"Right motor should be at full speed when far from wall");
|
||||
assertEquals(WallApproach.WallApproachState.APPROACHING, wallApproach.getState());
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Slows down when approaching threshold")
|
||||
void testSlowsDownNearWall() {
|
||||
sensor.setDistance(25.0); // 25cm = within slow zone (< 30cm)
|
||||
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Should slow down to 0.2
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001,
|
||||
"Should slow to 0.2 when within 30cm of wall");
|
||||
assertEquals(0.2, rightMotor.getPower(), 0.001);
|
||||
assertEquals(WallApproach.WallApproachState.SLOWING, wallApproach.getState());
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Unit: Stops at target distance")
|
||||
void testStopsAtTarget() {
|
||||
sensor.setDistance(10.0); // Exactly at stop distance
|
||||
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Should stop
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Should stop when reaching target distance");
|
||||
assertEquals(0.0, rightMotor.getPower(), 0.001);
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState());
|
||||
}
|
||||
|
||||
// ========== SYSTEM TESTS: Complete Scenarios ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("System: Complete approach from far to stopped")
|
||||
void testCompleteApproachSequence() {
|
||||
// Start far away
|
||||
sensor.setDistance(100.0);
|
||||
wallApproach.start();
|
||||
|
||||
// Phase 1: Fast approach (100cm → 35cm)
|
||||
for (int i = 0; i < 13; i++) { // 13 updates at 5cm each
|
||||
wallApproach.update();
|
||||
assertEquals(0.6, leftMotor.getPower(), 0.001,
|
||||
"Should maintain full speed while far");
|
||||
sensor.approach(5.0); // Get 5cm closer each update
|
||||
}
|
||||
|
||||
// Now at ~35cm - one more update should trigger slowing
|
||||
wallApproach.update();
|
||||
sensor.approach(5.0); // Now at 30cm
|
||||
|
||||
// Phase 2: Slow approach (30cm → 10cm)
|
||||
wallApproach.update();
|
||||
assertEquals(WallApproach.WallApproachState.SLOWING, wallApproach.getState(),
|
||||
"Should be slowing when distance < 30cm");
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001,
|
||||
"Should be at slow speed");
|
||||
|
||||
for (int i = 0; i < 4; i++) { // 4 updates at 5cm each
|
||||
wallApproach.update();
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001);
|
||||
sensor.approach(5.0);
|
||||
}
|
||||
|
||||
// Phase 3: Stop at target
|
||||
wallApproach.update();
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Should stop at 10cm");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should be stopped");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("System: Handles sensor noise gracefully")
|
||||
void testHandlesSensorNoise() {
|
||||
sensor.setDistance(50.0);
|
||||
sensor.setNoise(2.0); // Add ±2cm noise
|
||||
|
||||
wallApproach.start();
|
||||
|
||||
// Run 20 updates with noisy sensor
|
||||
for (int i = 0; i < 20; i++) {
|
||||
wallApproach.update();
|
||||
sensor.approach(0.5); // Get slightly closer each time
|
||||
|
||||
// Should not crash or behave erratically
|
||||
assertTrue(leftMotor.getPower() >= 0,
|
||||
"Motor power should never be negative");
|
||||
assertTrue(leftMotor.getPower() <= 1.0,
|
||||
"Motor power should never exceed 1.0");
|
||||
}
|
||||
|
||||
// Should still be in a valid state
|
||||
assertNotEquals(WallApproach.WallApproachState.ERROR, wallApproach.getState(),
|
||||
"Should not enter error state with valid noisy sensor");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("System: Emergency stop if too close initially")
|
||||
void testEmergencyStopIfTooClose() {
|
||||
// Robot is already too close!
|
||||
sensor.setDistance(5.0);
|
||||
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Should immediately stop
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Should immediately stop if already too close");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001);
|
||||
assertEquals(0.0, rightMotor.getPower(), 0.001);
|
||||
}
|
||||
|
||||
// ========== EDGE CASE TESTS ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Handles sensor failure")
|
||||
void testSensorFailureHandling() {
|
||||
sensor.setDistance(50.0);
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Motors should be running
|
||||
assertTrue(leftMotor.getPower() > 0);
|
||||
|
||||
// Sensor fails!
|
||||
sensor.simulateFailure();
|
||||
wallApproach.update();
|
||||
|
||||
// Should enter error state and stop
|
||||
assertEquals(WallApproach.WallApproachState.ERROR, wallApproach.getState(),
|
||||
"Should enter ERROR state on sensor failure");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Should stop motors on sensor failure");
|
||||
assertTrue(wallApproach.hasSensorError(),
|
||||
"Should report sensor error");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Recovers if pushed backward")
|
||||
void testRecoveryFromBackwardMotion() {
|
||||
// Start in slow zone
|
||||
sensor.setDistance(25.0);
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
assertEquals(WallApproach.WallApproachState.SLOWING, wallApproach.getState());
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001);
|
||||
|
||||
// Robot gets pushed backward (human intervention, etc.)
|
||||
sensor.setDistance(35.0);
|
||||
wallApproach.update();
|
||||
|
||||
// Should speed up again
|
||||
assertEquals(WallApproach.WallApproachState.APPROACHING, wallApproach.getState(),
|
||||
"Should transition back to APPROACHING if pushed back");
|
||||
assertEquals(0.6, leftMotor.getPower(), 0.001,
|
||||
"Should speed up when far again");
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Stays stopped once reached")
|
||||
void testStaysStoppedOnceReached() {
|
||||
sensor.setDistance(10.0);
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Should be stopped
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState());
|
||||
|
||||
// Run multiple more updates - should stay stopped
|
||||
for (int i = 0; i < 10; i++) {
|
||||
wallApproach.update();
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Should remain stopped");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should stay off");
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Manual stop works in any state")
|
||||
void testManualStop() {
|
||||
sensor.setDistance(50.0);
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
|
||||
// Motors running
|
||||
assertTrue(leftMotor.getPower() > 0);
|
||||
|
||||
// Manual stop
|
||||
wallApproach.stop();
|
||||
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Stop should immediately halt motors");
|
||||
assertEquals(0.0, rightMotor.getPower(), 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
@DisplayName("Edge: Threshold boundary behavior")
|
||||
void testThresholdBoundaries() {
|
||||
// Test exact boundary values
|
||||
|
||||
// Just above slow threshold (30cm)
|
||||
sensor.setDistance(30.1);
|
||||
wallApproach.start();
|
||||
wallApproach.update();
|
||||
assertEquals(0.6, leftMotor.getPower(), 0.001,
|
||||
"At 30.1cm should still be full speed");
|
||||
|
||||
// Just below slow threshold
|
||||
sensor.setDistance(29.9);
|
||||
wallApproach.update();
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001,
|
||||
"At 29.9cm should be slow speed");
|
||||
|
||||
// Just above stop threshold (10cm)
|
||||
sensor.setDistance(10.1);
|
||||
wallApproach.update();
|
||||
assertEquals(0.2, leftMotor.getPower(), 0.001,
|
||||
"At 10.1cm should still be moving slowly");
|
||||
|
||||
// Just below stop threshold
|
||||
sensor.setDistance(9.9);
|
||||
wallApproach.update();
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"At 9.9cm should be stopped");
|
||||
}
|
||||
|
||||
// ========== INTEGRATION TEST ==========
|
||||
|
||||
@Test
|
||||
@DisplayName("Integration: Full realistic approach with noise and variance")
|
||||
void testRealisticApproachScenario() {
|
||||
// Simulate a realistic approach with:
|
||||
// - Sensor noise
|
||||
// - Non-uniform distance changes
|
||||
// - Multiple updates per cm traveled
|
||||
|
||||
sensor.setDistance(80.0);
|
||||
sensor.setNoise(1.5); // Realistic noise level
|
||||
wallApproach.start();
|
||||
|
||||
int updateCount = 0;
|
||||
WallApproach.WallApproachState lastState = wallApproach.getState();
|
||||
|
||||
// Simulate approach with varying speeds
|
||||
while (sensor.getDistanceCm() > 10.0 && updateCount < 300) { // Increased from 200
|
||||
wallApproach.update();
|
||||
updateCount++;
|
||||
|
||||
// Approach speed varies (not constant)
|
||||
if (sensor.getDistanceCm() > 30) {
|
||||
sensor.approach(1.5); // Reduced from 2.0 for more realistic simulation
|
||||
} else {
|
||||
sensor.approach(0.3); // Reduced from 0.5 for slower approach
|
||||
}
|
||||
|
||||
// Track state transitions
|
||||
WallApproach.WallApproachState currentState = wallApproach.getState();
|
||||
if (currentState != lastState) {
|
||||
System.out.println("State transition: " + lastState + " → " + currentState +
|
||||
" at " + String.format("%.1f", sensor.getDistanceCm()) + "cm");
|
||||
lastState = currentState;
|
||||
}
|
||||
}
|
||||
|
||||
// Should have completed successfully
|
||||
assertEquals(WallApproach.WallApproachState.STOPPED, wallApproach.getState(),
|
||||
"Should successfully stop at target distance");
|
||||
assertTrue(sensor.getDistanceCm() <= 11.0,
|
||||
"Should stop very close to target (within noise tolerance)");
|
||||
assertEquals(0.0, leftMotor.getPower(), 0.001,
|
||||
"Motors should be stopped at end");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user