Challenge- Modify the XRPDrivetrain!

Test your skills by modifying the XRPDrivetrain for your robot.

As the challenge name suggests, you'll be modifying your XRPDrivetrain class! You will move the robot around using the two XRPMotors attached onto the side.

We are deliberately not using DifferentialDrive() and arcadeDrive() to practice using these motors.

Write a GetInstance() Method

Your code should:

• Return an instance of XRPDrivetrain

• Only have to use XRPDrivetrain's constructor once.

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

Sample Code

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

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

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

Write a moveRobot() Method

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

Conditions:

• Has 2 parameters double lSpeed and double rSpeed

• Set each of the XRP's XRPMotor objects to their respective parameter speeds.

• For safety, limit the motor's speed between two numbers.

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 that comes with your modded VSC.

WPILIB's MathUtil class has a clamp method, clamp(number, lowestBound, highestBound). This method will return the number you gave in the first parameter, but between lowestBound and highestBound inclusively.

Also import the MathUtil class!(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){

  // -3 and 3 are the bounds
  double checkedLSpeed = MathUtil.clamp(lSpeed, -3, 3);
  double checkedLSpeed = MathUtil.clamp(rSpeed, -3, 3);
  
  m_leftMotor.setVoltage(checkedLSpeed);
  m_rightMotor.setVoltage(checkedRSpeed);
}

Alternate Answer Code

double lowerBound = -3;
double upperBound = 3;

// Moves the Romi robot.
public void moveRobot(double lSpeed, double rSpeed){
  double checkedLSpeed = lSpeed;
  double checkedRSpeed = rSpeed;
  
  /* clamping is 10x better than this */
  if (lSpeed > upperBound) {
    lSpeed = upperBound;
  
  } else if (lSpeed < lowerBound) {
    lSpeed = lowerBound;
  }
  
  if (rSpeed > upperBound) {
    rSpeed = upperBound;
  
  } else if (rSpeed < lowerBound) {
    rSpeed = lowerBound;
    
  }
  
  m_leftMotor.setVoltage(checkedLSpeed);
  m_rightMotor.setVoltage(checkedRSpeed);
}

Sample Code

Sample 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.subsystems;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.xrp.XRPMotor;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class XRPDrivetrain extends SubsystemBase {
  private static XRPDrivetrain inst = new XRPDrivetrain();

  private static final double kGearRatio =
      (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
  private static final double kCountsPerMotorShaftRev = 12.0;
  private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
  private static final double kWheelDiameterInch = 2.3622; // 60 mm

  // The XRP has the left and right motors set to
  // channels 0 and 1 respectively
  private final XRPMotor m_leftMotor = new XRPMotor(0);
  private final XRPMotor m_rightMotor = new XRPMotor(1);

  // The XRP has onboard encoders that are hardcoded
  // to use DIO pins 4/5 and 6/7 for the left and right
  private final Encoder m_leftEncoder = new Encoder(4, 5);
  private final Encoder m_rightEncoder = new Encoder(6, 7);

  /** Creates a new XRPDrivetrain. */
  private XRPDrivetrain() {
    // Use inches as unit for encoder distances
    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
    resetEncoders();

    // Invert right side since motor is flipped
    m_rightMotor.setInverted(true);
  }

  public static XRPDrivetrain getInstance() {
    if (inst == null) {
      inst = new XRPDrivetrain();

    }

    return inst;
  }

  public void resetEncoders() {
    m_leftEncoder.reset();
    m_rightEncoder.reset();
  }

  public double getLeftDistanceInch() {
    return m_leftEncoder.getDistance();
  }

  public double getRightDistanceInch() {
    return m_rightEncoder.getDistance();
  }

  public void moveMotors(double lVoltage, double rVoltage) {

    // makes sure the voltage doesnt damage the motor via too much voltage.
    // the negative sign here is only a direction- negative voltage makes the motor spin in reverse.
    double checkedLVoltage = MathUtil.clamp(lVoltage, -10, 10);
    double checkedRVoltage = MathUtil.clamp(rVoltage, -10, 10);

    m_leftMotor.setVoltage(checkedLVoltage);
    m_rightMotor.setVoltage(checkedRVoltage);

  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }

  @Override
  public void simulationPeriodic() {
    // This method will be called once per scheduler run during simulation
  }
}