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.
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 file named Drivetrain.java in the mechanisms package. Then, copy-paste the following code into the file:

public class Drivetrain implements Mechanism {
  private static final int leftLeaderID = 0, rightLeaderID = 2;
  private final TalonFX leftLeader = new TalonFX(leftLeaderID, CANBus.systemcore(0)),
      leftFollower = new TalonFX(1, CANBus.systemcore(0)),
      rightLeader = new TalonFX(rightLeaderID, CANBus.systemcore(0)),
      rightFollower = new TalonFX(3, CANBus.systemcore(0));

  private final OnboardIMU imu = new OnboardIMU(MountOrientation.FLAT);
  private final DifferentialDrive differentialDrive =
      new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);

  private final DrivetrainSim drivetrainSim = new DrivetrainSim(leftLeader, rightLeader);

  public void periodic() {
    drivetrainSim.periodic();
  }
}

Then implement the following:

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

Item 2 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();
}

Item 3, Hint 1

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

Item 3, 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?

Task 2: Drivetrain Setup

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 3: Create Auto Mode Files

Create 2 java files in the opmode package, named DriveStraightAutoMode.java and TurnInPlaceAutoMode.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 3: Drive Straight Autonomous

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

1. Rename the class defined in DriveStraightAutoMode.java to DriveStraightAutoMode.
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 4: Rotate in Place Command

Create a new Command in Drivetrain.java, called rotateInPlace, with the parameters double angleDegrees and DoubleSupplier rotationThrottle. It should execute these actions, in order:

1. Define a local variable, named targetAngle, whose value is the sum of angleDegrees and the robot’s current yaw. To get the robot’s current yaw, call imu.getRotation2d().getDegrees().
2. Call differentialDrive.arcadeDrive() in a while loop, which should continue while the robot’s current yaw is less than the targetAngle.

Hint

double targetAngle = imu.getRotation2d().getDegrees() + angleDegrees;
while (true) {
  // What to add here?
  coroutine.yield();
}

Task 5: Rotate in Place Autonomous

Finally, use the rotateInPlace command to implement a turn-in-place autonomous within TurnInPlaceAutoMode.java. It should rotate the robot 90 degrees from its starting position, at a throttle of 0.2.

Hint

Use the Scheduler to schedule a new rotateInPlace command, just like how task 3 accomplishes this.