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: