Added PacGoat code for C++.

Change-Id: I4fd19fbdc65c25c5bbcdce937a31bc6fa1c11cb4

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/CheckForHotGoal.cpp

@@ -0,0 +1,25 @@
+#include "CheckForHotGoal.h"
+
+#include "Robot.h"
+
+CheckForHotGoal::CheckForHotGoal(double time) {
+	SetTimeout(time);
+}
+
+// Called just before this Command runs the first time
+void CheckForHotGoal::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void CheckForHotGoal::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool CheckForHotGoal::IsFinished() {
+	return IsTimedOut() || Robot::shooter->GoalIsHot();
+}
+
+// Called once after isFinished returns true
+void CheckForHotGoal::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void CheckForHotGoal::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/CheckForHotGoal.h

@@ -0,0 +1,22 @@
+#ifndef CheckForHotGoal_H
+#define CheckForHotGoal_H
+
+#include "WPILib.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 : Command {
+public:
+	CheckForHotGoal(double time);
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/CloseClaw.cpp

@@ -0,0 +1,27 @@
+#include "CloseClaw.h"
+
+#include "Robot.h"
+
+CloseClaw::CloseClaw() {
+	Requires(Robot::collector);
+}
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() {
+	Robot::collector->Close();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void CloseClaw::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool CloseClaw::IsFinished() {
+	return true;
+}
+
+// Called once after isFinished returns true
+void CloseClaw::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void CloseClaw::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/CloseClaw.h

@@ -0,0 +1,22 @@
+#ifndef CloseClaw_H
+#define CloseClaw_H
+
+#include "WPILib.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 Command {
+public:
+	CloseClaw();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/Collect.cpp

@@ -0,0 +1,14 @@
+#include "Collect.h"
+
+#include "Robot.h"
+#include "Commands/SetCollectionSpeed.h"
+#include "Commands/CloseClaw.h"
+#include "Commands/SetPivotSetpoint.h"
+#include "Commands/WaitForBall.h"
+
+Collect::Collect() {
+	AddSequential(new SetCollectionSpeed(Collector::FORWARD));
+	AddParallel(new CloseClaw());
+	AddSequential(new SetPivotSetpoint(Pivot::COLLECT));
+	AddSequential(new WaitForBall());
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/Collect.h

@@ -0,0 +1,14 @@
+#ifndef Collect_H
+#define Collect_H
+
+#include "WPILib.h"
+
+/**
+ * Get the robot set to collect balls.
+ */
+class Collect: public CommandGroup {
+public:
+	Collect();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveAndShootAutonomous.cpp

@@ -0,0 +1,20 @@
+#include "DriveAndShootAutonomous.h"
+
+#include "Robot.h"
+#include "Commands/WaitForPressure.h"
+#include "Commands/CloseClaw.h"
+#include "Commands/SetPivotSetpoint.h"
+#include "Commands/DriveForward.h"
+#include "Commands/Shoot.h"
+
+DriveAndShootAutonomous::DriveAndShootAutonomous() {
+    AddSequential(new WaitForPressure(), 2);
+ 	#ifdef REAL
+    	// NOTE: Simulation doesn't currently have the concept of hot.
+    	AddSequential(new CheckForHotGoal(2));
+	#endif
+    AddSequential(new CloseClaw());
+    AddSequential(new SetPivotSetpoint(45));
+    AddSequential(new DriveForward(8, 0.4));
+    AddSequential(new Shoot());
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveAndShootAutonomous.h

@@ -0,0 +1,15 @@
+#ifndef DriveAndShootAutonomous_H
+#define DriveAndShootAutonomous_H
+
+#include "WPILib.h"
+
+/**
+ * Drive over the line and then shoot the ball. If the hot goal is not detected,
+ * it will wait briefly.
+ */
+class DriveAndShootAutonomous: public CommandGroup {
+public:
+	DriveAndShootAutonomous();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveForward.cpp

@@ -0,0 +1,55 @@
+#include "DriveForward.h"
+
+#include "Robot.h"
+
+void DriveForward::init(double dist, double maxSpeed) {
+	Requires(Robot::drivetrain);
+	distance = dist;
+	driveForwardSpeed = maxSpeed;
+	error = 0;
+}
+
+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 (abs(error) <= TOLERANCE) || IsTimedOut();
+}
+
+// Called once after isFinished returns true
+void DriveForward::End() {
+	Robot::drivetrain->Stop();
+}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void DriveForward::Interrupted() {
+	End();
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveForward.h

@@ -0,0 +1,29 @@
+#ifndef DriveForward_H
+#define DriveForward_H
+
+#include "WPILib.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 Command {
+private:
+	double driveForwardSpeed;
+	double distance;
+	double error;
+	static const double TOLERANCE = .1;
+	static const double KP = -1.0 / 5.0;
+	void init(double dist, double maxSpeed);
+public:
+	DriveForward();
+	DriveForward(double dist);
+	DriveForward(double dist, double maxSpeed);
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveWithJoystick.cpp

@@ -0,0 +1,31 @@
+#include "DriveWithJoystick.h"
+
+#include "Robot.h"
+
+DriveWithJoystick::DriveWithJoystick() {
+	Requires(Robot::drivetrain);
+}
+
+// Called just before this Command runs the first time
+void DriveWithJoystick::Initialize() {}
+
+// 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();
+}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void DriveWithJoystick::Interrupted() {
+	End();
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/DriveWithJoystick.h

@@ -0,0 +1,20 @@
+#ifndef DriveWithJoystick_H
+#define DriveWithJoystick_H
+
+#include "WPILib.h"
+
+/**
+ * This command allows PS3 joystick to drive the robot. It is always running
+ * except when interrupted by another command.
+ */
+class DriveWithJoystick: public Command {
+public:
+	DriveWithJoystick();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/ExtendShooter.cpp

@@ -0,0 +1,32 @@
+#include "ExtendShooter.h"
+
+#include "Robot.h"
+
+ExtendShooter::ExtendShooter() {
+	Requires(Robot::shooter);
+	SetTimeout(1);
+}
+
+// Called just before this Command runs the first time
+void ExtendShooter::Initialize() {
+	Robot::shooter->ExtendBoth();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void ExtendShooter::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ExtendShooter::IsFinished() {
+	return IsTimedOut();
+}
+
+// Called once after isFinished returns true
+void ExtendShooter::End() {
+	Robot::shooter->RetractBoth();
+}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ExtendShooter::Interrupted() {
+	End();
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/ExtendShooter.h

@@ -0,0 +1,19 @@
+#ifndef ExtendShooter_H
+#define ExtendShooter_H
+
+#include "WPILib.h"
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+class ExtendShooter: public Command {
+public:
+	ExtendShooter();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/LowGoal.cpp

@@ -0,0 +1,12 @@
+#include "LowGoal.h"
+
+#include "Robot.h"
+#include "Commands/SetPivotSetpoint.h"
+#include "Commands/SetCollectionSpeed.h"
+#include "Commands/ExtendShooter.h"
+
+LowGoal::LowGoal() {
+	AddSequential(new SetPivotSetpoint(Pivot::LOW_GOAL));
+	AddSequential(new SetCollectionSpeed(Collector::REVERSE));
+	AddSequential(new ExtendShooter());
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/LowGoal.h

@@ -0,0 +1,14 @@
+#ifndef LowGoal_H
+#define LowGoal_H
+
+#include "WPILib.h"
+
+/**
+ * Spit the ball out into the low goal assuming that the robot is in front of it.
+ */
+class LowGoal: public CommandGroup {
+public:
+	LowGoal();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/OpenClaw.cpp

@@ -0,0 +1,27 @@
+#include "OpenClaw.h"
+
+#include "Robot.h"
+
+OpenClaw::OpenClaw() {
+	Requires(Robot::collector);
+}
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() {
+	Robot::collector->Open();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void OpenClaw::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool OpenClaw::IsFinished() {
+	return Robot::collector->IsOpen();
+}
+
+// Called once after isFinished returns true
+void OpenClaw::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void OpenClaw::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/OpenClaw.h

@@ -0,0 +1,19 @@
+#ifndef OpenClaw_H
+#define OpenClaw_H
+
+#include "WPILib.h"
+
+/**
+ * Opens the claw
+ */
+class OpenClaw: public Command {
+public:
+	OpenClaw();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/SetCollectionSpeed.cpp

@@ -0,0 +1,28 @@
+#include "SetCollectionSpeed.h"
+
+#include "Robot.h"
+
+SetCollectionSpeed::SetCollectionSpeed(double speed) {
+	Requires(Robot::collector);
+	this->speed = speed;
+}
+
+// Called just before this Command runs the first time
+void SetCollectionSpeed::Initialize() {
+	Robot::collector->SetSpeed(speed);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void SetCollectionSpeed::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetCollectionSpeed::IsFinished() {
+	return true;
+}
+
+// Called once after isFinished returns true
+void SetCollectionSpeed::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void SetCollectionSpeed::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/SetCollectionSpeed.h

@@ -0,0 +1,23 @@
+#ifndef SetCollectionSpeed_H
+#define SetCollectionSpeed_H
+
+#include "WPILib.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 Command {
+private:
+	double speed;
+public:
+	SetCollectionSpeed(double speed);
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/SetPivotSetpoint.cpp

@@ -0,0 +1,30 @@
+#include "SetPivotSetpoint.h"
+
+#include "Robot.h"
+
+SetPivotSetpoint::SetPivotSetpoint(double setpoint) {
+	this->setpoint = setpoint;
+	Requires(Robot::pivot);
+}
+
+// Called just before this Command runs the first time
+void SetPivotSetpoint::Initialize() {
+	Robot::pivot->Enable();
+	Robot::pivot->SetSetpoint(setpoint);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void SetPivotSetpoint::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetPivotSetpoint::IsFinished()
+{
+	return Robot::pivot->OnTarget();
+}
+
+// Called once after isFinished returns true
+void SetPivotSetpoint::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void SetPivotSetpoint::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/SetPivotSetpoint.h

@@ -0,0 +1,24 @@
+#ifndef SetPivotSetpoint_H
+#define SetPivotSetpoint_H
+
+#include "WPILib.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 Command {
+public:
+	SetPivotSetpoint(double setpoint);
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+
+private:
+	double setpoint;
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/Shoot.cpp

@@ -0,0 +1,14 @@
+#include "Shoot.h"
+
+#include "Robot.h"
+#include "Commands/WaitForPressure.h"
+#include "Commands/SetCollectionSpeed.h"
+#include "Commands/OpenClaw.h"
+#include "Commands/ExtendShooter.h"
+
+Shoot::Shoot() {
+	AddSequential(new WaitForPressure());
+	AddSequential(new SetCollectionSpeed(Collector::STOP));
+	AddSequential(new OpenClaw());
+	AddSequential(new ExtendShooter());
+}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/Shoot.h

@@ -0,0 +1,14 @@
+#ifndef Shoot_H
+#define Shoot_H
+
+#include "WPILib.h"
+
+/**
+ * Shoot the ball at the current angle.
+ */
+class Shoot: public CommandGroup {
+public:
+	Shoot();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/WaitForBall.cpp

@@ -0,0 +1,26 @@
+#include "WaitForBall.h"
+
+#include "Robot.h"
+
+WaitForBall::WaitForBall() {
+	Requires(Robot::collector);
+}
+
+// Called just before this Command runs the first time
+void WaitForBall::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void WaitForBall::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForBall::IsFinished()
+{
+	return Robot::collector->HasBall();
+}
+
+// Called once after isFinished returns true
+void WaitForBall::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void WaitForBall::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/WaitForBall.h

@@ -0,0 +1,21 @@
+#ifndef WaitForBall_H
+#define WaitForBall_H
+
+#include "WPILib.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 Command {
+public:
+	WaitForBall();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/WaitForPressure.cpp

@@ -0,0 +1,27 @@
+#include "WaitForPressure.h"
+
+#include "Robot.h"
+
+WaitForPressure::WaitForPressure()
+{
+	Requires(Robot::pneumatics);
+}
+
+// Called just before this Command runs the first time
+void WaitForPressure::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void WaitForPressure::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForPressure::IsFinished()
+{
+	return Robot::pneumatics->IsPressurized();
+}
+
+// Called once after isFinished returns true
+void WaitForPressure::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void WaitForPressure::Interrupted() {}

eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Commands/WaitForPressure.h

@@ -0,0 +1,20 @@
+#ifndef WaitForPressure_H
+#define WaitForPressure_H
+
+#include "WPILib.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 Command {
+public:
+	WaitForPressure();
+	void Initialize();
+	void Execute();
+	bool IsFinished();
+	void End();
+	void Interrupted();
+};
+
+#endif