Exercise - Kitbot Rewrite, Pt. 2

What You’ll Accomplish

In this second part of the kitbot rewrite, you will:

1. Create a Drivetrain mechanism class to allow for control of the drivetrain.
2. Define an arcadeDrive command that dynamically reads controller inputs using DoubleSuppliers.
3. Bind the default drivetrain command in your teleop OpMode to enable driving.
4. Define some basic autonomous modes.

Task 1: The Drivetrain Mechanism

Create a new Java class named Drivetrain.java in the mechanisms package. This class should implement Mechanism.

Then, do the following:

1. Initialize four motors as private class fields: leftLeader, leftFollower, rightLeader, and rightFollower, which are on CANBus 0 and CAN IDs 0-3 respectively.
2. Initialize a DifferentialDrive object named differentialDrive with the left and right leader motors.
3. Initialize an OnboardIMU object named imu.
4. Initialize a DrivetrainSim object named drivetrainSim and make a method named periodic() that runs the drivetrain sim periodic() method.

Hint

Refer back to Stage 1A for how to do these steps.

Task 2: Controlling the Drivetrain

Now that we have a Drivetrain class, we can create Commands that control the drivetrain.

To do that, write the following:

1. The drivetrain motor configuration code, which can be found in Robot.java from stage 1A.
2. An idle() command that commands 0.0 speed and 0.0 rotation, and is set as the default command in the constructor.
3. An arcadeDrive(DoubleSupplier forwardThrottle, DoubleSupplier rotationThrottle) command that continuously updates the drive motor speeds using the values from the suppliers.

Item 1, Hint 1

The constructor of the Robot class in stage 1A contains the code to configure the drivetrain motors.

Item 1, Hint 2

We want to configure our motors when a new Drivetrain instance is created. What Java declaration defines the code that runs when a class is instantiated?

Item 3 Hint

Since the joystick values change constantly, you must fetch the speed and rotation from the suppliers inside the command’s loop. You should have the following inside of your command’s body:

while (true) {
  differentialDrive.arcadeDrive(
      forwardThrottle.getAsDouble(), rotationThrottle.getAsDouble());
  coroutine.yield();
}

Task 3: Using the Drivetrain

Now that we have a Drivetrain class, we need to set it up to be used in our robot code.

1. Create a Drivetrain instance in Robot.java named drivetrain. It should be a public instead of private.
2. Call drivetrain.periodic(); in the robotPeriodic method.
3. Then, open MyTeleop.java (your teleop OpMode). In the constructor, set the drivetrain’s default command to arcadeDrive. Pass lambda expressions that read from your controller’s joystick axes.

Hint 1

You can create an arcade drive command like so:

robot.drivetrain.arcadeDrive(forwardThrottle, rotationThrottle);

Can you figure out the expressions to replace forwardThrottle and rotationThrottle with?

Hint 2

The expression for the forwardThrottle argument is () -> -xbox.getLeftY(), since we want to recompute the value of -xbox.getLeftY() while the arcadeDrive command is running.

Can you figure out the expression for the rotationThrottle argument, considering that the goal is to recompute the value of xbox.getRightX()?

Note

You might notice that we’re calling setDefaultCommand twice - once in Drivetrain.java, and once in MyTeleop.java.

The setDefaultCommand() call in MyTeleop.java will only be active while the MyTeleop opmode is selected. In all other circumstances, the default command set in the constructor (idle()) will apply.

Task 4: Create Auto Mode Files

We need to create two new files to define our autonomous OpModes.

Create 2 Java files in the opmode package, named DriveStraight.java and DriveBackThenShoot.java, before deleting the MyAuto.java file. Copy-paste the following code in each file, while replacing MyOpModeName with the correct name for each opmode:

class MyOpModeName extends PeriodicOpMode {
  private final Robot robot;

  public MyOpModeName(Robot robot) {
    this.robot = robot;
  }

  @Override
  public void start() {}
}

Task 5: Drive Straight Autonomous

This auto will drive the robot straight forwards for 4 seconds.

In the start() method of DriveStraight.java, schedule an arcadeDrive command with a timeout of 4 seconds. Then, do the following:

1. Rename the class defined in DriveStraight.java to DriveStraight.
2. In the start() method, schedule an arcadeDrive command with a timeout of 4 seconds. This arcadeDrive command should have a constant forward throttle of 0.5 and a constant rotational throttle of 0.

Hint 1

To schedule a command without a trigger, use Scheduler.getDefault().schedule(Command):

@Override
public void start() {
  Scheduler.getDefault().schedule(myAutoCommand);
}

Can you figure out what myAutoCommand should be?

Hint 2

A DoubleSupplier with syntax () -> 0.5 will always return 0.5 when its getAsDouble() method is called.

Use this information to complete the following Command by replacing forwardThrottle and rotationalThrottle with DoubleSuppliers:

robot.drivetrain
    .arcadeDrive(forwardThrottle, rotationThrottle)
    .withTimeout(Seconds.of(4));

Task 6: Drive Back then Shoot Autonomous

Driving back and shooting is a common auto found in the 2026 game, rebuilt. At the start of auto, the drive team would line up the kitbot to the wall of the scoring location. The kitbot would then drive backwards, and finally shoot the preloaded balls.

Create a Command-returning method named autoCommand(), with the following requirements:

1. The command returned from that method should have no requirements.
2. First, it should drive backwards with the robot.drivetrain.arcadeDrive() command, at a throttle of -0.2, for 2 seconds.
3. Then, it should run the robot.shooter.shoot() and robot.feeder.feed() commands simultaneously.

Then, in the start() method of the OpMode, schedule the autoCommand() we just created.

Hint 1

private Command autoCommand() {
  return null; // what should replace 'null' here?
}

Hint 2

What should you put in the curly brackets?
Command.noRequirements(coroutine -> {}).named("Drive Back Then Shoot")

Hint 3

coroutine.await(Command) runs a command from start to finish inside of another.

Hint 4

Look at the DriveStraight.java class. There, you wrote a command to drive forward, at a throttle of 0.5, for 4 seconds. Try copy-pasting that command into a coroutine.await() call, while changing some parameters.

Hint 5

To run 2 commands simultaneously, call coroutine.awaitAll(command1, command2) inside of another command.

Solution

The solutions to this stage can be found below: CTRE REV