Use PedroAutoPathCycleTest in the pedroPathing.examples package as an example. There are ActionSchedulers in the utils.software package, one for Auto and one for TeleOps that you can use to queue up actions.