Rename GearsBotNew example to GearsBot

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/CloseClaw.h"
+#include "commands/OpenClaw.h"
+#include "commands/Pickup.h"
+#include "commands/Place.h"
+#include "commands/PrepareToPickup.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/TankDrive.h"
+
+RobotContainer::RobotContainer()
+    : m_autonomousCommand(&m_claw, &m_wrist, &m_elevator, &m_drivetrain) {
+  frc::SmartDashboard::PutData(&m_drivetrain);
+  frc::SmartDashboard::PutData(&m_elevator);
+  frc::SmartDashboard::PutData(&m_wrist);
+  frc::SmartDashboard::PutData(&m_claw);
+
+  m_claw.Log();
+  m_wrist.Log();
+  m_elevator.Log();
+  m_drivetrain.Log();
+
+  m_drivetrain.SetDefaultCommand(TankDrive(
+      &m_drivetrain,
+      [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); },
+      [this] {
+        return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand);
+      }));
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+  frc2::JoystickButton m_dUp{&m_joy, 5};
+  frc2::JoystickButton m_dRight{&m_joy, 6};
+  frc2::JoystickButton m_dDown{&m_joy, 7};
+  frc2::JoystickButton m_dLeft{&m_joy, 8};
+  frc2::JoystickButton m_l2{&m_joy, 9};
+  frc2::JoystickButton m_r2{&m_joy, 10};
+  frc2::JoystickButton m_l1{&m_joy, 11};
+  frc2::JoystickButton m_r1{&m_joy, 12};
+
+  m_dUp.WhenPressed(SetElevatorSetpoint(0.2, &m_elevator));
+  m_dDown.WhenPressed(SetElevatorSetpoint(-0.2, &m_elevator));
+  m_dRight.WhenPressed(CloseClaw(&m_claw));
+  m_dLeft.WhenPressed(OpenClaw(&m_claw));
+
+  m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
+  m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
+  m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
+  m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // An example command will be run in autonomous
+  return &m_autonomousCommand;
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/Autonomous.h"
+
+#include <frc2/command/ParallelCommandGroup.h>
+
+#include "commands/CloseClaw.h"
+#include "commands/DriveStraight.h"
+#include "commands/Pickup.h"
+#include "commands/Place.h"
+#include "commands/PrepareToPickup.h"
+#include "commands/SetDistanceToBox.h"
+#include "commands/SetWristSetpoint.h"
+
+Autonomous::Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
+                       DriveTrain* drivetrain) {
+  SetName("Autonomous");
+  AddCommands(
+      // clang-format off
+      PrepareToPickup(claw, wrist, elevator),
+      Pickup(claw, wrist, elevator),
+      SetDistanceToBox(0.10, drivetrain),
+      // DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken
+      Place(claw, wrist, elevator),
+      SetDistanceToBox(0.6, drivetrain),
+      // DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken
+      frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
+                                 CloseClaw(claw)));
+  // clang-format on
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/CloseClaw.h"
+
+#include "Robot.h"
+
+CloseClaw::CloseClaw(Claw* claw) : m_claw(claw) {
+  SetName("CloseClaw");
+  AddRequirements({m_claw});
+}
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() { m_claw->Close(); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool CloseClaw::IsFinished() { return m_claw->IsGripping(); }
+
+// Called once after isFinished returns true
+void CloseClaw::End(bool) {
+// NOTE: Doesn't stop in simulation due to lower friction causing the can to
+// fall out
+// + there is no need to worry about stalling the motor or crushing the can.
+#ifndef SIMULATION
+  m_claw->Stop();
+#endif
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/DriveStraight.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Robot.h"
+
+DriveStraight::DriveStraight(double distance, DriveTrain* drivetrain)
+    : frc2::CommandHelper<frc2::PIDCommand, DriveStraight>(
+          frc2::PIDController(4, 0, 0),
+          [this]() { return m_drivetrain->GetDistance(); }, distance,
+          [this](double output) { m_drivetrain->Drive(output, output); },
+          {drivetrain}),
+      m_drivetrain(drivetrain) {
+  m_controller.SetAbsoluteTolerance(0.01);
+}
+
+// Called just before this Command runs the first time
+void DriveStraight::Initialize() {
+  // Get everything in a safe starting state.
+  m_drivetrain->Reset();
+  frc2::PIDCommand::Initialize();
+}
+
+bool DriveStraight::IsFinished() { return m_controller.AtSetpoint(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/OpenClaw.h"
+
+#include "Robot.h"
+
+OpenClaw::OpenClaw(Claw* claw)
+    : frc2::CommandHelper<frc2::WaitCommand, OpenClaw>(1), m_claw(claw) {
+  SetName("OpenClaw");
+  AddRequirements({m_claw});
+}
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() {
+  m_claw->Open();
+  frc2::WaitCommand::Initialize();
+}
+
+// Called once after isFinished returns true
+void OpenClaw::End(bool) { m_claw->Stop(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/Pickup.h"
+
+#include <frc2/command/ParallelCommandGroup.h>
+
+#include "commands/CloseClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+Pickup::Pickup(Claw* claw, Wrist* wrist, Elevator* elevator) {
+  SetName("Pickup");
+  AddCommands(CloseClaw(claw),
+              frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
+                                         SetElevatorSetpoint(0.25, elevator)));
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/Place.h"
+
+#include "commands/OpenClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+Place::Place(Claw* claw, Wrist* wrist, Elevator* elevator) {
+  SetName("Place");
+  // clang-format off
+  AddCommands(SetElevatorSetpoint(0.25, elevator),
+              SetWristSetpoint(0, wrist),
+              OpenClaw(claw));
+  // clang-format on
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/PrepareToPickup.h"
+
+#include <frc2/command/ParallelCommandGroup.h>
+
+#include "commands/OpenClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+PrepareToPickup::PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator) {
+  SetName("PrepareToPickup");
+  AddCommands(OpenClaw(claw),
+              frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator),
+                                         SetWristSetpoint(0, wrist)));
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/SetDistanceToBox.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Robot.h"
+
+SetDistanceToBox::SetDistanceToBox(double distance, DriveTrain* drivetrain)
+    : frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox>(
+          frc2::PIDController(-2, 0, 0),
+          [this]() { return m_drivetrain->GetDistanceToObstacle(); }, distance,
+          [this](double output) { m_drivetrain->Drive(output, output); },
+          {drivetrain}),
+      m_drivetrain(drivetrain) {
+  m_controller.SetAbsoluteTolerance(0.01);
+}
+
+// Called just before this Command runs the first time
+void SetDistanceToBox::Initialize() {
+  // Get everything in a safe starting state.
+  m_drivetrain->Reset();
+  frc2::PIDCommand::Initialize();
+}
+
+bool SetDistanceToBox::IsFinished() { return m_controller.AtSetpoint(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/SetElevatorSetpoint.h"
+
+#include <cmath>
+
+#include "Robot.h"
+
+SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator* elevator)
+    : m_setpoint(setpoint), m_elevator(elevator) {
+  SetName("SetElevatorSetpoint");
+  AddRequirements({m_elevator});
+}
+
+// Called just before this Command runs the first time
+void SetElevatorSetpoint::Initialize() {
+  m_elevator->SetSetpoint(m_setpoint);
+  m_elevator->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetElevatorSetpoint::IsFinished() {
+  return m_elevator->GetController().AtSetpoint();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/SetWristSetpoint.h"
+
+#include "Robot.h"
+
+SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist* wrist)
+    : m_setpoint(setpoint), m_wrist(wrist) {
+  SetName("SetWristSetpoint");
+  AddRequirements({m_wrist});
+}
+
+// Called just before this Command runs the first time
+void SetWristSetpoint::Initialize() {
+  m_wrist->SetSetpoint(m_setpoint);
+  m_wrist->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetWristSetpoint::IsFinished() {
+  return m_wrist->GetController().AtSetpoint();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TankDrive.h"
+
+#include "Robot.h"
+
+TankDrive::TankDrive(DriveTrain* drivetrain, std::function<double()> left,
+                     std::function<double()> right)
+    : m_drivetrain(drivetrain), m_left(left), m_right(right) {
+  SetName("TankDrive");
+  AddRequirements({m_drivetrain});
+}
+
+// Called repeatedly when this Command is scheduled to run
+void TankDrive::Execute() { m_drivetrain->Drive(m_left(), m_right()); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool TankDrive::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void TankDrive::End(bool) { m_drivetrain->Drive(0, 0); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/Claw.h"
+
+Claw::Claw() {
+  // Let's show everything on the LiveWindow
+  SetName("Claw");
+  AddChild("Motor", &m_motor);
+}
+
+void Claw::Open() { m_motor.Set(-1); }
+
+void Claw::Close() { m_motor.Set(1); }
+
+void Claw::Stop() { m_motor.Set(0); }
+
+bool Claw::IsGripping() { return m_contact.Get(); }
+
+void Claw::Log() {}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveTrain.h"
+
+#include <frc/Joystick.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+DriveTrain::DriveTrain() {
+// Encoders may measure differently in the real world and in
+// simulation. In this example the robot moves 0.042 barleycorns
+// per tick in the real world, but the simulated encoders
+// simulate 360 tick encoders. This if statement allows for the
+// real robot to handle this difference in devices.
+#ifndef SIMULATION
+  m_leftEncoder.SetDistancePerPulse(0.042);
+  m_rightEncoder.SetDistancePerPulse(0.042);
+#else
+  // Circumference in ft = 4in/12(in/ft)*PI
+  m_leftEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
+                                    360.0);
+  m_rightEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
+                                     360.0);
+#endif
+  SetName("DriveTrain");
+  // Let's show everything on the LiveWindow
+  AddChild("Front_Left Motor", &m_frontLeft);
+  AddChild("Rear Left Motor", &m_rearLeft);
+  AddChild("Front Right Motor", &m_frontRight);
+  AddChild("Rear Right Motor", &m_rearRight);
+  AddChild("Left Encoder", &m_leftEncoder);
+  AddChild("Right Encoder", &m_rightEncoder);
+  AddChild("Rangefinder", &m_rangefinder);
+  AddChild("Gyro", &m_gyro);
+}
+
+void DriveTrain::Log() {
+  frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
+  frc::SmartDashboard::PutNumber("Right Distance",
+                                 m_rightEncoder.GetDistance());
+  frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
+  frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
+  frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
+}
+
+void DriveTrain::Drive(double left, double right) {
+  m_robotDrive.TankDrive(left, right);
+}
+
+double DriveTrain::GetHeading() { return m_gyro.GetAngle(); }
+
+void DriveTrain::Reset() {
+  m_gyro.Reset();
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double DriveTrain::GetDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+double DriveTrain::GetDistanceToObstacle() {
+  // Really meters in simulation since it's a rangefinder...
+  return m_rangefinder.GetAverageVoltage();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/Elevator.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Elevator::Elevator()
+    : frc2::PIDSubsystem(frc2::PIDController(kP_real, kI_real, 0)) {
+#ifdef SIMULATION  // Check for simulation and update PID values
+  GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
+#endif
+  m_controller.SetAbsoluteTolerance(0.005);
+
+  SetName("Elevator");
+  // Let's show everything on the LiveWindow
+  AddChild("Motor", &m_motor);
+  AddChild("Pot", &m_pot);
+}
+
+void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); }
+
+double Elevator::GetMeasurement() { return m_pot.Get(); }
+
+void Elevator::UseOutput(double d) { m_motor.Set(d); }
+
+double Elevator::GetSetpoint() { return m_setpoint; }
+
+void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/Wrist.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Wrist::Wrist() : frc2::PIDSubsystem(frc2::PIDController(kP_real, 0, 0)) {
+#ifdef SIMULATION  // Check for simulation and update PID values
+  GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
+#endif
+  m_controller.SetAbsoluteTolerance(2.5);
+
+  SetName("Wrist");
+  // Let's show everything on the LiveWindow
+  AddChild("Motor", &m_motor);
+  AddChild("Pot", &m_pot);
+}
+
+void Wrist::Log() {
+  // frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
+}
+
+double Wrist::GetSetpoint() { return m_setpoint; }
+
+void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
+double Wrist::GetMeasurement() { return m_pot.Get(); }
+
+void Wrist::UseOutput(double d) { m_motor.Set(d); }

wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Joystick.h>
+#include <frc2/command/Command.h>
+
+#include "commands/Autonomous.h"
+#include "subsystems/Claw.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * 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.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // The robot's subsystems and commands are defined here...
+  frc::Joystick m_joy{0};
+
+  Claw m_claw;
+  Wrist m_wrist;
+  Elevator m_elevator;
+  DriveTrain m_drivetrain;
+
+  Autonomous m_autonomousCommand;
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * The main autonomous command to pickup and deliver the soda to the box.
+ */
+class Autonomous
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup, Autonomous> {
+ public:
+  Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
+             DriveTrain* drivetrain);
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Claw.h"
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class CloseClaw : public frc2::CommandHelper<frc2::CommandBase, CloseClaw> {
+ public:
+  explicit CloseClaw(Claw* claw);
+  void Initialize() override;
+  bool IsFinished() override;
+  void End(bool interrupted) override;
+
+ private:
+  Claw* m_claw;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/PIDCommand.h>
+
+#include "subsystems/DriveTrain.h"
+
+/**
+ * Drive the given distance straight (negative values go backwards).
+ * Uses a local PID controller to run a simple PID loop that is only
+ * enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
+ */
+class DriveStraight
+    : public frc2::CommandHelper<frc2::PIDCommand, DriveStraight> {
+ public:
+  explicit DriveStraight(double distance, DriveTrain* drivetrain);
+  void Initialize() override;
+  bool IsFinished() override;
+
+ private:
+  DriveTrain* m_drivetrain;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/WaitCommand.h>
+
+#include "subsystems/Claw.h"
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class OpenClaw : public frc2::CommandHelper<frc2::WaitCommand, OpenClaw> {
+ public:
+  explicit OpenClaw(Claw* claw);
+  void Initialize() override;
+  void End(bool interrupted) override;
+
+ private:
+  Claw* m_claw;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * Pickup a soda can (if one is between the open claws) and
+ * get it in a safe state to drive around.
+ */
+class Pickup
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup, Pickup> {
+ public:
+  Pickup(Claw* claw, Wrist* wrist, Elevator* elevator);
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h

@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * Place a held soda can onto the platform.
+ */
+class Place : public frc2::CommandHelper<frc2::SequentialCommandGroup, Place> {
+ public:
+  Place(Claw* claw, Wrist* wrist, Elevator* elevator);
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Claw.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+/**
+ * Make sure the robot is in a state to pickup soda cans.
+ */
+class PrepareToPickup : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+                                                   PrepareToPickup> {
+ public:
+  PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator);
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/PIDCommand.h>
+
+#include "subsystems/DriveTrain.h"
+
+/**
+ * Drive until the robot is the given distance away from the box. Uses a local
+ * PID controller to run a simple PID loop that is only enabled while this
+ * command is running. The input is the averaged values of the left and right
+ * encoders.
+ */
+class SetDistanceToBox
+    : public frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox> {
+ public:
+  explicit SetDistanceToBox(double distance, DriveTrain* drivetrain);
+  void Initialize() override;
+  bool IsFinished() override;
+
+ private:
+  DriveTrain* m_drivetrain;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Elevator.h"
+
+/**
+ * Move the elevator to a given location. This command finishes when it is
+ * within
+ * the tolerance, but leaves the PID loop running to maintain the position.
+ * Other
+ * commands using the elevator should make sure they disable PID!
+ */
+class SetElevatorSetpoint
+    : public frc2::CommandHelper<frc2::CommandBase, SetElevatorSetpoint> {
+ public:
+  explicit SetElevatorSetpoint(double setpoint, Elevator* elevator);
+  void Initialize() override;
+  bool IsFinished() override;
+
+ private:
+  double m_setpoint;
+  Elevator* m_elevator;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Wrist.h"
+
+/**
+ * Move the wrist to a given angle. This command finishes when it is within
+ * the tolerance, but leaves the PID loop running to maintain the position.
+ * Other commands using the wrist should make sure they disable PID!
+ */
+class SetWristSetpoint
+    : public frc2::CommandHelper<frc2::CommandBase, SetWristSetpoint> {
+ public:
+  explicit SetWristSetpoint(double setpoint, Wrist* wrist);
+  void Initialize() override;
+  bool IsFinished() override;
+
+ private:
+  double m_setpoint;
+  Wrist* m_wrist;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/DriveTrain.h"
+
+/**
+ * Have the robot drive tank style using the PS3 Joystick until interrupted.
+ */
+class TankDrive : public frc2::CommandHelper<frc2::CommandBase, TankDrive> {
+ public:
+  TankDrive(DriveTrain* drivetrain, std::function<double()> left,
+            std::function<double()> right);
+  void Execute() override;
+  bool IsFinished() override;
+  void End(bool interrupted) override;
+
+ private:
+  DriveTrain* m_drivetrain;
+  std::function<double()> m_left;
+  std::function<double()> m_right;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc2/command/SubsystemBase.h>
+
+/**
+ * The claw subsystem is a simple system with a motor for opening and closing.
+ * If using stronger motors, you should probably use a sensor so that the
+ * motors don't stall.
+ */
+class Claw : public frc2::SubsystemBase {
+ public:
+  Claw();
+
+  /**
+   * Set the claw motor to move in the open direction.
+   */
+  void Open();
+
+  /**
+   * Set the claw motor to move in the close direction.
+   */
+  void Close();
+
+  /**
+   * Stops the claw motor from moving.
+   */
+  void Stop();
+
+  /**
+   * Return true when the robot is grabbing an object hard enough
+   * to trigger the limit switch.
+   */
+  bool IsGripping();
+
+  void Log();
+
+ private:
+  frc::PWMVictorSPX m_motor{7};
+  frc::DigitalInput m_contact{5};
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/AnalogInput.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+namespace frc {
+class Joystick;
+}  // namespace frc
+
+/**
+ * The DriveTrain subsystem incorporates the sensors and actuators attached to
+ * the robots chassis. These include four drive motors, a left and right encoder
+ * and a gyro.
+ */
+class DriveTrain : public frc2::SubsystemBase {
+ public:
+  DriveTrain();
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  void Log();
+
+  /**
+   * Tank style driving for the DriveTrain.
+   * @param left Speed in range [-1,1]
+   * @param right Speed in range [-1,1]
+   */
+  void Drive(double left, double right);
+
+  /**
+   * @return The robots heading in degrees.
+   */
+  double GetHeading();
+
+  /**
+   * Reset the robots sensors to the zero states.
+   */
+  void Reset();
+
+  /**
+   * @return The distance driven (average of left and right encoders).
+   */
+  double GetDistance();
+
+  /**
+   * @return The distance to the obstacle detected by the rangefinder.
+   */
+  double GetDistanceToObstacle();
+
+ private:
+  frc::PWMVictorSPX m_frontLeft{1};
+  frc::PWMVictorSPX m_rearLeft{2};
+  frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
+
+  frc::PWMVictorSPX m_frontRight{3};
+  frc::PWMVictorSPX m_rearRight{4};
+  frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
+
+  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+  frc::Encoder m_leftEncoder{1, 2};
+  frc::Encoder m_rightEncoder{3, 4};
+  frc::AnalogInput m_rangefinder{6};
+  frc::AnalogGyro m_gyro{1};
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogPotentiometer.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc2/command/PIDSubsystem.h>
+
+/**
+ * The elevator subsystem uses PID to go to a given height. Unfortunately, in
+ * it's current
+ * state PID values for simulation are different than in the real world do to
+ * minor differences.
+ */
+class Elevator : public frc2::PIDSubsystem {
+ public:
+  Elevator();
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  void Log();
+
+  /**
+   * Use the potentiometer as the PID sensor. This method is automatically
+   * called by the subsystem.
+   */
+  double GetMeasurement() override;
+
+  /**
+   * Use the motor as the PID output. This method is automatically called
+   * by
+   * the subsystem.
+   */
+  void UseOutput(double d) override;
+
+  double GetSetpoint() override;
+
+  /**
+   * Sets the setpoint for the subsystem.
+   */
+  void SetSetpoint(double setpoint);
+
+ private:
+  frc::PWMVictorSPX m_motor{5};
+  double m_setpoint = 0;
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+  frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
+#else
+  frc::AnalogPotentiometer m_pot{2};  // Defaults to meters
+#endif
+
+  static constexpr double kP_real = 4;
+  static constexpr double kI_real = 0.07;
+  static constexpr double kP_simulation = 18;
+  static constexpr double kI_simulation = 0.2;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogPotentiometer.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc2/command/PIDSubsystem.h>
+
+/**
+ * The wrist subsystem is like the elevator, but with a rotational joint instead
+ * of a linear joint.
+ */
+class Wrist : public frc2::PIDSubsystem {
+ public:
+  Wrist();
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  void Log();
+
+  /**
+   * Use the potentiometer as the PID sensor. This method is automatically
+   * called by the subsystem.
+   */
+  double GetMeasurement() override;
+
+  /**
+   * Use the motor as the PID output. This method is automatically called
+   * by
+   * the subsystem.
+   */
+  void UseOutput(double d) override;
+
+  double GetSetpoint() override;
+
+  /**
+   * Sets the setpoint for the subsystem.
+   */
+  void SetSetpoint(double setpoint);
+
+ private:
+  frc::PWMVictorSPX m_motor{6};
+  double m_setpoint = 0;
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+  frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
+#else
+  frc::AnalogPotentiometer m_pot{3};  // Defaults to degrees
+#endif
+
+  static constexpr double kP_real = 1;
+  static constexpr double kP_simulation = 0.05;
+};