Rotating Arm Sample Code

The code below, when built and deployed, controls a motor that moves an arm via rotation. The controller moves it to specific points via motion profiling, and on top of that, the controller can also control the rotation with the left joystick.

RobotContainer.java

// 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 frc.robot.Constants.OperatorConstants;
import frc.robot.commands.Autos;
import frc.robot.commands.ControllerCommand;
import frc.robot.commands.ExampleCommand;
import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;

/**
 * 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 trigger mappings) should be declared here.
 */
public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  private final ExampleSubsystem m_exampleSubsystem = ExampleSubsystem.getInstance();
  public static boolean PIDMode = false;

  // Replace with CommandPS4Controller or CommandJoystick if needed
  private final CommandXboxController m_driverController =
      new CommandXboxController(OperatorConstants.kDriverControllerPort);

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

  /**
   * Use this method to define your trigger->command mappings. Triggers can be created via the
   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
   * predicate, or via the named factories in {@link
   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
   * joysticks}.
   */
  private void configureBindings() {
    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
    new Trigger(m_exampleSubsystem::exampleCondition)
        .onTrue(new ExampleCommand(ExampleSubsystem.sub.state1));

    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
    // cancelling on release.
    m_driverController.y().onTrue(new ExampleCommand(ExampleSubsystem.sub.state2));
    m_driverController.x().onTrue(new ExampleCommand(ExampleSubsystem.sub.state1));
    m_driverController.b().onTrue(new ExampleCommand(ExampleSubsystem.sub.state3));
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // An example command will be run in autonomous
    return Autos.exampleAuto(m_exampleSubsystem);
  }

  public Command teleopcmd(){
    if(PIDMode){
      return null;
    }
    return new ControllerCommand(m_driverController.getLeftX());
  }

  
}

---

ExampleSubsystem.java

// 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 com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;


public class ExampleSubsystem extends SubsystemBase {
  public static ExampleSubsystem sub;

  
  
  
  SparkFlex motor = new SparkFlex(4, SparkLowLevel.MotorType.kBrushless);

  SparkAbsoluteEncoder encoder = motor.getAbsoluteEncoder();
  ProfiledPIDController PID = new ProfiledPIDController(2, 0, 0, new TrapezoidProfile.Constraints(1, 4));
  public TrapezoidProfile.State state1 = new TrapezoidProfile.State(0.88,0);
  public TrapezoidProfile.State state2 = new TrapezoidProfile.State(0.64,0);
  public TrapezoidProfile.State state3 = new TrapezoidProfile.State(0.39,0);
  
  /** Creates a new ExampleSubsystem. */
  public ExampleSubsystem() {
    //motor stuff
  }

  /**
   * Example command factory method.
   *
   * @return a command
   */
  
  
  
   public Command exampleMethodCommand() {
    // Inline construction of command goes here.
    // Subsystem::RunOnce implicitly requires `this` subsystem.
    return runOnce(
        () -> {
          /* one-time action goes here */
        });
  }

  
  

  

  public void stop(){
    motor.set(0);
  }
  public boolean moveMotorViaPID(TrapezoidProfile.State state){
    double speed = PID.calculate(encoder.getPosition(), state);
    
    motor.set(speed*-1);
    if(Math.abs(speed) <= 0.009){
      return true;
    }
    return false;

  }

  public void move(double speed){
    motor.set(speed);
  }
  

  
  
  /**
   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
   *
   * @return value of some boolean subsystem state, such as a digital sensor.
   */
  public boolean exampleCondition() {
    // Query some boolean state, such as a digital sensor.
    return false;
  }

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

  public static ExampleSubsystem getInstance(){
    if(sub == null){
      sub = new ExampleSubsystem();
    }
    return sub;
  }

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

---

ExampleCommand.java

// 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.commands;

import frc.robot.RobotContainer;
import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;

/** An example command that uses an example subsystem. */
public class ExampleCommand extends Command {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final ExampleSubsystem m_subsystem;
  private TrapezoidProfile.State m_state;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public ExampleCommand(TrapezoidProfile.State state) {
    m_subsystem = ExampleSubsystem.getInstance();
    m_state = state;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    RobotContainer.PIDMode = true;

  }

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

  

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

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

---

ControllerCommand.java

// 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.commands;

import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;

/** An example command that uses an example subsystem. */
public class ControllerCommand extends Command {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final ExampleSubsystem m_subsystem;
  private double m_speed;
  private boolean finished = false;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public ControllerCommand(double Speed) {
    m_subsystem = ExampleSubsystem.getInstance();
    m_speed = Speed;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

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

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

  // 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 finished;
  }
}