auto_command.h

 
#pragma once
 
#include "vex.h"
#include <atomic>
#include <functional>
#include <queue>
#include <vector>
 
class Condition {
public:
  Condition *Or(Condition *b);
  Condition *And(Condition *b);
  virtual bool test() = 0;
};
 
class AutoCommand {
public:
  static constexpr double default_timeout = 10.0;
  virtual bool run() { return true; }
  virtual void on_timeout() {}
  AutoCommand *withTimeout(double t_seconds) {
    if (this->timeout_seconds < 0) {
      // should never be timed out
      return this;
    }
    this->timeout_seconds = t_seconds;
    return this;
  }
  AutoCommand *withCancelCondition(Condition *true_to_end) {
    this->true_to_end = true_to_end;
    return this;
  }
  double timeout_seconds = default_timeout;
  Condition *true_to_end = nullptr;
};
 
class FunctionCommand : public AutoCommand {
public:
  FunctionCommand(std::function<bool(void)> f) : f(f) {}
  bool run() { return f(); }
 
private:
  std::function<bool(void)> f;
};
 
// Times tested 3
// Test 1 -> false
// Test 2 -> false
// Test 3 -> true
// Returns false until the Nth time that it is called
// This is pretty much only good for implementing RepeatUntil
class TimesTestedCondition : public Condition {
public:
  TimesTestedCondition(size_t N) : max(N) {}
  bool test() override {
    count++;
    if (count >= max) {
      return true;
    }
    return false;
  }
 
private:
  size_t count = 0;
  size_t max;
};
 
class FunctionCondition : public Condition {
public:
  FunctionCondition(
      std::function<bool()> cond, std::function<void(void)> timeout = []() {})
      : cond(cond), timeout(timeout) {}
  bool test() override;
 
private:
  std::function<bool()> cond;
  std::function<void(void)> timeout;
};
 
class IfTimePassed : public Condition {
public:
  IfTimePassed(double time_s);
  bool test() override;
 
private:
  double time_s;
  vex::timer tmr;
};
 
class WaitUntilCondition : public AutoCommand {
public:
  WaitUntilCondition(Condition *cond) : cond(cond) {}
  bool run() override { return cond->test(); }
 
private:
  Condition *cond;
};
 
 
class InOrder : public AutoCommand {
public:
  InOrder(const InOrder &other) = default;
  InOrder(std::queue<AutoCommand *> cmds);
  InOrder(std::initializer_list<AutoCommand *> cmds);
  bool run() override;
  void on_timeout() override;
 
private:
  AutoCommand *current_command = nullptr;
  std::queue<AutoCommand *> cmds;
  vex::timer tmr;
};
 
class Parallel : public AutoCommand {
public:
  Parallel(std::initializer_list<AutoCommand *> cmds);
  bool run() override;
  void on_timeout() override;
 
private:
  std::vector<AutoCommand *> cmds;
  std::vector<vex::task *> runners;
};
 
class Branch : public AutoCommand {
public:
  Branch(Condition *cond, AutoCommand *false_choice, AutoCommand *true_choice);
  ~Branch();
  bool run() override;
  void on_timeout() override;
 
private:
  AutoCommand *false_choice;
  AutoCommand *true_choice;
  Condition *cond;
  bool choice = false;
  bool chosen = false;
  vex::timer tmr;
};
 
class Async : public AutoCommand {
public:
  Async(AutoCommand *cmd) : cmd(cmd) {}
  bool run() override;
 
private:
  AutoCommand *cmd = nullptr;
};
 
class RepeatUntil : public AutoCommand {
public:
  RepeatUntil(InOrder cmds, size_t repeats);
  RepeatUntil(InOrder cmds, Condition *true_to_end);
  bool run() override;
  void on_timeout() override;
 
private:
  const InOrder cmds;
  InOrder *working_cmds;
  Condition *cond;
};