Coding the Romi- Basic Movement

Challenge: Code the Romi

Now, you've just learned all the tools you'll use to code the Romi. This might be a difficult task, but splitting it up into smaller chunks will make it significantly easier to deal with.

Generally, your code should:

• Have triggers that run commands

• Commands that carry important data to RomiDrivetrain subsystem

• RomiDrivetrain manages motors

• have a public method inside RomiDrivetrain that sets the speed of its motors

Come up with a thorough plan before you code, with backup plans. It will save you the hassle of rewriting everything mid-code. Also, bounce ideas off other programmers, and make sure to work with them. It's a good skill to work with others in a FIRST Robotics team, anyway.

---

RomiDrivetrain (Subsystem)

Write a GetInstance() Method

Your code should:

• Return an instance of TankDriveSubsystem

• Only have to use TankDriveSubsystem's constructor once.

• Make sure your code follows the fundamental OOP concepts (Encapsulation, Abstraction)

Sample Code + Explanation

/* class header */
/* This is what I alluded to in Hint #1. */
private static TankDriveSubsystem subsystem;

/* this should only run once- for creating the subsystem object above. */
public TankDriveSubsystem() {
    /* [code] */
}

/* Returns a DriveSubsystem instance. */
public static TankDriveSubsystem getInstance() {
    if (subsystem != null) {
      subsystem = new TankDriveSubsystem(); /* create the aforementioned instance */
    }
    
    return subsystem;
}

Write a moveRobot() Method

Now that you learned about how to set motors, at the bottom of the RomiDrivetrain class, make a helper method called moveRobot().

Conditions:

• Has 2 parameters double lSpeed and double rSpeed

• Set each of the Romi's Spark motors to their respective parameter speeds.

• For safety, limit the Romi's speed between two specific values. This is to make sure the Romi doesn't move too fast in a direction and either break itself or slam into someone, and it's generally a good habit. Set these bounds to something small, like -5 and 5, or -7 and 7.

For the last point, although you can use math.abs() or a bunch of if-statements, there's another way we can do this, but using one of the built-in libraries in WPILIB VSC.

WPILIB's MathUtil class has a clamp method, clamp(number, lowestBound, highestBound).

Whatever's in the number parameter will never be smaller than the lowestBound and greater than the highestBound.

For example, if I were to do int num = MathUtil.clamp(3, 4, 6), num will be some value between 4 and 6 inclusively.

Make sure to import it! (Put this in your class header: import edu.wpi.first.math.MathUtil)

Once you have what you believe to be your answer, compare your code with the answer code below.

Answer Code

/* make sure that you import MathUtil!!!
*/
import edu.wpi.first.math.MathUtil;

// Moves the Romi robot.
public void moveRobot(double lSpeed, double rSpeed){
  double checkedLSpeed = MathUtil.clamp(lSpeed, -1, 1);
  double checkedLSpeed = MathUtil.clamp(rSpeed, -1, 1);
  
  m_leftMotor.set(checkedLSpeed);
  m_rightMotor.set(checkedRSpeed);
}

Alternate Answer Code

// Moves the Romi robot.
public void moveRobot(double lSpeed, double rSpeed){
  double checkedLSpeed = lSpeed;
  double checkedRSpeed = rSpeed;
  
  /* look at how cleaner the other one is!!!!!! */
  if (lSpeed > 1) {
    lSpeed = 1;
  
  } else if (lSpeed < -1) {
    lSpeed = -1;
  }
  
  if (rSpeed > 1) {
    rSpeed = 1;
  
  } else if (rSpeed < -1) {
    rSpeed = -1;
    
  }
  
  
  m_leftMotor.set(checkedLSpeed);
  m_rightMotor.set(checkedRSpeed);
}

Answer Breakdown

DO NOT OPEN UNTIL YOU FINISHED WRITING CODE!

[sorry WIP]

---

Command

Create a MoveRobot() command

All you'll need to do is make a class that:

• Create a Command with the type MoveRobot. Your command will need all the methods and fields mentioned above.

• The command has fields: double leftSpeed, double rightSpeed, and RomiDrivetrain subsystem.

• Make the MoveRobot constructor have three parameters: RomiDrivetrain m_subsystem, double newLeftSpeed, and double newRightSpeed.

• In the command's initialize() function, we'll call the subsystem's MoveMotors() method, and put the speed variables in it.

• You can repurpose the ExampleCommand that comes with your project.

Sample Code

package frc.robot.commands;

import frc.robot.subsystems.RomiDrivetrain;
import edu.wpi.first.wpilibj2.command.Command;

/** An example command that uses an example subsystem. */
public class MoveRobotCommand extends Command {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final RomiDrivetrain m_subsystem;
  private double leftSpeed;
  private double rightSpeed;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public MoveRobotCommand(RomiDrivetrain subsystem, double lSpeed, double rSpeed) {
    m_subsystem = subsystem;
    leftSpeed = lSpeed;
    rightSpeed = rSpeed;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    m_subsystem.moveMotors(leftSpeed, rightSpeed);
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    

  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    m_subsystem.moveMotors(0, 0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

Sample Code (alternate)

package frc.robot.commands;

import frc.robot.subsystems.RomiDrivetrain;
import edu.wpi.first.wpilibj2.command.Command;

/** An example command that uses an example subsystem. */
public class MoveRobotCommand extends Command {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final RomiDrivetrain m_subsystem;
  private double leftSpeed;
  private double rightSpeed;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public MoveRobotCommand(RomiDrivetrain subsystem, double lSpeed, double rSpeed) {
    m_subsystem = subsystem;
    leftSpeed = lSpeed;
    rightSpeed = rSpeed;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {}

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    m_subsystem.moveMotors(leftSpeed, rightSpeed);

  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {}

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

Answer Breakdown

DO NOT OPEN UNTIL YOU FINISHED WRITING CODE!

Let's break down these two sets of code.

The header looks pretty normal, but you might find this line.

 @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})

VSC gives warnings in your console for a variety of reasons. PMD.UnusedPrivateField refers to having private fields that are declared, but are never actually used. PMD.SingularField is creating a variable you'll only use in one spot of code, and never again.

Generally, doing these is considered messier, and VSC gives these warnings to help unsuspecting programmers optimize and clean up their code. However, due to the nature of FRC Programming, these warnings tend to be a nuisance rather than helpful. For example, what if you created a Spark variable, but didn't know how often you'd use it in your code? You'll create a new Spark variable at the top, instead of having to create a brand new one every time you want to use it. It's just more convenient.

This line of code stops VSC from panicking whenever these errors happen.

The constructor is pretty standard, nothing interesting to say about it, aside from the addRequirements() method. The addRequirements() method simply takes subsystem(s) in its parameters, and as the name suggests, forces the command to require the subsystem. Without it, your code will not work properly.

There are two approaches for using commands that I like to use.

---

RobotContainer

Bind MoveRobot() Command to Trigger

This is the final step before you can move the robot! All you'll need to do is:

• Use the triggers from your CommandController, and put your command inside the parameters.

• Use the two joysticks on your controller to move the robot. Get their axes, then set them as the speed variables of your command

Answer Code

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import frc.robot.commands.MoveRobotCommand;
import frc.robot.subsystems.RomiDrivetrain;
import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and button mappings) should be declared here.
 */
public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  private final RomiDrivetrain m_romiDrivetrain = RomiDrivetrain.getInstance();
  private final CommandXboxController controller = new CommandXboxController(0);

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
    // Configure the button bindings
    configureButtonBindings();
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be created by
   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    /* Triggers I used to make the robot move. */
    controller.x().whileTrue(new MoveRobotCommand(m_romiDrivetrain, controller.getLeftY(), controller.getRightY()));
    controller.x().onFalse(new MoveRobotCommand(m_romiDrivetrain, 0, 0));
    
    /* you can also use OnTrue() instead of WhileTrue(). */
  
  }
}