Software

Foundation
With this year's robot, our team needed a codebase that could efficiently expand with the
multiple different robot hardware systems and sensor inputs we had.

The structure that we decided to use was the subsystem programming model. In this
programming model, each isolated system of our robot is split up into its own class in our
codebase.

Name Functionality

Intake Controls our extension, our coaxial virtual four bar, claw, and wrist rotation.

Outtake Controls our vertical lift, the arm end effector, and claw.

Drivetrain Controls all four drivetrain motors, robot mecanum movement logic, and
heading sensor inputs.

Each one of these subsystems are based on a template Subsystem interface, and is registered
in a pool of other subsystems within the main robot program.

An example Subsystem may look like this:

class MySubsystem(val opMode: LinearOpMode) : Subsystem

{
fun onInitialize()
{
// the hardware devices are initialized here
}

fun onClose()
{
// the program ends here
}
}

Standardization
Having a standardized subsystem model (as described above) for our codebase allows us to
write more robust code, more efficiently. Instead of re-writing boilerplate code for each new
robot program we wish to make, programmers are able to use a well-defined set of functions
that the subsystems expose to the rest of the codebase.

Each subsystem also has its own set of standardized states. This allows us to track where
exactly any part of our robot might be at any time, and allows us to apply strict limitations for
where a subsystem may be able to go at any specific time:

enum class RotationState

{
Transfer, Rest, Deposit
}

Having these well-defined states also prevents confusion in the codebase, and allows us to
onboard new programmers quickly.

This past month, we have been able to move our sister team "Bad Wolf, team 6980" to our
well-defined codebase. It has improved their productivity by a lot, and has enabled them to
do much more with their existing robot.

Putting this all together, we can see now that instead of repeating a series of multiple fine-tuned
hardware updates everywhere in the program, a standard function can be used:

// How it was done before:

outtakeRotationLeft.position = 0.53
outtakeRotationRight.position = 0.38
Thread.sleep(1000L)

outtakeClaw.position = 0.2

// How it is done now:

class Outtake : Subsystem
{
// ...
fun depositSample()
{
if (intake.state == OuttakeState.Active)
{
// Apply a strict limitation, preventing
// the outtake from activating when the
// intake is active
return
 }
rotateOuttake(RotationState.Deposit)
Thread.sleep(1000L)

setClaw(ClawState.Open)
}
// ...
}

Hardware Abstraction
Within our subsystems and well-defined state functions, our team often uses a set of shared
algorithms and logic for our hardware devices (Servos, Motors, Sensors). For ease of use, our
team has moved these logic and algorithm implementations to "wrapper" classes on top of
hardware devices.

Over this past year, our team has implemented several different algorithms into our codebase:

PDF Controller: Used in favor of FTC's motor RUN_TO_POSITION system, which has a slow
refresh rate of only 20hz.
Allows for a dynamic feedforward constant, based on the motor's current position and
velocity.
Runs at a refresh rate of 70hz, resulting in more smooth movement and rapid response
to environmental changes.
Used on our vertical and horizontal slide systems.
Stuck Protection: An intelligence feature equipped on our motors that uses the motor's
current velocity to detect and mitigate scenarios where an external object may be blocking
the motor from operating.
At a high level, this algorithm requires the motor's velocity to be greater than an
arbitrary threshold while the motor is "travelling".
Trapezoidal Motion Profiling: One of the core features equipped on nearly all of the
servos on our robot, responsible for limiting our servo's travel to a specific acceleration,
deceleration, and max velocity.

Instead of implementing these algorithms in each situation we need them, we have two
abstractions that have these algorithms pre-implemented:

MotorGroup : A single motor, or collection of motors, that has PDF, and Stuck Protection
enabled.
MotionProfiledServo : A single servo that uses a trapezoidal motion profile to
incrementally set the servo position until it reaches its target.
In a subsystem, we can declare and use these hardware wrappers very easily, with little to no
redundancy in code:

MotorGroup:

val slides = MotorGroup(

PIDCoefficients(kP, kI, kD), // Defines the PID coefficients used
{ position, target, velocity -> // Defines an arbitrary, dynamic
feedforward solution on this motor.
val error = position - target
if (error > 10 && error < 70) { // If extendo is close to
being fully retracted, pull harder
-0.3
} else
{
0.0
}
},
primary = robot.hardware.extensionMotorRight, // Defines the
primary motor. Will use the encoder position for PID.
secondary = listOf(robot.hardware.extensionMotorLeft) // Defines
the motors that will run based off ot he primary motor's sensors & PID
).withTimeout(1500L)
.enableStuckProtection(StuckProtection(
minimumRequiredPositionDifference = 5,
timeStuckUnderMinimumMillis = 200L
)) // Enables our Stuck Protection algorithm.

MotionProfiledServo :

val wrist = motionProfiledServo(

"intake_wr", // The name, for debugging purposes.
robot.hardware.intakeWrist, // The underlying hardware device.
ProfileConstraints(45.0, 12.0, 12.0) // The acceleration, deceleration,
and max velocity.
)

Intra-Robot Interactions
With a complex robot, each subsystem is not entirely independent. For anything to occur on our
robot, we can assume that more than one subsystem will be running at the same time, or more
than one subsystem will require another subsystem to complete some sort of action before
moving on.
Since our team wanted a robust, speedy, and redundant robot to be competitive in matches, we
built a framework for intra-robot interactions that we will describe below.

At a high level, this system allows us to run actions involving chains of multiple subsystem
updates, just as it is happening in real time. With this system, we are also able to do this without
any arbitrary "sleeps", as our code is kept in sync with the robot's true subsystem positions.

To understand how our system works, we must define a term that is commonly used in this
intra-robot interaction system:

Job : A task in which a subsystem is moving an actuator from one position, to another.
A job starts when a subsystem defines a new job.
A job ends when the actuator is at its required position in real life
A job may be cancelled, or overridden in favor of a new job.

Jobs can be either chained, or run in parallel, as the entire intra-robot interaction system uses
Java's asynchronous Future programming model.

For example, let's say we wanted to move our arm to a certain position, and only when the arm
was at that position, we wanted to open the claw.

setArm(ArmState.Intake)
.thenRun {
// this section of code will ONLY run when the arm is at it's Intake
position in real life
setClaw(ClawState.Open)
}

We can also deal with failures easily, and account for them to ensure that our actions are
redundant:

setArm(ArmState.Intake)
.thenRun {
// this section of code will ONLY run when the arm is at it's Intake
position in real life
setClaw(ClawState.Open)
}
.exceptionally { error ->
// The arm, or claw, failed to move to the right position.
// It may have been stuck, so we want to move it back to
// its "Rest" position.
setArm(ArmState.Rest)
}
Want to run two things at once, and wait for both of them to finish? You are able to do that as
well:

allOf(
setClaw(ClawState.Open),
setArm(ArmState.Rest)
).thenRun {
// At this point, both the claw and arm are at its correct position.
// We can now close the claw.
setClaw(ClawState.Closed)
}

Using this powerful declarative framework, we are able to define advanced, and incredibly
efficient actions that our robot may want to take.

Using this Job system, our robot's Subsystem s, and hardware wrappers, we then created a
single class called CompositeInteraction .

This class has each and every intra-robot interaction defined in a central location, allowing any
other part of our codebase to easily run standardized tasks. At a high level, our robot tracks its
position in these states:

enum class InteractionCompositeState

{
Rest, // everything packed inside the robot
Pickup, // picking up a sample externally
Confirm, // waiting for the driver to confirm the sample selection
WallIntakeViaOuttake, // intaking a Specimen using the outtake
OuttakeReady, // sample available in the outtake, ready to initiate a
deposit
Outtaking, // slides up, ready for the driver to deposit
InProgress // robot is in progress, moving from one state to another.
}

A special state that our robot has is the InProgress state. This state is activated whenever a
robot is in the process of moving from a specific state, to another state. We are able to track the
robot's progress using the Job system that we defined earlier.

The InProgress state is essential to the reliability of the robot, as it prevents drivers, or
autonomous programs, from spamming the system from multiple different actions, when one is
still occurring.

Driver Control
Our complex robot requires our drivers to have a fine-grained control over it, to ensure the
greatest level of functionality. However, at the same time, we want driver control to be as
intuitive as possible.

Last year, right before the FIRST World Championship, we developed a declarative framework
for building gamepad bindings.

This framework allows us to easily bind gamepad buttons and dynamic triggers to a specific
task. It also gives us fine-grained control over the behavior of the buttons (whether it should run
something when it is held, released, or over a certain threshold (for triggers)).

This system also integrates well with our intra-robot interactions system, as we can limit certain
actions to certain robot states. For example, our robot will never be able to extend its horizontal
slides when the vertical slides are at its full extension, as there is no state that allows that.

An added benefit of this gamepad action system is that it prevents programmer mistakes, as it
is used in a builder-style with shared functionality, and is easy for new programmers to use.

For example, a gamepad action may look like this:

where(ButtonType.ButtonY)
.onlyWhen {
intakeComposite.state != InteractionCompositeState.InProgress
}
.triggers {
if (intakeComposite.state == InteractionCompositeState.Rest) {
intakeComposite.wallOuttakeFromRest()
} else if (intakeComposite.state ==
InteractionCompositeState.WallIntakeViaOuttake) {
intakeComposite.wallOuttakeToOuttakeReady()
}
}
.whenPressedOnce()

External Sensor Input

Our robot uses a collection of robot sensor inputs, and these inputs must be managed properly
to maintain the high refresh rates we need.

We use several sensors, but the primary sensor inputs are:

Motor Encoders: Accurate position of motor actuators

2-Wheel Odometry: Accurate positioning of the robot on the field, in an X-Y coordinate
plane.
 IMU: Accurate heading of the robot

All of these sensors are read through the I2C protocol, and the main issues with this protocol is
that hardware reads are incredibly slow over I2C (10ms+). As a result, reading these devices
decrease our program's main loop refresh rate by large factor, which hinders the robot's
performance.

To mitigate this, we built a sensor reading caching system that reads these hardware devices in
bulk (20ms instead of 4*10ms=40ms), and caches the result for the rest of the program to use
in the specified refresh rate interval.

This allows us to maintain a high refresh rate without having to worry about old data with sensor
reads.

Hardware-Software Reliability
Every team faces unseen events on the field relating to software reliability. Some of these
reliability issues include:

Software bugs
Robot disconnection
Improper robot states

However, our team has mitigated some of these issues with our robot after implementing
certain features, and building our robot in a strategic manner.

Strategic Wiring

Voltage spikes can occur regularly with any robot. However, as a team using all 8 motors and
12 servos, voltage drops are common. To mitigate the effects of a voltage spike on our robot's
control systems, we strategically wired our robot so that any spikes would not affect the
operation of our robot's control hub.

By using a goBILDA XT30 Expansion Hub, power to our servos, our two hubs, and the
grounding strap are delivered in parallel instead of in chain (typically power switch -> battery ->
hub 1 -> hub 2).

By using this expansion hub, voltage spikes from servos or motors on the expansion hub do not
affect the operation of the primary control hub, as power is not delivered through the hub, but
alongside it.

Improper Robot States

As described in prior sections, we have a robot framework for managing robot states. However,
we always expect failures. By expecting failures, we are able to build recovery solutions for
them, so that we waste little to no time in a timed competition.

One of the primary recovery solutions we have is a "reset sequence". This reset sequence is
used whenever the robot crashes, stops, or starts in an invalid state.

This reset sequence expects the robot to be in an incorrect state, and uses a combination of the
servo's position, motor velocity readings, and motor encoder readings, to slowly put the robot
back into its correct "Rest" position for normal operation.

Open Sourcing
Many of the "Frameworks" talked about in this software section are built into a fully open-source
and maintained library called Mono . Mono is currently used by three teams:

4215 Hypnotic Robotics

19477 Bit By Bit
6980 Bad Wolf

And these teams have seen massive success after using the framework/tooling built by us.

Mono can be viewed on GitHub: https://github.com/liftgate/mono/

Our robot's entire codebase is also on GitHub, available for public view. We encourage other
teams to take code from us, and improve on it.
https://github.com/rdhsrobotics/into-the-deep