Writing FTC Robot Code without a Robot

For many FTC teams, the 2020/2021 Ultimate Goal season is going to require us to invent new ways of working together. Our team’s plan is to work remotely when we can. Software development lends itself to that model, especially when the right tools are available. For FTC teams, this does present some challenges with how much code can be developed before you need a robot, but we’ve found a way around this issue.

Over the past few seasons, I’ve been encouraging our software team to use unit tests to improve the quality of their code, and more interestingly write code and test that it works before the hardware on the robot is available to test. I’ve also been able to use unit tests as an effective teaching tool for some workshops I’ve been delivering via Zoom to new team members over the summer.

As an off-season project two summers ago, the team built software-only “fake” implementations of key FTC hardware, such as motors, servos, and sensors. Fakes are created by simply taking the FTC SDK interface for something like a DcMotorEx, and filling out the methods used by your code, for example setPower() and getPower() to take input values from the code being tested, and return those same values to the test itself to make assertions about expected behavior:

public class FakeDcMotorEx implements DcMotorEx {
    private boolean motorEnable = true;

    private Map<RunMode, PIDCoefficients> pidCoefficients = new HashMap<>();

    private Map<RunMode, PIDFCoefficients> pidfCoefficients = new HashMap<>();

    private double motorPower;

    private int currentEncoderPosition;

    private int targetPositionTolerance;

    // .... (other methods left out for clarity)

    @Override
    public void setPower(double power) {
        this.motorPower = power;
    }

    @Override
    public double getPower() {
        return motorPower;
    }
}

By then structuring their robot code into classes that model the key components of the robot, they are able to simulate the behavior of these components using this fake hardware and make assertions about their code before the mechanisms on their robot physically exist.

The first big win for this approach was during the team’s Rover Ruckus Season. The build of the elevator lift-based scoring mechanism was done in parallel with the software team working on the code to end up with a highly-automated control system that required only one meeting’s worth of time to integrate and test.

TNT’s Rover Ruckus Particle Scoring State Machine

Building out the fake implementations is a good educational project for your team. However, if you’d like to get started with unit tests right away, The Tech Ninja Team publishes their “FTC Core Library” which is directly usable from Android Studio, and includes fake implementations of DcMotors, Servos, DigitalIO (limit switches) and DistanceSensors in their “fakeHardware” library. You can get more information about the TNT FTC Core Library at https://github.com/HF-Robotics/TntFtcCore

Once the team had working fake implementations of FTC hardware, they had to evolve the structure of their programs to make it possible to test. Our journey happened over three stages and multiple seasons – but it’s possible to jump directly to the last stage to take advantage of this pattern – which would also enable FTC teams taking this approach to do some meaningful software development while working remotely.

Stage 1 – Just like the FTC Pushbot Example

The first style of coding that almost every FTC team starts with is a Pushbot style op-mode, with everything all in the loop() method, or for autonomous, everything directly a single method in a linear opmode. Our first season’s code was essentially “Pushbot extended” as we learned the ins and outs of the FTC SDK and how it interacted with the hardware. This example is from the FTC SDK itself:

   /*
    * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
    */
    @Override
    public void loop() {
        // Setup a variable for each drive wheel to save power level for telemetry
        double leftPower;
        double rightPower;

        // Choose to drive using either Tank Mode, or POV Mode
        // Comment out the method that's not used.  The default below is POV.

        // POV Mode uses left stick to go forward, and right stick to turn.
        // - This uses basic math to combine motions and is easier to drive straight.
        double drive = -gamepad1.left_stick_y;
        double turn  =  gamepad1.right_stick_x;
        leftPower    = Range.clip(drive + turn, -1.0, 1.0) ;
        rightPower   = Range.clip(drive - turn, -1.0, 1.0) ;

        // Tank Mode uses one stick to control each wheel.
        // - This requires no math, but it is hard to drive forward slowly and keep straight.
        // leftPower  = -gamepad1.left_stick_y ;
        // rightPower = -gamepad1.right_stick_y ;

        // Send calculated power to wheels
        leftDrive.setPower(leftPower);
        rightDrive.setPower(rightPower);

        // Show the elapsed game time and wheel power.
        telemetry.addData("Status", "Run Time: " + runtime.toString());
        telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
    }

Stage 2 – Isolating Code for Each Subsystem To Its Own Method

In this stage, I had the software team move code for each “function” of the robot – driving, end-effectors and manipulators into separate methods. This also made it easier to change code for one function, without breaking the others. This turned out to be especially helpful when the team needed to make changes during competitions, as it provided some safety, preventing unintended bugs being introduced to working code during stressful circumstances. Here is some tele-op code from their robot for Velocity Vortex that demonstrates this structure:

    /**
     * Implement a state machine that controls the robot during
     * manual-operation.  The state machine uses gamepad input to transition
     * between states.
     * <p>
     * The system calls this member repeatedly while the OpMode is running.
     */
    @Override
    public void loop()

    {
        handleDrive();
        handleCollector();
        handleParticleShooter();
        handleBallGrabber();
        handleLift();
        updateGamepadTelemetry();
    }

    private void handleBallGrabber() {
        ballGrabberStateMachine.doOneStateLoop();
    }

    /**
     * Runs the state machine for the particle shooter
     */
    private void handleParticleShooter() {
        // FIXME Figure out how to make these work together
        //if (shooterTrigger.isPressed()) {
        //    shooterOn();
        //} else {
        //    shooterOff();
        //}
        particleShooterStateMachine.doOneStateLoop();
    }

    /**
     * Runs the state machine for the collector mechanism
     */
    private void handleCollector() {
        collectorToggleState = collectorToggleState.doStuffAndGetNextState();
        collectorReverseToggleState = collectorReverseToggleState.doStuffAndGetNextState();
    }

Notice that the loop() method is just calling other methods that handle the various subsystems in the robot. The team’s code for this season was their first attempt at automating some common tasks, so many of the “handle” methods are just turning around and calling into per-subsystem state machines to do something.

What is nice about this structure is that changes in the code for one subsystem, are isolated from others. This structure could also be used to let multiple students work on different functions of the robot relatively independently.

Stage 3 – Modeling the mechanical subsystems of the robot as classes

During Rover Ruckus, the team started to model the mechanical subsystems of the robot as classes that own all of their hardware and the logic that makes it work. These subsystems are things like the drive base, intakes, scoring mechanisms, etc.

Once the code is structured in this way, it’s possible to write code for these mechanisms, without having access to the physical robot for testing, through the use of fakes and mocks and unit testing.

The following is code from the Skystone season that managed the capstone mechanism on the robot. The TNT software team likes to build in various safeties to protect the robot from mechanical failure and prevent human errors.

The mechanical design of the capstone mechanism was such that if the capstone was released before end-game, the robot would not have been able to intake stones, and would be unable to score points. The team added code that would not allow the capstone to be released until 70 seconds into the match. They also added code that would prevent the capstone mechanism from being released if the robot did not have a stone loaded into the robot’s gripper mechanism. These were key safety concepts the software team wanted to make sure worked correctly, which they were able to do through unit tests.

public class CapstoneMechanism {
    private Servo capstoneServo;

    private Servo fingerServo;

    @Setter
    private OnOffButton unsafeButton;

    @Setter
    private DebouncedButton dropButton;

    private final Stopwatch elapsedTimer;

    private final Telemetry telemetry;

    public final long THRESHOLD_FOR_DROP_OKAY_MILLIS = TimeUnit.SECONDS.toMillis(70);

    public static final double HOLDING_POSITION = 0;

    public static final double DROPPING_POSITION = .366;

    public boolean isHolding = true;

    public CapstoneMechanism(@NonNull SimplerHardwareMap hardwareMap,
                             @NonNull Telemetry telemetry,
                             @NonNull Ticker ticker) {
        try {
            this.capstoneServo = hardwareMap.get(Servo.class, "capstoneServo");
            this.fingerServo = hardwareMap.get(Servo.class, "fingerServo");
        } catch (Throwable t) {
            Log.e(LOG_TAG, "Missing hardware - capstone or finger servo. Disabling capstone mechanism");
        }

        this.telemetry = telemetry;

        this.elapsedTimer = Stopwatch.createUnstarted(ticker);

        this.capstoneServo.setPosition(HOLDING_POSITION);
    }

    public void periodicTask() {
        if (capstoneServo == null || fingerServo == null) {
            return;
        }

        if ((elapsedTimer.elapsed(TimeUnit.MILLISECONDS) < THRESHOLD_FOR_DROP_OKAY_MILLIS)) {
            if (unsafeButton != null && !unsafeButton.isPressed()) {
                telemetry.addData("Capstone", "not endgame (use unsafe)");
            } else {
                telemetry.addData("Capstone", "unsafed");
            }
        } else {
            if (unsafeButton != null && !unsafeButton.isPressed()) {
                if (Math.abs(fingerServo.getPosition() - DeliveryMechanism.FINGER_GRIP) < .01) {
                    telemetry.addData("Capstone", "free");
                } else {
                    telemetry.addData("Capstone", "not gripped (use unsafe)");
                }
            } else {
                telemetry.addData("Capstone", "free");
            }
        }

        if (!elapsedTimer.isRunning()) {
            elapsedTimer.start();
        }

        if (dropButton != null && dropButton.getRise() && okayToDrop()) {
            isHolding = !isHolding;
        }

        if (isHolding) {
            capstoneServo.setPosition(HOLDING_POSITION);

        } else {
            capstoneServo.setPosition(DROPPING_POSITION);

        }
    }

    private boolean okayToDrop() {

        if (unsafeButton != null && unsafeButton.isPressed())
        {
            return true;
        }

        if ((elapsedTimer.elapsed(TimeUnit.MILLISECONDS)> THRESHOLD_FOR_DROP_OKAY_MILLIS)&&
                (Math.abs(fingerServo.getPosition() - DeliveryMechanism.FINGER_GRIP) < .01))
        {
            return true;
        }

        return false;
    }
}

Our team uses JUnit to run their unit tests. If you’d like to follow in their footsteps, there’s some built-in support for this in Android Studio, however, you’ll need to add the dependency to your TeamCode’s build.gradle file:

dependencies {
    testImplementation 'junit:junit:4.12'
}

and then add a directory hierarchy “test/java” rooted in your TeamCode “src” folder, where you will need to add your unit test classes so that Android Studio will automatically recognize them as test code:

The following snippet from the team’s Skystone code (https://github.com/HF-Robotics/SkyStone/tree/master/TeamCode/src/main/java/com/hfrobots/tnt/season1920) shows how the unit test is setup, with fake servos, fake game controller buttons, and the subsystem under test, the CapstoneMechanism. A FakeTicker is also used to advance the clock arbitrarily since some of the functionality to be tested is time-based.

public class CapstoneMechanismTest {
    private FakeHardwareMap hardwareMap;

    private FakeServo fingerServo;

    private FakeServo capstoneServo;

    private FakeOnOffButton unsafeButton;

    private FakeOnOffButton dropButton;

    private FakeTicker ticker;

    private CapstoneMechanism capstoneMechanism;

    @Before
    public void setUp() {
        ticker = new FakeTicker();
        ticker.setAutoIncrementStep(5, TimeUnit.SECONDS);

        hardwareMap = new FakeHardwareMap();

        fingerServo = new FakeServo();
        capstoneServo = new FakeServo();

        dropButton = new FakeOnOffButton();
        unsafeButton = new FakeOnOffButton();

        hardwareMap.addDevice("fingerServo", fingerServo);
        hardwareMap.addDevice("capstoneServo", capstoneServo);

        FakeTelemetry telemetry = new FakeTelemetry();

        capstoneMechanism = new CapstoneMechanism(hardwareMap, telemetry,
                ticker);
        capstoneMechanism.setUnsafeButton(unsafeButton);
        capstoneMechanism.setDropButton(new DebouncedButton(dropButton));
    }
    
    // ....  
}

The key part to look at is the setUp() method – especially towards the end, where a new CapstoneMechanism is created using the fake hardware implementations.

Here is how those “soft safeties” are tested:

    @Test
    public void testSafeties() {
        // Scenario 1 - there is no stone gripped, the capstone 
        // mechanism should be in the "holding" position

        fingerServo.setPosition(DeliveryMechanism.FINGER_UNGRIP);

        Assert.assertEquals(CapstoneMechanism.HOLDING_POSITION, capstoneServo.getPosition(), 0.001);
        Assert.assertEquals(DeliveryMechanism.FINGER_UNGRIP, fingerServo.getPosition(), 0.001);

        capstoneMechanism.periodicTask();
        
        // Scenarios 2 - Attempt to ask drop the capstone (which shouldn't be possible)

        dropButton.setPressed(true);

        capstoneMechanism.periodicTask();

        Assert.assertEquals(CapstoneMechanism.HOLDING_POSITION, capstoneServo.getPosition(), .001);

        dropButton.setPressed(false);
        capstoneMechanism.periodicTask();

        // Scenario 3 - Now, grip a stone, and see if we can drop 
        // the capstone on top of the stone, which shouldn't happen since
        // not enough time has passed

        fingerServo.setPosition(DeliveryMechanism.FINGER_GRIP);
        dropButton.setPressed(true);
        capstoneMechanism.periodicTask();

        Assert.assertEquals(CapstoneMechanism.HOLDING_POSITION, capstoneServo.getPosition(), .001);

        dropButton.setPressed(false);
        capstoneMechanism.periodicTask();

        // Scenario 3 - Move the elapsed time into end-game territory....
        ticker.advance(75, TimeUnit.SECONDS);
        dropButton.setPressed(true);
        capstoneMechanism.periodicTask();

        // Now, we should be able to drop the capstone
        Assert.assertEquals(CapstoneMechanism.DROPPING_POSITION, capstoneServo.getPosition(), .001);

        dropButton.setPressed(false);
        capstoneMechanism.periodicTask();

        dropButton.setPressed(true);
        capstoneMechanism.periodicTask();

        Assert.assertEquals(CapstoneMechanism.HOLDING_POSITION, capstoneServo.getPosition(), .001);
    }

This test shows another typical pattern of ours. While the software team puts in “soft safeties” to prevent human error, they also always give an “escape hatch” to our robot operators, usually called “The Unsafe Button”, which allows the safeties to be ignored in unanticipated circumstances. The following test shows how this functionality is tested:

    @Test
    public void testUnsafeButton() {
        fingerServo.setPosition(DeliveryMechanism.FINGER_UNGRIP);

        Assert.assertEquals(CapstoneMechanism.HOLDING_POSITION, capstoneServo.getPosition(), 0.001);
        Assert.assertEquals(DeliveryMechanism.FINGER_UNGRIP, fingerServo.getPosition(), 0.001);

        capstoneMechanism.periodicTask();
        
        // "Press" the drop button, and the "unsafe" button, we should
        // be able to drop the capstone once this has happened
        dropButton.setPressed(true);
        unsafeButton.setPressed(true);

        capstoneMechanism.periodicTask();

        Assert.assertEquals(CapstoneMechanism.DROPPING_POSITION, capstoneServo.getPosition(), .001);
    }

How Might This Play Out for Tech Ninja Team’s Ultimate Goal Season?

Over the summer, we’ve had quite a bit of interest in the software development roles for our upcoming Ultimate Goal season. Modeling our robot as subsystems and having good unit tests should make it possible to keep the team engaged in writing code while working from home.

We already know that when it comes time to integrate with the physical robot, we’ve had much fewer issues with unit-tested code.

I hope that I’ve given your team some ideas about how to work more efficiently on your robot code, and TNT’s software team hopes you check out the TNT FTC Core Library at https://github.com/HF-Robotics/TntFtcCore, which does have more “match-tested” useful components in addition to the unit test support of fake hardware!

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

%d bloggers like this:
search previous next tag category expand menu location phone mail time cart zoom edit close