5 - Autonomous

After reading this, you might also want to check out AutoDevil, a graphical autonomous planning tool that can generate code for you.

AutoBuilder

auto autoBuilder = AutoBuilder(chassis, odom);
// Set the robot starting position
autoBuilder.jumpTo({-46, -14, Units::degToRad(180)});
// Drive somewhere on the field. driveTo will switch between driveRAMSETEStep and driveHolonomicStep depending on if your chassis is holonomic.
autoBuilder.driveTo({-14.5f, -11, Units::degToRad(45)})->startSync();
// Rotate to some angle
autoBuilder.rotateTo(180)->startSync();

Pose Transformer

This function allows you to mirror entire autos across the field so you can switch between red and blue alliance sides. All following driving and turn commands will be transformed. MirrorTransformY inverts the Y-coordinates and angles (flip over X-axis) and MirrorTransformX does the opposite.

autoBuilder.setPoseTransformer(std::make_unique<MirrorTransformY>());
// Will drive to -15, +12 and an angle of -225 because of the transformer
autoBuilder.driveTo({-15, -12, Units::degToRad(225)})->startSync();

AutoSteps

AutoSteps allow you to run actions asynchronously while your auto is running. This is very useful for things like a lift that needs to update its PID controller every loop, or an intake with a sensor continuously checking for a game object.

Custom AutoStep class for the action you want:

#pragma once
#include "../subsystems/StickSystem.hpp"

namespace devils
{
    class StickAutoStep : public AutoStep
    {
    public:
        StickAutoStep(StickSystem& stickSystem)
            : stickSystem(stickSystem)
        {
        }

        void onUpdate() override
        {
            stickSystem.moveStick();
        }

    private:
        StickSystem& stickSystem;
    };
}

Auto code:

const auto stickAutoStep = std::make_shared<StickAutoStep>(stick);
stickAutoStep->start();

stick.setState(StickSystem::State::EXTENDED_FAST);