Code structure

Main Loop

The main loop keeps the logic of the robot running in real time. Each iteration is called a "tick", and the loop runs at 500 ticks per second (2 ms interval). The refresh function in subsystems and the execute function in commands are called by the command scheduler as part of this loop.

Note: There is also a faster loop used for updating certain critical IO drivers such as CAN.

Commands

Commands provide the high level instructions a robot may execute while being operated. These are usually bound to user inputs so that the robots can be operated via a wireless controller or mouse and keyboard. Commands can also be executed by default when no there are no inputs present, and some are run on startup (e.g. the sound that plays when the robot powers on). When implementing new controls and robot functions, commands represent the interface between inputs from the user and subsequent actions of the robot.

Examples of commands include:

• Move Chassis via joystick or keyboard
• Aim turret via joystick or mouse
• Rotate agitator (continuous or burst-fire)
• Flywheels on/off
• Play startup sound

Commands are executed by the Command Scheduler after being queued up by input bindings or queued manually. Each command is tied to a specific subsystem, and only one command can be executed that subsystem; this is to avoid conflicts with multiple commands trying to control the same motor, for example.

Once a command is scheduled to be run, the following steps occur:

1. void initialize() is called once to reset the command's internal state. Note: The same instance of a command's class is reused time a command is scheduled, so any internal state will be carried over to subsequent calls unless manually reset.
2. void execute() is repeatedly called each tick while the command is running.
3. bool isFinished() is queried each tick to check if the command has finished running.
4. void end(bool interrupted) is finally called after the command is finished and has been descheduled. Note: Commands may be descheduled even when isFinished() is still true, such as if the command is bound to a key which is then released. Commands may also be ended prematurely; this happens when another command is run on the same subsystem. In this case, interrupted will be set to true.

Here is a code snippet for a command which aims the turret with mouse input:

// command_move_turret_mouse.cpp

#include "command_move_turret_mouse.hpp"

namespace commands
{
void CommandMoveTurretMouse::initialize()
{
    // reset isCalibrated to ensure re-calibration each time the command is scheduled
    isCalibrated = false; 
}

void CommandMoveTurretMouse::execute()
{
    // "calibrate" by grabbing the current yaw and pitch angles from the turret subsystem.
    // this avoids the turret snapping into place when the command is executed
    if (!isCalibrated && turret->getIsCalibrated())
    {
        yaw = turret->getCurrentLocalYaw() + turret->getChassisYaw();
        pitch = turret->getCurrentLocalPitch();
        isCalibrated = true;
    }

    if (isCalibrated)
    {
        Remote* remote = &drivers->remote;
        float yawInput = 0.0f;
        float pitchInput = 0.0f;

        // grab mouse movement deltas from the remote and scale according to constants
        yawInput = remote->getMouseX() * MOUSE_SENS_YAW * DT;
        pitchInput = -remote->getMouseY() * MOUSE_SENS_PITCH * DT;

        // apply delta angles to our absolute yaw and pitch variables
        yaw -= yawInput;
        pitch += pitchInput;

        // keep the pitch angle within the allowed physical range
        pitch = modm::min(modm::max(pitch, PITCH_MIN), PITCH_MAX);

        // send the new yaw and pitch angles back to the turret subsystem
        turret->setTargetWorldAngles(yaw, pitch);
    }
}

bool CommandMoveTurretMouse::isFinished(void) const
{
    // keep running until the another command is scheduled in our place
    return false;
}

void CommandMoveTurretMouse::end(bool) {} // nothing to clean up
}

Subsystems

Subsystems encapsulate the broad logical components of a robot. They act primarily as an abstraction layer from low-level hardware control into higher-level functions that can be used by commands.

The core subsystem interface consists of two functions: initialize and refresh.

• void initialize() is called once during initialization when the robot is powered.
• void refresh() is repeatedly called each tick.

Most subsystem logic will require reside in refresh. For example, updating chassis motor controllers based on the current and desired movement speeds of the robot.

Examples of subsystems include:

Subsystem	Responsibility	Hardware
Chassis	Move the robot with desired velocity	Wheel motors
Turret	Aim and hold the turret at a particular angle	Yaw and pitch motors, yaw encoder, IMU
Flywheels	Spin flywheels up to the appropriate speed for firing projectiles	Flywheel motors
Agitators	Rotate agitators to feed projectiles into the launcher	Agitator motors
Sound	Play fun sounds on the robot, such as at startup	Buzzer

Here is a code snippet of a simple agitator subsystem implementation:

// agitator_subsystem.cpp

#include "agitator_subsystem.hpp"

namespace subsystems::agitator
{
AgitatorSubsystem::AgitatorSubsystem(src::Drivers* drivers)
    : Subsystem(drivers), drivers(drivers), agitator{drivers, AGITATOR}
{
    // AGITATOR is a motor configuration constant, defined in [robot]_constants.hpp
}

void AgitatorSubsystem::initialize()
{
    // initialize the motor
    agitator.initialize();
}

void AgitatorSubsystem::refresh()
{
    // calculate motor velocity based on desired rate of fire and agitator geometry
    float velocity = ballsPerSecond / BALLS_PER_REV
    agitator.updateVelocity(velocity);
}

//
// Public functions for use in commands
//
void AgitatorSubsystem::setBallsPerSecond(float bps)
{
    ballsPerSecond = bps;
}

float AgitatorSubsystem::getPosition()
{
    return agitator.measurePosition();
}

float AgitatorSubsystem::getVelocity()
{
    return agitator.measureVelocity();
}
}

Command Mappings

Command mappings bind user input to particular commands. There are various types of mapping, depending on the desired behavior, including press, hold, toggle, and hold repeat. These are setup in the [robot]_control.hpp files, along with their respective commands.

An example command mapping:

// define the command. note: &agitator is a reference to the agitator subsystem
CommandRotateAgitator rotateAgitator{drivers, &agitator};

// define a mapping 
HoldCommandMapping leftMouseDown{
    drivers,
    {&rotateAgitator},
    RemoteMapState(RemoteMapState::MouseButton::LEFT)
};

// ... elsewhere in the code, add to the command mapper
void initialize()
{
    drivers->commandMapper.addMap(&leftMouseDown);
}

Drivers

The Drivers class bundles instances of low-level hardware drivers into a single reference that can be passed around to subsystems and commands. It contains drivers for things like the CAN interface, UART communication with the CV board, external encoders (I2C), the remote controller, etc.

The global Drivers instance is created in main.cpp, and the drivers are initialized/updated separately from subsystems.

Controls Classes

Subsystems, commands, and input mappings are instantiated, initialized, and registered in special RobotControl classes inside the [robot]_control.hpp files. The particular setup varies with robot type. These controls are created in main.cpp.

Constants

Constants are parameters which are hard-coded into the firmware and do not change while the robot is running. These include things like:

• Physical characteristics such as wheel radius, maximum pitch angle, and camera offsets.
• Electrical information like which IDs and CAN buses are mapped to which motors.
• Functional parameters including max velocity, flywheel speed, firing rate, heat buffer, etc.

Since we are dealing with physical robots, constants are very useful and you'll see them all throughout the code. In order to keep track of them and easily modify the values when needed, all constants are placed in the appropriate [robot]_constants.hpp files and then #included in files where needed. Constants are split up by robot type to allow each robot to have unique characteristics while sharing the much of the same code.