Add cpp examples (#659)

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

* Ran formatter

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/CheckForHotGoal.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 "CheckForHotGoal.h"
+
+#include "../Robot.h"
+
+CheckForHotGoal::CheckForHotGoal(double time) {
+	SetTimeout(time);
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool CheckForHotGoal::IsFinished() {
+	return IsTimedOut() || Robot::shooter->GoalIsHot();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/CheckForHotGoal.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>
+
+/**
+ * This command looks for the hot goal and waits until it's detected or timed
+ * out. The timeout is because it's better to shoot and get some autonomous
+ * points than get none. When called sequentially, this command will block until
+ * the hot goal is detected or until it is timed out.
+ */
+class CheckForHotGoal : public frc::Command {
+public:
+	explicit CheckForHotGoal(double time);
+	bool IsFinished() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/CloseClaw.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 "CloseClaw.h"
+
+#include "../Robot.h"
+
+CloseClaw::CloseClaw() {
+	Requires(Robot::collector.get());
+}
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() {
+	Robot::collector->Close();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/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/InstantCommand.h>
+
+/**
+ * Close the claw.
+ *
+ * NOTE: It doesn't wait for the claw to close since there is no sensor to
+ * detect that.
+ */
+class CloseClaw : public frc::InstantCommand {
+public:
+	CloseClaw();
+	void Initialize() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/Collect.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Collect.h"
+
+#include "../Robot.h"
+#include "CloseClaw.h"
+#include "SetCollectionSpeed.h"
+#include "SetPivotSetpoint.h"
+#include "WaitForBall.h"
+
+Collect::Collect() {
+	AddSequential(new SetCollectionSpeed(Collector::kForward));
+	AddParallel(new CloseClaw());
+	AddSequential(new SetPivotSetpoint(Pivot::kCollect));
+	AddSequential(new WaitForBall());
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/Collect.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>
+
+/**
+ * Get the robot set to collect balls.
+ */
+class Collect : public frc::CommandGroup {
+public:
+	Collect();
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveAndShootAutonomous.cpp

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "DriveAndShootAutonomous.h"
+
+#include "../Robot.h"
+#include "CheckForHotGoal.h"
+#include "CloseClaw.h"
+#include "DriveForward.h"
+#include "SetPivotSetpoint.h"
+#include "Shoot.h"
+#include "WaitForPressure.h"
+
+DriveAndShootAutonomous::DriveAndShootAutonomous() {
+	AddSequential(new CloseClaw());
+	AddSequential(new WaitForPressure(), 2);
+#ifndef SIMULATION
+	// NOTE: Simulation doesn't currently have the concept of hot.
+	AddSequential(new CheckForHotGoal(2));
+#endif
+	AddSequential(new SetPivotSetpoint(45));
+	AddSequential(new DriveForward(8, 0.3));
+	AddSequential(new Shoot());
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveAndShootAutonomous.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>
+
+/**
+ * Drive over the line and then shoot the ball. If the hot goal is not detected,
+ * it will wait briefly.
+ */
+class DriveAndShootAutonomous : public frc::CommandGroup {
+public:
+	DriveAndShootAutonomous();
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveForward.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "DriveForward.h"
+
+#include <cmath>
+
+#include "../Robot.h"
+
+void DriveForward::init(double dist, double maxSpeed) {
+	Requires(Robot::drivetrain.get());
+	distance = dist;
+	driveForwardSpeed = maxSpeed;
+}
+
+DriveForward::DriveForward() {
+	init(10, 0.5);
+}
+
+DriveForward::DriveForward(double dist) {
+	init(dist, 0.5);
+}
+
+DriveForward::DriveForward(double dist, double maxSpeed) {
+	init(dist, maxSpeed);
+}
+
+// Called just before this Command runs the first time
+void DriveForward::Initialize() {
+	Robot::drivetrain->GetRightEncoder()->Reset();
+	SetTimeout(2);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void DriveForward::Execute() {
+	error = (distance
+			- Robot::drivetrain->GetRightEncoder()->GetDistance());
+	if (driveForwardSpeed * kP * error >= driveForwardSpeed) {
+		Robot::drivetrain->TankDrive(
+				driveForwardSpeed, driveForwardSpeed);
+	} else {
+		Robot::drivetrain->TankDrive(driveForwardSpeed * kP * error,
+				driveForwardSpeed * kP * error);
+	}
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveForward::IsFinished() {
+	return (std::fabs(error) <= kTolerance) || IsTimedOut();
+}
+
+// Called once after isFinished returns true
+void DriveForward::End() {
+	Robot::drivetrain->Stop();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveForward.h

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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>
+
+/**
+ * This command drives the robot over a given distance with simple proportional
+ * control This command will drive a given distance limiting to a maximum speed.
+ */
+class DriveForward : public frc::Command {
+public:
+	DriveForward();
+	explicit DriveForward(double dist);
+	DriveForward(double dist, double maxSpeed);
+	void Initialize() override;
+	void Execute() override;
+	bool IsFinished() override;
+	void End() override;
+
+private:
+	double driveForwardSpeed;
+	double distance;
+	double error = 0;
+	static constexpr double kTolerance = 0.1;
+	static constexpr double kP = -1.0 / 5.0;
+
+	void init(double dist, double maxSpeed);
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveWithJoystick.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 "DriveWithJoystick.h"
+
+#include "../Robot.h"
+
+DriveWithJoystick::DriveWithJoystick() {
+	Requires(Robot::drivetrain.get());
+}
+
+// Called repeatedly when this Command is scheduled to run
+void DriveWithJoystick::Execute() {
+	Robot::drivetrain->TankDrive(Robot::oi->GetJoystick());
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveWithJoystick::IsFinished() {
+	return false;
+}
+
+// Called once after isFinished returns true
+void DriveWithJoystick::End() {
+	Robot::drivetrain->Stop();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/DriveWithJoystick.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>
+
+/**
+ * This command allows PS3 joystick to drive the robot. It is always running
+ * except when interrupted by another command.
+ */
+class DriveWithJoystick : public frc::Command {
+public:
+	DriveWithJoystick();
+	void Execute() override;
+	bool IsFinished() override;
+	void End() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/ExtendShooter.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ExtendShooter.h"
+
+#include "../Robot.h"
+
+ExtendShooter::ExtendShooter()
+    : frc::TimedCommand(1.0) {
+	Requires(Robot::shooter.get());
+}
+
+// Called just before this Command runs the first time
+void ExtendShooter::Initialize() {
+	Robot::shooter->ExtendBoth();
+}
+
+// Called once after isFinished returns true
+void ExtendShooter::End() {
+	Robot::shooter->RetractBoth();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/ExtendShooter.h

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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/TimedCommand.h>
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+class ExtendShooter : public frc::TimedCommand {
+public:
+	ExtendShooter();
+	void Initialize() override;
+	void End() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/LowGoal.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 "LowGoal.h"
+
+#include "../Robot.h"
+#include "ExtendShooter.h"
+#include "SetCollectionSpeed.h"
+#include "SetPivotSetpoint.h"
+
+LowGoal::LowGoal() {
+	AddSequential(new SetPivotSetpoint(Pivot::kLowGoal));
+	AddSequential(new SetCollectionSpeed(Collector::kReverse));
+	AddSequential(new ExtendShooter());
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/LowGoal.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>
+
+/**
+ * Spit the ball out into the low goal assuming that the robot is in front of
+ * it.
+ */
+class LowGoal : public frc::CommandGroup {
+public:
+	LowGoal();
+};

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

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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() {
+	Requires(Robot::collector.get());
+}
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() {
+	Robot::collector->Open();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool OpenClaw::IsFinished() {
+	return Robot::collector->IsOpen();
+}

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

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+ */
+class OpenClaw : public frc::Command {
+public:
+	OpenClaw();
+	void Initialize() override;
+	bool IsFinished() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/SetCollectionSpeed.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "SetCollectionSpeed.h"
+
+#include "../Robot.h"
+
+SetCollectionSpeed::SetCollectionSpeed(double speed) {
+	Requires(Robot::collector.get());
+	this->speed = speed;
+}
+
+// Called just before this Command runs the first time
+void SetCollectionSpeed::Initialize() {
+	Robot::collector->SetSpeed(speed);
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/SetCollectionSpeed.h

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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/InstantCommand.h>
+
+/**
+ * This command sets the collector rollers spinning at the given speed. Since
+ * there is no sensor for detecting speed, it finishes immediately. As a result,
+ * the spinners may still be adjusting their speed.
+ */
+class SetCollectionSpeed : public frc::InstantCommand {
+public:
+	explicit SetCollectionSpeed(double speed);
+	void Initialize() override;
+
+private:
+	double speed;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/SetPivotSetpoint.cpp

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

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/SetPivotSetpoint.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>
+
+/**
+ * Moves the  pivot 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 pivot should make sure they disable PID!
+ */
+class SetPivotSetpoint : public frc::Command {
+public:
+	explicit SetPivotSetpoint(double setpoint);
+	void Initialize() override;
+	bool IsFinished() override;
+
+private:
+	double setpoint;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/Shoot.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Shoot.h"
+
+#include "../Robot.h"
+#include "ExtendShooter.h"
+#include "OpenClaw.h"
+#include "SetCollectionSpeed.h"
+#include "WaitForPressure.h"
+
+Shoot::Shoot() {
+	AddSequential(new WaitForPressure());
+	AddSequential(new SetCollectionSpeed(Collector::kStop));
+	AddSequential(new OpenClaw());
+	AddSequential(new ExtendShooter());
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/Shoot.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>
+
+/**
+ * Shoot the ball at the current angle.
+ */
+class Shoot : public frc::CommandGroup {
+public:
+	Shoot();
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/WaitForBall.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 "WaitForBall.h"
+
+#include "../Robot.h"
+
+WaitForBall::WaitForBall() {
+	Requires(Robot::collector.get());
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForBall::IsFinished() {
+	return Robot::collector->HasBall();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/WaitForBall.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>
+
+/**
+ * Wait until the collector senses that it has the ball. This command does
+ * nothing and is intended to be used in command groups to wait for this
+ * condition.
+ */
+class WaitForBall : public frc::Command {
+public:
+	WaitForBall();
+	bool IsFinished() override;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/WaitForPressure.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 "WaitForPressure.h"
+
+#include "../Robot.h"
+
+WaitForPressure::WaitForPressure() {
+	Requires(Robot::pneumatics.get());
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForPressure::IsFinished() {
+	return Robot::pneumatics->IsPressurized();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Commands/WaitForPressure.h

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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>
+
+/**
+ * Wait until the pneumatics are fully pressurized. This command does nothing
+ * and is intended to be used in command groups to wait for this condition.
+ */
+class WaitForPressure : public frc::Command {
+public:
+	WaitForPressure();
+	bool IsFinished() override;
+};

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

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "OI.h"
+
+#include "Commands/Collect.h"
+#include "Commands/DriveForward.h"
+#include "Commands/LowGoal.h"
+#include "Commands/SetCollectionSpeed.h"
+#include "Commands/SetPivotSetpoint.h"
+#include "Commands/Shoot.h"
+#include "Subsystems/Collector.h"
+#include "Subsystems/Pivot.h"
+
+OI::OI() {
+	R1.WhenPressed(new LowGoal());
+	R2.WhenPressed(new Collect());
+
+	L1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
+	L2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
+
+	sticks.WhenActive(new Shoot());
+
+	// SmartDashboard Buttons
+	frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
+	frc::SmartDashboard::PutData("Drive Backward", new DriveForward(-2.25));
+	frc::SmartDashboard::PutData("Start Rollers",
+			new SetCollectionSpeed(Collector::kForward));
+	frc::SmartDashboard::PutData("Stop Rollers",
+			new SetCollectionSpeed(Collector::kStop));
+	frc::SmartDashboard::PutData("Reverse Rollers",
+			new SetCollectionSpeed(Collector::kReverse));
+}
+
+frc::Joystick* OI::GetJoystick() {
+	return &joystick;
+}

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

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <Buttons/JoystickButton.h>
+#include <Joystick.h>
+
+#include "Triggers/DoubleButton.h"
+
+class OI {
+public:
+	OI();
+	frc::Joystick* GetJoystick();
+
+private:
+	frc::Joystick joystick{0};
+
+	frc::JoystickButton L1{&joystick, 11};
+	frc::JoystickButton L2{&joystick, 9};
+	frc::JoystickButton R1{&joystick, 12};
+	frc::JoystickButton R2{&joystick, 10};
+
+	DoubleButton sticks{&joystick, 2, 3};
+};

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

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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>
+
+#include <SmartDashboard/SmartDashboard.h>
+
+std::shared_ptr<DriveTrain> Robot::drivetrain = std::make_shared<DriveTrain>();
+std::shared_ptr<Pivot> Robot::pivot = std::make_shared<Pivot>();
+std::shared_ptr<Collector> Robot::collector = std::make_shared<Collector>();
+std::shared_ptr<Shooter> Robot::shooter = std::make_shared<Shooter>();
+std::shared_ptr<Pneumatics> Robot::pneumatics = std::make_shared<Pneumatics>();
+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(pivot.get());
+	frc::SmartDashboard::PutData(collector.get());
+	frc::SmartDashboard::PutData(shooter.get());
+	frc::SmartDashboard::PutData(pneumatics.get());
+
+	// instantiate the command used for the autonomous period
+	autoChooser.AddDefault("Drive and Shoot", driveAndShootAuto.get());
+	autoChooser.AddObject("Drive Forward", driveForwardAuto.get());
+	frc::SmartDashboard::PutData("Auto Mode", &autoChooser);
+
+	pneumatics->Start();  // Pressurize the pneumatics.
+}
+
+void Robot::AutonomousInit() {
+	autonomousCommand = autoChooser.GetSelected();
+	autonomousCommand->Start();
+}
+
+void Robot::AutonomousPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+	Log();
+}
+
+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 (autonomousCommand != nullptr) {
+		autonomousCommand->Cancel();
+	}
+	std::cout << "Starting Teleop" << std::endl;
+}
+
+void Robot::TeleopPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+	Log();
+}
+
+void Robot::TestPeriodic() {
+	frc::LiveWindow::GetInstance()->Run();
+}
+
+void Robot::DisabledInit() {
+	shooter->Unlatch();
+}
+
+void Robot::DisabledPeriodic() {
+	Log();
+}
+
+/**
+ * Log interesting values to the SmartDashboard.
+ */
+void Robot::Log() {
+	Robot::pneumatics->WritePressure();
+	frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot->GetAngle());
+	frc::SmartDashboard::PutNumber("Left Distance",
+			drivetrain->GetLeftEncoder()->GetDistance());
+	frc::SmartDashboard::PutNumber("Right Distance",
+			drivetrain->GetRightEncoder()->GetDistance());
+}
+
+START_ROBOT_CLASS(Robot)

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Robot.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 <memory>
+
+#include <Commands/Command.h>
+#include <IterativeRobot.h>
+#include <SmartDashboard/SendableChooser.h>
+
+#include "Commands/DriveAndShootAutonomous.h"
+#include "Commands/DriveForward.h"
+#include "OI.h"
+#include "Subsystems/Collector.h"
+#include "Subsystems/DriveTrain.h"
+#include "Subsystems/Pivot.h"
+#include "Subsystems/Pneumatics.h"
+#include "Subsystems/Shooter.h"
+
+class Robot : public IterativeRobot {
+public:
+	static std::shared_ptr<DriveTrain> drivetrain;
+	static std::shared_ptr<Pivot> pivot;
+	static std::shared_ptr<Collector> collector;
+	static std::shared_ptr<Shooter> shooter;
+	static std::shared_ptr<Pneumatics> pneumatics;
+	static std::unique_ptr<OI> oi;
+
+private:
+	frc::Command* autonomousCommand = nullptr;
+	std::unique_ptr<frc::Command> driveAndShootAuto{
+			new DriveAndShootAutonomous()};
+	std::unique_ptr<frc::Command> driveForwardAuto{new DriveForward()};
+	SendableChooser<frc::Command*> autoChooser;
+
+	void RobotInit() override;
+	void AutonomousInit() override;
+	void AutonomousPeriodic() override;
+	void TeleopInit() override;
+	void TeleopPeriodic() override;
+	void TestPeriodic() override;
+	void DisabledInit() override;
+	void DisabledPeriodic() override;
+
+	void Log();
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.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 "Collector.h"
+
+#include <LiveWindow/LiveWindow.h>
+
+Collector::Collector()
+    : frc::Subsystem("Collector") {
+	// Put everything to the LiveWindow for testing.
+	// XXX: LiveWindow::GetInstance()->AddActuator("Collector", "Roller
+	// Motor", &rollerMotor);
+	LiveWindow::GetInstance()->AddSensor(
+			"Collector", "Ball Detector", &ballDetector);
+	LiveWindow::GetInstance()->AddSensor(
+			"Collector", "Claw Open Detector", &openDetector);
+	LiveWindow::GetInstance()->AddActuator("Collector", "Piston", &piston);
+}
+
+bool Collector::HasBall() {
+	return ballDetector.Get();  // TODO: prepend ! to reflect real robot
+}
+
+void Collector::SetSpeed(double speed) {
+	rollerMotor.Set(-speed);
+}
+
+void Collector::Stop() {
+	rollerMotor.Set(0);
+}
+
+bool Collector::IsOpen() {
+	return openDetector.Get();  // TODO: prepend ! to reflect real robot
+}
+
+void Collector::Open() {
+	piston.Set(true);
+}
+
+void Collector::Close() {
+	piston.Set(false);
+}
+
+void Collector::InitDefaultCommand() {}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.h

@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <Solenoid.h>
+#include <Victor.h>
+
+/**
+ * The Collector subsystem has one motor for the rollers, a limit switch for
+ * ball
+ * detection, a piston for opening and closing the claw, and a reed switch to
+ * check if the piston is open.
+ */
+class Collector : public frc::Subsystem {
+public:
+	// Constants for some useful speeds
+	static constexpr double kForward = 1;
+	static constexpr double kStop = 0;
+	static constexpr double kReverse = -1;
+
+	Collector();
+
+	/**
+	 * NOTE: The current simulation model uses the the lower part of the
+	 * claw
+	 * since the limit switch wasn't exported. At some point, this will be
+	 * updated.
+	 *
+	 * @return Whether or not the robot has the ball.
+	 */
+	bool HasBall();
+
+	/**
+	 * @param speed The speed to spin the rollers.
+	 */
+	void SetSpeed(double speed);
+
+	/**
+	 * Stop the rollers from spinning
+	 */
+	void Stop();
+
+	/**
+	 * @return Whether or not the claw is open.
+	 */
+	bool IsOpen();
+
+	/**
+	 * Open the claw up. (For shooting)
+	 */
+	void Open();
+
+	/**
+	 * Close the claw. (For collecting and driving)
+	 */
+	void Close();
+
+	/**
+	 * No default command.
+	 */
+	void InitDefaultCommand() override;
+
+private:
+	// Subsystem devices
+	frc::Victor rollerMotor{6};
+	frc::DigitalInput ballDetector{10};
+	frc::Solenoid piston{1};
+	frc::DigitalInput openDetector{6};
+};

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

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <cmath>
+
+#include <Joystick.h>
+#include <LiveWindow/LiveWindow.h>
+
+#include "../Commands/DriveWithJoystick.h"
+
+DriveTrain::DriveTrain()
+    : frc::Subsystem("DriveTrain") {
+	// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Left
+	// CIM", &frontLeftCIM);
+	// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front
+	// Right CIM", &frontRightCIM);
+	// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Left
+	// CIM", &backLeftCIM);
+	// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Right
+	// CIM", &backRightCIM);
+
+	// Configure the RobotDrive to reflect the fact that all our motors are
+	// wired backwards and our drivers sensitivity preferences.
+	drive.SetSafetyEnabled(false);
+	drive.SetExpiration(0.1);
+	drive.SetSensitivity(0.5);
+	drive.SetMaxOutput(1.0);
+	drive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
+	drive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
+	drive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
+	drive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
+
+	// Configure encoders
+	rightEncoder->SetPIDSourceType(PIDSourceType::kDisplacement);
+	leftEncoder->SetPIDSourceType(PIDSourceType::kDisplacement);
+
+#ifndef SIMULATION
+	// Converts to feet
+	rightEncoder->SetDistancePerPulse(0.0785398);
+	leftEncoder->SetDistancePerPulse(0.0785398);
+#else
+	// Convert to feet 4in diameter wheels with 360 tick simulated encoders
+	rightEncoder->SetDistancePerPulse(
+			(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
+	leftEncoder->SetDistancePerPulse(
+			(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
+#endif
+
+	LiveWindow::GetInstance()->AddSensor(
+			"DriveTrain", "Right Encoder", rightEncoder);
+	LiveWindow::GetInstance()->AddSensor(
+			"DriveTrain", "Left Encoder", leftEncoder);
+
+// Configure gyro
+#ifndef SIMULATION
+	gyro.SetSensitivity(0.007);  // TODO: Handle more gracefully?
+#endif
+	LiveWindow::GetInstance()->AddSensor("DriveTrain", "Gyro", &gyro);
+}
+
+void DriveTrain::InitDefaultCommand() {
+	SetDefaultCommand(new DriveWithJoystick());
+}
+
+void DriveTrain::TankDrive(Joystick* joy) {
+	drive.TankDrive(joy->GetY(), joy->GetRawAxis(4));
+}
+
+void DriveTrain::TankDrive(double leftAxis, double rightAxis) {
+	drive.TankDrive(leftAxis, rightAxis);
+}
+
+void DriveTrain::Stop() {
+	drive.TankDrive(0.0, 0.0);
+}
+
+std::shared_ptr<Encoder> DriveTrain::GetLeftEncoder() {
+	return leftEncoder;
+}
+
+std::shared_ptr<Encoder> DriveTrain::GetRightEncoder() {
+	return rightEncoder;
+}
+
+double DriveTrain::GetAngle() {
+	return gyro.GetAngle();
+}

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

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <AnalogGyro.h>
+#include <Commands/Subsystem.h>
+#include <Encoder.h>
+#include <RobotDrive.h>
+#include <Victor.h>
+
+namespace frc {
+class Joystick;
+}
+
+/**
+ * The DriveTrain subsystem controls the robot's chassis and reads in
+ * information about it's speed and position.
+ */
+class DriveTrain : public frc::Subsystem {
+public:
+	DriveTrain();
+
+	/**
+	 * When other commands aren't using the drivetrain, allow tank drive
+	 * with
+	 * the joystick.
+	 */
+	void InitDefaultCommand();
+
+	/**
+	 * @param joy PS3 style joystick to use as the input for tank drive.
+	 */
+	void TankDrive(frc::Joystick* joy);
+
+	/**
+	 * @param leftAxis Left sides value
+	 * @param rightAxis Right sides value
+	 */
+	void TankDrive(double leftAxis, double rightAxis);
+
+	/**
+	 * Stop the drivetrain from moving.
+	 */
+	void Stop();
+
+	/**
+	 * @return The encoder getting the distance and speed of left side of
+	 * the drivetrain.
+	 */
+	std::shared_ptr<Encoder> GetLeftEncoder();
+
+	/**
+	 * @return The encoder getting the distance and speed of right side of
+	 * the drivetrain.
+	 */
+	std::shared_ptr<Encoder> GetRightEncoder();
+
+	/**
+	 * @return The current angle of the drivetrain.
+	 */
+	double GetAngle();
+
+private:
+	// Subsystem devices
+	frc::Victor frontLeftCIM{1};
+	frc::Victor rearLeftCIM{2};
+	frc::Victor frontRightCIM{3};
+	frc::Victor rearRightCIM{4};
+	frc::RobotDrive drive{frontRightCIM, rearLeftCIM, frontRightCIM,
+			rearRightCIM};
+	std::shared_ptr<frc::Encoder> rightEncoder =
+			std::make_shared<frc::Encoder>(
+					1, 2, true, Encoder::k4X);
+	std::shared_ptr<frc::Encoder> leftEncoder =
+			std::make_shared<frc::Encoder>(
+					3, 4, false, Encoder::k4X);
+	frc::AnalogGyro gyro{0};
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Pivot.h"
+
+#include <LiveWindow/LiveWindow.h>
+
+Pivot::Pivot()
+    : frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) {
+	SetAbsoluteTolerance(0.005);
+	GetPIDController()->SetContinuous(false);
+#ifdef SIMULATION
+	// PID is different in simulation.
+	GetPIDController()->SetPID(0.5, 0.001, 2);
+	SetAbsoluteTolerance(5);
+#endif
+
+	// Put everything to the LiveWindow for testing.
+	frc::LiveWindow::GetInstance()->AddSensor(
+			"Pivot", "Upper Limit Switch", &upperLimitSwitch);
+	frc::LiveWindow::GetInstance()->AddSensor(
+			"Pivot", "Lower Limit Switch", &lowerLimitSwitch);
+	// XXX: frc::LiveWindow::GetInstance()->AddSensor("Pivot", "Pot", &pot);
+	// XXX: frc::LiveWindow::GetInstance()->AddActuator("Pivot", "Motor",
+	// &motor);
+	frc::LiveWindow::GetInstance()->AddActuator(
+			"Pivot", "PIDSubsystem Controller", GetPIDController());
+}
+
+void InitDefaultCommand() {}
+
+double Pivot::ReturnPIDInput() {
+	return pot.Get();
+}
+
+void Pivot::UsePIDOutput(double output) {
+	motor.PIDWrite(output);
+}
+
+bool Pivot::IsAtUpperLimit() {
+	return upperLimitSwitch.Get();  // TODO: inverted from real robot
+					// (prefix with !)
+}
+
+bool Pivot::IsAtLowerLimit() {
+	return lowerLimitSwitch.Get();  // TODO: inverted from real robot
+					// (prefix with !)
+}
+
+double Pivot::GetAngle() {
+	return pot.Get();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.h

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <DigitalInput.h>
+#include <Victor.h>
+
+/**
+ * The Pivot subsystem contains the Van-door motor and the pot for PID control
+ * of angle of the pivot and claw.
+ */
+class Pivot : public frc::PIDSubsystem {
+public:
+	// Constants for some useful angles
+	static constexpr double kCollect = 105;
+	static constexpr double kLowGoal = 90;
+	static constexpr double kShoot = 45;
+	static constexpr double kShootNear = 30;
+
+	Pivot();
+
+	/**
+	 *  No default command, if PID is enabled, the current setpoint will be
+	 * maintained.
+	 */
+	void InitDefaultCommand() override {}
+
+	/**
+	 * @return The angle read in by the potentiometer
+	 */
+	double ReturnPIDInput() override;
+
+	/**
+	 * Set the motor speed based off of the PID output
+	 */
+	void UsePIDOutput(double output) override;
+
+	/**
+	 * @return If the pivot is at its upper limit.
+	 */
+	bool IsAtUpperLimit();
+
+	/**
+	 * @return If the pivot is at its lower limit.
+	 */
+	bool IsAtLowerLimit();
+
+	/**
+	 * @return The current angle of the pivot.
+	 */
+	double GetAngle();
+
+private:
+	// Subsystem devices
+
+	// Sensors for measuring the position of the pivot
+	frc::DigitalInput upperLimitSwitch{13};
+	frc::DigitalInput lowerLimitSwitch{12};
+
+	/* 0 degrees is vertical facing up.
+	 * Angle increases the more forward the pivot goes.
+	 */
+	frc::AnalogPotentiometer pot{1};
+
+	// Motor to move the pivot
+	frc::Victor motor{5};
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.cpp

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Pneumatics.h"
+
+#include <LiveWindow/LiveWindow.h>
+
+Pneumatics::Pneumatics()
+    : frc::Subsystem("Pneumatics") {
+	frc::LiveWindow::GetInstance()->AddSensor(
+			"Pneumatics", "Pressure Sensor", pressureSensor);
+}
+
+/**
+ * No default command
+ */
+void Pneumatics::InitDefaultCommand() {}
+
+/**
+ * Start the compressor going. The compressor automatically starts and stops as
+ * it goes above and below maximum pressure.
+ */
+void Pneumatics::Start() {
+#ifndef SIMULATION
+	compressor.Start();
+#endif
+}
+
+/**
+ * @return Whether or not the system is fully pressurized.
+ */
+bool Pneumatics::IsPressurized() {
+#ifndef SIMULATION
+	return kMaxPressure <= pressureSensor.GetVoltage();
+#else
+	return true;  // NOTE: Simulation always has full pressure
+#endif
+}
+
+/**
+ * Puts the pressure on the SmartDashboard.
+ */
+void Pneumatics::WritePressure() {
+	frc::SmartDashboard::PutNumber("Pressure", pressureSensor.GetVoltage());
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.h

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <AnalogInput.h>
+#include <Commands/Subsystem.h>
+#include <Compressor.h>
+
+/**
+ * The Pneumatics subsystem contains the compressor and a pressure sensor.
+ *
+ * NOTE: The simulator currently doesn't support the compressor or pressure
+ * sensors.
+ */
+class Pneumatics : public frc::Subsystem {
+public:
+	Pneumatics();
+
+	/**
+	 * No default command
+	 */
+	void InitDefaultCommand() override;
+
+	/**
+	 * Start the compressor going. The compressor automatically starts and
+	 * stops as it goes above and below maximum pressure.
+	 */
+	void Start();
+
+	/**
+	 * @return Whether or not the system is fully pressurized.
+	 */
+	bool IsPressurized();
+
+	/**
+	 * Puts the pressure on the SmartDashboard.
+	 */
+	void WritePressure();
+
+private:
+	frc::AnalogInput pressureSensor{3};
+
+#ifndef SIMULATION
+	frc::Compressor compressor{1};  // TODO: (1, 14, 1, 8);
+#endif
+
+	static constexpr double kMaxPressure = 2.55;
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.cpp

@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Shooter.h"
+
+#include <LiveWindow/LiveWindow.h>
+
+Shooter::Shooter()
+    : Subsystem("Shooter") {
+	// Put everything to the LiveWindow for testing.
+	frc::LiveWindow::GetInstance()->AddSensor(
+			"Shooter", "Hot Goal Sensor", &hotGoalSensor);
+	frc::LiveWindow::GetInstance()->AddSensor("Shooter",
+			"Piston1 Reed Switch Front ", &piston1ReedSwitchFront);
+	frc::LiveWindow::GetInstance()->AddSensor("Shooter",
+			"Piston1 Reed Switch Back ", &piston1ReedSwitchBack);
+	frc::LiveWindow::GetInstance()->AddActuator(
+			"Shooter", "Latch Piston", &latchPiston);
+}
+
+void Shooter::InitDefaultCommand() {
+	// Set the default command for a subsystem here.
+	// SetDefaultCommand(new MySpecialCommand());
+}
+
+void Shooter::ExtendBoth() {
+	piston1.Set(frc::DoubleSolenoid::kForward);
+	piston2.Set(frc::DoubleSolenoid::kForward);
+}
+
+void Shooter::RetractBoth() {
+	piston1.Set(frc::DoubleSolenoid::kReverse);
+	piston2.Set(frc::DoubleSolenoid::kReverse);
+}
+
+void Shooter::Extend1() {
+	piston1.Set(frc::DoubleSolenoid::kForward);
+}
+
+void Shooter::Retract1() {
+	piston1.Set(frc::DoubleSolenoid::kReverse);
+}
+
+void Shooter::Extend2() {
+	piston2.Set(frc::DoubleSolenoid::kReverse);
+}
+
+void Shooter::Retract2() {
+	piston2.Set(frc::DoubleSolenoid::kForward);
+}
+
+void Shooter::Off1() {
+	piston1.Set(frc::DoubleSolenoid::kOff);
+}
+
+void Shooter::Off2() {
+	piston2.Set(frc::DoubleSolenoid::kOff);
+}
+
+void Shooter::Unlatch() {
+	latchPiston.Set(true);
+}
+
+void Shooter::Latch() {
+	latchPiston.Set(false);
+}
+
+void Shooter::ToggleLatchPosition() {
+	latchPiston.Set(!latchPiston.Get());
+}
+
+bool Shooter::Piston1IsExtended() {
+	return !piston1ReedSwitchFront.Get();
+}
+
+bool Shooter::Piston1IsRetracted() {
+	return !piston1ReedSwitchBack.Get();
+}
+
+void Shooter::OffBoth() {
+	piston1.Set(frc::DoubleSolenoid::kOff);
+	piston2.Set(frc::DoubleSolenoid::kOff);
+}
+
+bool Shooter::GoalIsHot() {
+	return hotGoalSensor.Get();
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.h

@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <DoubleSolenoid.h>
+#include <Solenoid.h>
+
+/**
+ * The Shooter subsystem handles shooting. The mechanism for shooting is
+ * slightly complicated because it has to pneumatic cylinders for shooting, and
+ * a third latch to allow the pressure to partially build up and reduce the
+ * effect of the airflow. For shorter shots, when full power isn't needed, only
+ * one cylinder fires.
+ *
+ * NOTE: Simulation currently approximates this as as single pneumatic cylinder
+ * and ignores the latch.
+ */
+class Shooter : public frc::Subsystem {
+public:
+	Shooter();
+	void InitDefaultCommand() override;
+
+	/**
+	 * Extend both solenoids to shoot.
+	 */
+	void ExtendBoth();
+
+	/**
+	 * Retract both solenoids to prepare to shoot.
+	 */
+	void RetractBoth();
+
+	/**
+	 * Extend solenoid 1 to shoot.
+	 */
+	void Extend1();
+
+	/**
+	 * Retract solenoid 1 to prepare to shoot.
+	 */
+	void Retract1();
+
+	/**
+	 * Extend solenoid 2 to shoot.
+	 */
+	void Extend2();
+
+	/**
+	 * Retract solenoid 2 to prepare to shoot.
+	 */
+	void Retract2();
+
+	/**
+	 * Turns off the piston1 double solenoid. This won't actuate anything
+	 * because double solenoids preserve their state when turned off. This
+	 * should be called in order to reduce the amount of time that the coils
+	 * are
+	 * powered.
+	 */
+	void Off1();
+
+	/**
+	 * Turns off the piston1 double solenoid. This won't actuate anything
+	 * because double solenoids preserve their state when turned off. This
+	 * should be called in order to reduce the amount of time that the coils
+	 * are
+	 * powered.
+	 */
+	void Off2();
+
+	/**
+	 * Release the latch so that we can shoot
+	 */
+	void Unlatch();
+
+	/**
+	 * Latch so that pressure can build up and we aren't limited by air
+	 * flow.
+	 */
+	void Latch();
+
+	/**
+	 * Toggles the latch postions
+	 */
+	void ToggleLatchPosition();
+
+	/**
+	 * @return Whether or not piston 1 is fully extended.
+	 */
+	bool Piston1IsExtended();
+
+	/**
+	 * @return Whether or not piston 1 is fully retracted.
+	 */
+	bool Piston1IsRetracted();
+
+	/**
+	 * Turns off all double solenoids. Double solenoids hold their position
+	 * when
+	 * they are turned off. We should turn them off whenever possible to
+	 * extend
+	 * the life of the coils
+	 */
+	void OffBoth();
+
+	/**
+	 * @return Whether or not the goal is hot as read by the banner sensor
+	 */
+	bool GoalIsHot();
+
+private:
+	// Devices
+	frc::DoubleSolenoid piston1{3, 4};
+	frc::DoubleSolenoid piston2{5, 6};
+	frc::Solenoid latchPiston{1, 2};
+	frc::DigitalInput piston1ReedSwitchFront{9};
+	frc::DigitalInput piston1ReedSwitchBack{11};
+	frc::DigitalInput hotGoalSensor{
+			7};  // NOTE: Currently ignored in simulation
+};

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Triggers/DoubleButton.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "DoubleButton.h"
+
+#include <Joystick.h>
+
+DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2) {
+	this->joy = joy;
+	this->button1 = button1;
+	this->button2 = button2;
+}
+
+bool DoubleButton::Get() {
+	return joy->GetRawButton(button1) && joy->GetRawButton(button2);
+}

wpilibcExamples/src/main/cpp/examples/PacGoat/src/Triggers/DoubleButton.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Trigger.h>
+
+namespace frc {
+class Joystick;
+}
+
+class DoubleButton : public frc::Trigger {
+public:
+	DoubleButton(frc::Joystick* joy, int button1, int button2);
+
+	bool Get();
+
+private:
+	frc::Joystick* joy;
+	int button1;
+	int button2;
+};