Generalized CommandBase and moved subsystems to Robot

src/CommandBase.h

@@ -9,50 +9,64 @@
 
 #include <Commands/Command.h>
 #include <yaml-cpp/yaml.h>
-#include "Subsystems/Claw.h"
 #include "Common/Logger.h"
 
 namespace tator {
 
-class Claw;
-class Drive;
-class Shuttle;
-class Thief;
-class ToteFeed;
-class Otto;
-
 /**
- * The base for all commands.
+ * The base for all commands. All subclasses must implement static std::string
+ * GetBaseName() in order to work with Kremlin.
  */
+template<typename T>
 class CommandBase: public Command {
-	friend class Tester;
-	friend class Robot;
-	friend class Shuttle;
 public:
-	// Remeber to implement static std::string GetBaseName()
 	CommandBase(const std::string& name);
 	virtual ~CommandBase();
+
 	/**
 	 * Initializes all static subsystem pointers
 	 */
 	static void InitSubsystems(YAML::Node subsystem);
 
+	static T* robot;
+
 protected:
 	Logger log;
-	virtual void Initialize();
+
+	virtual void Initialize() override;
 	virtual void Execute() = 0;
 	virtual bool IsFinished() = 0;
-	virtual void End();
+	virtual void End() override;
 	virtual void Interrupted();
-
-	static Claw* claw;
-	static Drive* drive;
-	static Shuttle* shuttle;
-	static Thief* thief;
-	static ToteFeed* toteFeed;
-	static Otto* otto;
 };
 
+template<typename T>
+T* CommandBase<T>::robot = nullptr;
+
+template<typename T>
+CommandBase<T>::CommandBase(const std::string& name) :
+		Command(name.c_str()), log(name.c_str()) {
+}
+
+template<typename T>
+CommandBase<T>::~CommandBase() {
+}
+
+template<typename T>
+void CommandBase<T>::Initialize() {
+	log.Info("%s has Initialized", GetName().c_str());
+}
+
+template<typename T>
+void CommandBase<T>::End() {
+	log.Info("%s has Ended", GetName().c_str());
+}
+
+template<typename T>
+void CommandBase<T>::Interrupted() {
+	log.Warn("%s has been Interrupted", GetName().c_str());
+}
+
 } /* namespace tator */
 
 #endif /* COMMANDBASE_H_ */

src/Commands/Claw/ClawClamp.h

@@ -6,15 +6,15 @@
 #ifndef CLAWCLAMP_H
 #define CLAWCLAMP_H
 
-#include "CommandBase.h"
+#include "Robot.h"
 #include "Subsystems/Claw.h"
 
 namespace tator {
 
-class ClawClamp: public CommandBase {
+class ClawClamp: public RobotCommand {
 public:
 	ClawClamp(std::string name, YAML::Node config) :
-			CommandBase(name) {
+			RobotCommand(name) {
 		clampStatus = (Claw::ClampStatus) config["status"].as<int>();
 	}
 
@@ -24,7 +24,7 @@ class ClawClamp: public CommandBase {
 
 protected:
 	void Execute() override {
-		claw->SetClampStatus(clampStatus);
+		robot->claw->SetClampStatus(clampStatus);
 	}
 
 	bool IsFinished() override {

src/Commands/Claw/ClawClampToggle.h

@@ -7,15 +7,15 @@
 #ifndef CLAWCLAMPTOGGLE_H_
 #define CLAWCLAMPTOGGLE_H_
 
-#include "CommandBase.h"
+#include "Robot.h"
 #include "Subsystems/Claw.h"
 
 namespace tator {
 
-class ClawClampToggle: public CommandBase {
+class ClawClampToggle: public RobotCommand {
 public:
 	ClawClampToggle(std::string name, YAML::Node config) :
-			CommandBase(name) {
+			RobotCommand(name) {
 
 	}
 
@@ -25,12 +25,12 @@ class ClawClampToggle: public CommandBase {
 
 protected:
 	void Execute() override {
-		if (claw->GetClampStatus() == Claw::ClampStatus::kDeathGrip) {
+		if (robot->claw->GetClampStatus() == Claw::ClampStatus::kDeathGrip) {
 			log.Info("Releasing claw clamp");
-			claw->SetClampStatus(Claw::ClampStatus::kReleased);
+			robot->claw->SetClampStatus(Claw::ClampStatus::kReleased);
 		} else {
 			log.Info("Death gripping the claw");
-			claw->SetClampStatus(Claw::ClampStatus::kDeathGrip);
+			robot->claw->SetClampStatus(Claw::ClampStatus::kDeathGrip);
 		}
 	}
 

src/Commands/Claw/ClawEstablishHome.h

@@ -1,16 +1,16 @@
 #ifndef CLAWESTABLISHHOME_H_
 #define CLAWESTABLISHHOME_H_
 
-#include "CommandBase.h"
+#include "Robot.h"
 #include "Subsystems/Claw.h"
 #include <DriverStation.h>
 
 namespace tator {
 
-class ClawEstablishHome: public CommandBase {
+class ClawEstablishHome: public RobotCommand {
 public:
 	ClawEstablishHome(std::string name, YAML::Node config) :
-			CommandBase(name) {
+			RobotCommand(name) {
 	}
 
 	static std::string GetBaseName() {
@@ -19,7 +19,7 @@ class ClawEstablishHome: public CommandBase {
 
 protected:
 	void Execute() override {
-		claw->Home();
+		robot->claw->Home();
 	}
 
 	bool IsFinished() override {

src/Commands/Claw/ClawHomeTop.h

@@ -6,15 +6,15 @@
 #ifndef CLAWHOMETOP_H_
 #define CLAWHOMETOP_H_
 
-#include "CommandBase.h"
+#include "Robot.h"
 
 namespace tator {
 
-class ClawHomeTop: public CommandBase {
+class ClawHomeTop: public RobotCommand {
 public:
 	ClawHomeTop(std::string name, YAML::Node config) :
-			CommandBase(name) {
-		Requires(claw);
+			RobotCommand(name) {
+		Requires(robot->claw);
 
 		speed = config["speed"].as<double>();
 	}
@@ -25,8 +25,8 @@ class ClawHomeTop: public CommandBase {
 
 protected:
 	void Initialize() override {
-		CommandBase::Initialize();
-		if (claw->IsHomed()) {
+		RobotCommand::Initialize();
+		if (robot->claw->IsHomed()) {
 			log.Error("Claw is already homed!");
 			this->Cancel();
 		} else {
@@ -35,22 +35,22 @@ class ClawHomeTop: public CommandBase {
 	}
 
 	void Execute() override {
-		claw->ForceSetLiftSpeed(speed);
+		robot->claw->ForceSetLiftSpeed(speed);
 	}
 
 	bool IsFinished() override {
-		return claw->IsAtTop();
+		return robot->claw->IsAtTop();
 	}
 
 	void End() override {
-		CommandBase::End();
-		claw->SetLiftSpeed(0);
-		claw->Home();
+		RobotCommand::End();
+		robot->claw->SetLiftSpeed(0);
+		robot->claw->Home();
 	}
 
 	void Interrupted() override {
-		CommandBase::Interrupted();
-		claw->SetLiftSpeed(0);
+		RobotCommand::Interrupted();
+		robot->claw->SetLiftSpeed(0);
 	}
 
 private:

src/Commands/Claw/ClawSmartRollers.h

@@ -1,18 +1,18 @@
 #ifndef CLAWSMARTROLLERS_H
 #define CLAWSMARTROLLERS_H
 
-#include "CommandBase.h"
+#include "Robot.h"
 #include "Subsystems/Claw.h"
 
 namespace tator {
 
-class ClawSmartRollers: public CommandBase {
+class ClawSmartRollers: public RobotCommand {
 public:
 	ClawSmartRollers(std::string name, YAML::Node config) :
-			CommandBase(name) {
+			RobotCommand(name) {
 		waitTime = config["waitTime"].as<double>();
 		isWaiting = false;
-		Requires(claw);
+		Requires(robot->claw);
 	}
 
 	static std::string GetBaseName() {
@@ -21,16 +21,16 @@ class ClawSmartRollers: public CommandBase {
 
 protected:
 	void Initialize() override {
-		CommandBase::Initialize();
-		claw->SetRollerSpeed(Claw::RollerStatus::kInward);
+		RobotCommand::Initialize();
+		robot->claw->SetRollerSpeed(Claw::RollerStatus::kInward);
 		isWaiting = false;
 		timer.Reset();
 	}
 
 	void Execute() override {
-		if (claw->HasContainer() && !isWaiting) {
+		if (robot->claw->HasContainer() && !isWaiting) {
 			isWaiting = true;
-			claw->SetClampStatus(Claw::ClampStatus::kDeathGrip);
+			robot->claw->SetClampStatus(Claw::ClampStatus::kDeathGrip);
 			timer.Start();
 		}
 	}
@@ -40,15 +40,15 @@ class ClawSmartRollers: public CommandBase {
 	}
 
 	void End() override {
-		claw->SetRollerSpeed(Claw::RollerStatus::kStopped);
+		robot->claw->SetRollerSpeed(Claw::RollerStatus::kStopped);
 		timer.Stop();
-		CommandBase::End();
+		RobotCommand::End();
 	}
 
 	void Interrupted() override {
-		claw->SetRollerSpeed(Claw::RollerStatus::kStopped);
+		robot->claw->SetRollerSpeed(Claw::RollerStatus::kStopped);
 		timer.Stop();
-		CommandBase::Interrupted();
+		RobotCommand::Interrupted();
 	}
 
 private: