Add cpp examples (#659)

* Added C++ robot project examples and set up sub .clang-format for them

* Ran formatter

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

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Autonomous.h"
+
+#include <iostream>
+
+#include "CloseClaw.h"
+#include "DriveStraight.h"
+#include "Pickup.h"
+#include "Place.h"
+#include "PrepareToPickup.h"
+#include "SetDistanceToBox.h"
+#include "SetWristSetpoint.h"
+
+Autonomous::Autonomous()
+    : frc::CommandGroup("Autonomous") {
+	AddSequential(new PrepareToPickup());
+	AddSequential(new Pickup());
+	AddSequential(new SetDistanceToBox(0.10));
+	// AddSequential(new DriveStraight(4));  // Use Encoders if ultrasonic
+	// is broken
+	AddSequential(new Place());
+	AddSequential(new SetDistanceToBox(0.60));
+	// AddSequential(new DriveStraight(-2));  // Use Encoders if ultrasonic
+	// is broken
+	AddParallel(new SetWristSetpoint(-45));
+	AddSequential(new CloseClaw());
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/Autonomous.h

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/CommandGroup.h>
+
+/**
+ * The main autonomous command to pickup and deliver the soda to the box.
+ */
+class Autonomous : public frc::CommandGroup {
+public:
+	Autonomous();
+};

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

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "CloseClaw.h"
+
+#include "../Robot.h"
+
+CloseClaw::CloseClaw()
+    : frc::Command("CloseClaw") {
+	Requires(Robot::claw.get());
+}
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() {
+	Robot::claw->Close();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool CloseClaw::IsFinished() {
+	return Robot::claw->IsGripping();
+}
+
+// Called once after isFinished returns true
+void CloseClaw::End() {
+// 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
+	Robot::claw->Stop();
+#endif
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/CloseClaw.h

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.h>
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class CloseClaw : public frc::Command {
+public:
+	CloseClaw();
+	void Initialize() override;
+	bool IsFinished() override;
+	void End() override;
+};

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "DriveStraight.h"
+
+#include <PIDController.h>
+
+#include "../Robot.h"
+
+DriveStraight::DriveStraight(double distance) {
+	Requires(Robot::drivetrain.get());
+	pid = new frc::PIDController(4, 0, 0, new DriveStraightPIDSource(),
+			new DriveStraightPIDOutput());
+	pid->SetAbsoluteTolerance(0.01);
+	pid->SetSetpoint(distance);
+}
+
+// Called just before this Command runs the first time
+void DriveStraight::Initialize() {
+	// Get everything in a safe starting state.
+	Robot::drivetrain->Reset();
+	pid->Reset();
+	pid->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveStraight::IsFinished() {
+	return pid->OnTarget();
+}
+
+// Called once after isFinished returns true
+void DriveStraight::End() {
+	// Stop PID and the wheels
+	pid->Disable();
+	Robot::drivetrain->Drive(0, 0);
+}
+
+double DriveStraightPIDSource::PIDGet() {
+	return Robot::drivetrain->GetDistance();
+}
+
+void DriveStraightPIDOutput::PIDWrite(double d) {
+	Robot::drivetrain->Drive(d, d);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/DriveStraight.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.h>
+#include <PIDOutput.h>
+#include <PIDSource.h>
+
+namespace frc {
+class PIDController;
+}
+
+/**
+ * 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 frc::Command {
+public:
+	explicit DriveStraight(double distance);
+	void Initialize() override;
+	bool IsFinished() override;
+	void End() override;
+
+private:
+	frc::PIDController* pid;
+};
+
+class DriveStraightPIDSource : public PIDSource {
+public:
+	virtual ~DriveStraightPIDSource() = default;
+	double PIDGet() override;
+};
+
+class DriveStraightPIDOutput : public PIDOutput {
+public:
+	virtual ~DriveStraightPIDOutput() = default;
+	void PIDWrite(double d) override;
+};

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "OpenClaw.h"
+
+#include "../Robot.h"
+
+OpenClaw::OpenClaw()
+    : frc::Command("OpenClaw") {
+	Requires(Robot::claw.get());
+	SetTimeout(1);
+}
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() {
+	Robot::claw->Open();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool OpenClaw::IsFinished() {
+	return IsTimedOut();
+}
+
+// Called once after isFinished returns true
+void OpenClaw::End() {
+	Robot::claw->Stop();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/OpenClaw.h

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.h>
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class OpenClaw : public frc::Command {
+public:
+	OpenClaw();
+	void Initialize() override;
+	bool IsFinished() override;
+	void End() override;
+};

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

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Pickup.h"
+
+#include "CloseClaw.h"
+#include "SetElevatorSetpoint.h"
+#include "SetWristSetpoint.h"
+
+Pickup::Pickup()
+    : frc::CommandGroup("Pickup") {
+	AddSequential(new CloseClaw());
+	AddParallel(new SetWristSetpoint(-45));
+	AddSequential(new SetElevatorSetpoint(0.25));
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/Pickup.h

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/CommandGroup.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 frc::CommandGroup {
+public:
+	Pickup();
+};

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

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Place.h"
+
+#include "OpenClaw.h"
+#include "SetElevatorSetpoint.h"
+#include "SetWristSetpoint.h"
+
+Place::Place()
+    : frc::CommandGroup("Place") {
+	AddSequential(new SetElevatorSetpoint(0.25));
+	AddSequential(new SetWristSetpoint(0));
+	AddSequential(new OpenClaw());
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/Place.h

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/CommandGroup.h>
+
+/**
+ * Place a held soda can onto the platform.
+ */
+class Place : public frc::CommandGroup {
+public:
+	Place();
+};

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

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "PrepareToPickup.h"
+
+#include "OpenClaw.h"
+#include "SetElevatorSetpoint.h"
+#include "SetWristSetpoint.h"
+
+PrepareToPickup::PrepareToPickup()
+    : frc::CommandGroup("PrepareToPickup") {
+	AddParallel(new OpenClaw());
+	AddParallel(new SetWristSetpoint(0));
+	AddSequential(new SetElevatorSetpoint(0));
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/PrepareToPickup.h

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/CommandGroup.h>
+
+/**
+ * Make sure the robot is in a state to pickup soda cans.
+ */
+class PrepareToPickup : public frc::CommandGroup {
+public:
+	PrepareToPickup();
+};

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "SetDistanceToBox.h"
+
+#include <PIDController.h>
+
+#include "../Robot.h"
+
+SetDistanceToBox::SetDistanceToBox(double distance) {
+	Requires(Robot::drivetrain.get());
+	pid = new frc::PIDController(-2, 0, 0, new SetDistanceToBoxPIDSource(),
+			new SetDistanceToBoxPIDOutput());
+	pid->SetAbsoluteTolerance(0.01);
+	pid->SetSetpoint(distance);
+}
+
+// Called just before this Command runs the first time
+void SetDistanceToBox::Initialize() {
+	// Get everything in a safe starting state.
+	Robot::drivetrain->Reset();
+	pid->Reset();
+	pid->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetDistanceToBox::IsFinished() {
+	return pid->OnTarget();
+}
+
+// Called once after isFinished returns true
+void SetDistanceToBox::End() {
+	// Stop PID and the wheels
+	pid->Disable();
+	Robot::drivetrain->Drive(0, 0);
+}
+
+double SetDistanceToBoxPIDSource::PIDGet() {
+	return Robot::drivetrain->GetDistanceToObstacle();
+}
+
+void SetDistanceToBoxPIDOutput::PIDWrite(double d) {
+	Robot::drivetrain->Drive(d, d);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/SetDistanceToBox.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.h>
+#include <PIDOutput.h>
+#include <PIDSource.h>
+
+namespace frc {
+class PIDController;
+}
+
+/**
+ * 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 frc::Command {
+public:
+	explicit SetDistanceToBox(double distance);
+	void Initialize() override;
+	bool IsFinished() override;
+	void End() override;
+
+private:
+	frc::PIDController* pid;
+};
+
+class SetDistanceToBoxPIDSource : public frc::PIDSource {
+public:
+	virtual ~SetDistanceToBoxPIDSource() = default;
+	double PIDGet() override;
+};
+
+class SetDistanceToBoxPIDOutput : public frc::PIDOutput {
+public:
+	virtual ~SetDistanceToBoxPIDOutput() = default;
+	void PIDWrite(double d) override;
+};

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

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "SetElevatorSetpoint.h"
+
+#include <cmath>
+
+#include "../Robot.h"
+
+SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
+    : frc::Command("SetElevatorSetpoint") {
+	this->setpoint = setpoint;
+	Requires(Robot::elevator.get());
+}
+
+// Called just before this Command runs the first time
+void SetElevatorSetpoint::Initialize() {
+	Robot::elevator->SetSetpoint(setpoint);
+	Robot::elevator->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetElevatorSetpoint::IsFinished() {
+	return Robot::elevator->OnTarget();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/SetElevatorSetpoint.h

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.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 frc::Command {
+public:
+	explicit SetElevatorSetpoint(double setpoint);
+	void Initialize() override;
+	bool IsFinished() override;
+
+private:
+	double setpoint;
+};

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "SetWristSetpoint.h"
+
+#include "../Robot.h"
+
+SetWristSetpoint::SetWristSetpoint(double setpoint)
+    : frc::Command("SetWristSetpoint") {
+	this->setpoint = setpoint;
+	Requires(Robot::wrist.get());
+}
+
+// Called just before this Command runs the first time
+void SetWristSetpoint::Initialize() {
+	Robot::wrist->SetSetpoint(setpoint);
+	Robot::wrist->Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetWristSetpoint::IsFinished() {
+	return Robot::wrist->OnTarget();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/SetWristSetpoint.h

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.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 frc::Command {
+public:
+	explicit SetWristSetpoint(double setpoint);
+	void Initialize() override;
+	bool IsFinished() override;
+
+private:
+	double setpoint;
+};

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/TankDriveWithJoystick.cpp

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "TankDriveWithJoystick.h"
+
+#include "../Robot.h"
+
+TankDriveWithJoystick::TankDriveWithJoystick()
+    : frc::Command("TankDriveWithJoystick") {
+	Requires(Robot::drivetrain.get());
+}
+
+// Called repeatedly when this Command is scheduled to run
+void TankDriveWithJoystick::Execute() {
+	Robot::drivetrain->Drive(Robot::oi->GetJoystick());
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool TankDriveWithJoystick::IsFinished() {
+	return false;
+}
+
+// Called once after isFinished returns true
+void TankDriveWithJoystick::End() {
+	Robot::drivetrain->Drive(0, 0);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Commands/TankDriveWithJoystick.h

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Command.h>
+
+/**
+ * Have the robot drive tank style using the PS3 Joystick until interrupted.
+ */
+class TankDriveWithJoystick : public frc::Command {
+public:
+	TankDriveWithJoystick();
+	void Execute() override;
+	bool IsFinished() override;
+	void End() override;
+};

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

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "OI.h"
+
+#include <SmartDashboard/SmartDashboard.h>
+
+#include "Commands/Autonomous.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"
+
+OI::OI() {
+	frc::SmartDashboard::PutData("Open Claw", new OpenClaw());
+	frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
+
+	// Connect the buttons to commands
+	d_up.WhenPressed(new SetElevatorSetpoint(0.2));
+	d_down.WhenPressed(new SetElevatorSetpoint(-0.2));
+	d_right.WhenPressed(new CloseClaw());
+	d_left.WhenPressed(new OpenClaw());
+
+	r1.WhenPressed(new PrepareToPickup());
+	r2.WhenPressed(new Pickup());
+	l1.WhenPressed(new Place());
+	l2.WhenPressed(new Autonomous());
+}
+
+frc::Joystick* OI::GetJoystick() {
+	return &joy;
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/OI.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Buttons/JoystickButton.h>
+#include <Joystick.h>
+
+class OI {
+public:
+	OI();
+	frc::Joystick* GetJoystick();
+
+private:
+	frc::Joystick joy{0};
+
+	// Create some buttons
+	frc::JoystickButton d_up{&joy, 5};
+	frc::JoystickButton d_right{&joy, 6};
+	frc::JoystickButton d_down{&joy, 7};
+	frc::JoystickButton d_left{&joy, 8};
+	frc::JoystickButton l2{&joy, 9};
+	frc::JoystickButton r2{&joy, 10};
+	frc::JoystickButton l1{&joy, 11};
+	frc::JoystickButton r1{&joy, 12};
+};

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <iostream>
+
+std::shared_ptr<DriveTrain> Robot::drivetrain = std::make_shared<DriveTrain>();
+std::shared_ptr<Elevator> Robot::elevator = std::make_shared<Elevator>();
+std::shared_ptr<Wrist> Robot::wrist = std::make_shared<Wrist>();
+std::shared_ptr<Claw> Robot::claw = std::make_shared<Claw>();
+std::unique_ptr<OI> Robot::oi = std::make_unique<OI>();
+
+void Robot::RobotInit() {
+	// Show what command your subsystem is running on the SmartDashboard
+	frc::SmartDashboard::PutData(drivetrain.get());
+	frc::SmartDashboard::PutData(elevator.get());
+	frc::SmartDashboard::PutData(wrist.get());
+	frc::SmartDashboard::PutData(claw.get());
+}
+
+void Robot::AutonomousInit() {
+	autonomousCommand.Start();
+	std::cout << "Starting Auto" << std::endl;
+}
+
+void Robot::AutonomousPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+}
+
+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.
+	autonomousCommand.Cancel();
+	std::cout << "Starting Teleop" << std::endl;
+}
+
+void Robot::TeleopPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+}
+
+void Robot::TestPeriodic() {
+	lw->Run();
+}
+
+START_ROBOT_CLASS(Robot)

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <memory>
+
+#include <Commands/Command.h>
+#include <IterativeRobot.h>
+#include <LiveWindow/LiveWindow.h>
+
+#include "Commands/Autonomous.h"
+#include "OI.h"
+#include "Subsystems/Claw.h"
+#include "Subsystems/DriveTrain.h"
+#include "Subsystems/Elevator.h"
+#include "Subsystems/Wrist.h"
+
+class Robot : public frc::IterativeRobot {
+public:
+	static std::shared_ptr<DriveTrain> drivetrain;
+	static std::shared_ptr<Elevator> elevator;
+	static std::shared_ptr<Wrist> wrist;
+	static std::shared_ptr<Claw> claw;
+	static std::unique_ptr<OI> oi;
+
+private:
+	Autonomous autonomousCommand;
+	frc::LiveWindow* lw = frc::LiveWindow::GetInstance();
+
+	void RobotInit() override;
+	void AutonomousInit() override;
+	void AutonomousPeriodic() override;
+	void TeleopInit() override;
+	void TeleopPeriodic() override;
+	void TestPeriodic() override;
+};

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

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Claw.h"
+
+#include <LiveWindow/LiveWindow.h>
+
+Claw::Claw()
+    : frc::Subsystem("Claw") {
+	// Let's show everything on the LiveWindow
+	// frc::LiveWindow::GetInstance()->AddActuator("Claw", "Motor", &motor);
+}
+
+void Claw::InitDefaultCommand() {}
+
+void Claw::Open() {
+	motor.Set(-1);
+}
+
+void Claw::Close() {
+	motor.Set(1);
+}
+
+void Claw::Stop() {
+	motor.Set(0);
+}
+
+bool Claw::IsGripping() {
+	return contact.Get();
+}
+
+void Claw::Log() {}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Claw.h

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <Commands/Subsystem.h>
+#include <DigitalInput.h>
+#include <Victor.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 frc::Subsystem {
+public:
+	Claw();
+
+	void InitDefaultCommand() override;
+
+	/**
+	 * 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::Victor motor{7};
+	frc::DigitalInput contact{5};
+};

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

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "DriveTrain.h"
+
+#include <Joystick.h>
+#include <LiveWindow/LiveWindow.h>
+
+#include "../Commands/TankDriveWithJoystick.h"
+
+DriveTrain::DriveTrain()
+    : frc::Subsystem("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
+	leftEncoder.SetDistancePerPulse(0.042);
+	rightEncoder.SetDistancePerPulse(0.042);
+#else
+	// Circumference in ft = 4in/12(in/ft)*PI
+	leftEncoder.SetDistancePerPulse(
+			static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
+	rightEncoder.SetDistancePerPulse(
+			static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
+#endif
+
+	// Let's show everything on the LiveWindow
+	// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
+	// 		"Front_Left Motor", &frontLeft);
+	// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
+	// 		"Rear Left Motor", &rearLeft);
+	// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
+	// 		"Front Right Motor", &frontRight);
+	// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
+	// 		"Rear Right Motor", &rearRight);
+	// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Left
+	// Encoder",
+	// 		&leftEncoder);
+	// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Right
+	// Encoder",
+	// 		&rightEncoder);
+	// frc::LiveWindow::GetInstance()->AddSensor("Drive Train",
+	// "Rangefinder",
+	// 		&rangefinder);
+	// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Gyro",
+	// &gyro);
+}
+
+/**
+ * When no other command is running let the operator drive around
+ * using the PS3 joystick.
+ */
+void DriveTrain::InitDefaultCommand() {
+	SetDefaultCommand(new TankDriveWithJoystick());
+}
+
+/**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+void DriveTrain::Log() {
+	frc::SmartDashboard::PutNumber(
+			"Left Distance", leftEncoder.GetDistance());
+	frc::SmartDashboard::PutNumber(
+			"Right Distance", rightEncoder.GetDistance());
+	frc::SmartDashboard::PutNumber("Left Speed", leftEncoder.GetRate());
+	frc::SmartDashboard::PutNumber("Right Speed", rightEncoder.GetRate());
+	frc::SmartDashboard::PutNumber("Gyro", gyro.GetAngle());
+}
+
+void DriveTrain::Drive(double left, double right) {
+	drive.TankDrive(left, right);
+}
+
+void DriveTrain::Drive(frc::Joystick* joy) {
+	Drive(-joy->GetY(), -joy->GetRawAxis(4));
+}
+
+double DriveTrain::GetHeading() {
+	return gyro.GetAngle();
+}
+
+void DriveTrain::Reset() {
+	gyro.Reset();
+	leftEncoder.Reset();
+	rightEncoder.Reset();
+}
+
+double DriveTrain::GetDistance() {
+	return (leftEncoder.GetDistance() + rightEncoder.GetDistance()) / 2;
+}
+
+double DriveTrain::GetDistanceToObstacle() {
+	// Really meters in simulation since it's a rangefinder...
+	return rangefinder.GetAverageVoltage();
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/DriveTrain.h

@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <AnalogGyro.h>
+#include <AnalogInput.h>
+#include <Commands/Subsystem.h>
+#include <Encoder.h>
+#include <RobotDrive.h>
+#include <Talon.h>
+
+namespace frc {
+class Joystick;
+}
+
+/**
+ * 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 frc::Subsystem {
+public:
+	DriveTrain();
+
+	/**
+	 * When no other command is running let the operator drive around
+	 * using the PS3 joystick.
+	 */
+	void InitDefaultCommand() override;
+
+	/**
+	 * 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);
+
+	/**
+	 * @param joy The ps3 style joystick to use to drive tank style.
+	 */
+	void Drive(frc::Joystick* joy);
+
+	/**
+	 * @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::Talon frontLeft{1};
+	frc::Talon rearLeft{2};
+	frc::Talon frontRight{3};
+	frc::Talon rearRight{4};
+	frc::RobotDrive drive{frontLeft, rearLeft, frontRight, rearRight};
+	frc::Encoder leftEncoder{1, 2};
+	frc::Encoder rightEncoder{3, 4};
+	frc::AnalogInput rangefinder{6};
+	frc::AnalogGyro gyro{1};
+};

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

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Elevator.h"
+
+#include <LiveWindow/LiveWindow.h>
+#include <SmartDashboard/SmartDashboard.h>
+
+Elevator::Elevator()
+    : frc::PIDSubsystem("Elevator", kP_real, kI_real, 0.0) {
+#ifdef SIMULATION  // Check for simulation and update PID values
+	GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
+#endif
+	SetAbsoluteTolerance(0.005);
+
+	// Let's show everything on the LiveWindow
+	// frc::LiveWindow::GetInstance()->AddActuator("Elevator", "Motor",
+	// &motor);
+	// frc::LiveWindow::GetInstance()->AddSensor("Elevator", "Pot", &pot);
+	// frc::LiveWindow::GetInstance()->AddActuator("Elevator", "PID",
+	// 		GetPIDController());
+}
+
+void Elevator::InitDefaultCommand() {}
+
+void Elevator::Log() {
+	// frc::SmartDashboard::PutData("Wrist Pot", &pot);
+}
+
+double Elevator::ReturnPIDInput() {
+	return pot.Get();
+}
+
+void Elevator::UsePIDOutput(double d) {
+	motor.Set(d);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Elevator.h

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <AnalogPotentiometer.h>
+#include <Commands/PIDSubsystem.h>
+#include <Victor.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 frc::PIDSubsystem {
+public:
+	Elevator();
+
+	void InitDefaultCommand() override;
+
+	/**
+	 * 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 ReturnPIDInput();
+
+	/**
+	 * Use the motor as the PID output. This method is automatically called
+	 * by
+	 * the subsystem.
+	 */
+	void UsePIDOutput(double d);
+
+private:
+	frc::Victor motor{5};
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+	frc::AnalogPotentiometer pot{2, -2.0 / 5};
+#else
+	frc::AnalogPotentiometer 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/src/Subsystems/Wrist.cpp

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "Wrist.h"
+
+#include <LiveWindow/LiveWindow.h>
+#include <SmartDashboard/SmartDashboard.h>
+
+Wrist::Wrist()
+    : frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
+#ifdef SIMULATION  // Check for simulation and update PID values
+	GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
+#endif
+	SetAbsoluteTolerance(2.5);
+
+	// Let's show everything on the LiveWindow
+	// frc::LiveWindow::GetInstance()->AddActuator("Wrist", "Motor",
+	// &motor);
+	// frc::LiveWindow::GetInstance()->AddSensor("Wrist", "Pot", &pot);
+	frc::LiveWindow::GetInstance()->AddActuator(
+			"Wrist", "PID", GetPIDController());
+}
+
+void Wrist::InitDefaultCommand() {}
+
+void Wrist::Log() {
+	// frc::SmartDashboard::PutData("Wrist Angle", &pot);
+}
+
+double Wrist::ReturnPIDInput() {
+	return pot.Get();
+}
+
+void Wrist::UsePIDOutput(double d) {
+	motor.Set(d);
+}

wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Wrist.h

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <AnalogPotentiometer.h>
+#include <Commands/PIDSubsystem.h>
+#include <Victor.h>
+
+/**
+ * The wrist subsystem is like the elevator, but with a rotational joint instead
+ * of a linear joint.
+ */
+class Wrist : public PIDSubsystem {
+public:
+	Wrist();
+
+	void InitDefaultCommand() override;
+
+	/**
+	 * 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 ReturnPIDInput() override;
+
+	/**
+	 * Use the motor as the PID output. This method is automatically called
+	 * by
+	 * the subsystem.
+	 */
+	void UsePIDOutput(double d) override;
+
+private:
+	frc::Victor motor{6};
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+	frc::AnalogPotentiometer pot{3, -270.0 / 5};
+#else
+	frc::AnalogPotentiometer pot{3};  // Defaults to degrees
+#endif
+
+	static constexpr double kP_real = 1;
+	static constexpr double kP_simulation = 0.05;
+};