Rename GearsBotNew example to GearsBot

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;
+};