[wombat/behaviours] Create Trigger API

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,11 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Selector": "String Chooser"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/cpp/AlphaArm.cpp

@@ -22,7 +22,6 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       _setAlphaArmVoltage = 0_V;
       break;
     case AlphaArmState::kRaw:
-      std::cout << "RawControl" << std::endl;
       _setAlphaArmVoltage = _rawArmVoltage;
 
       break;
@@ -73,8 +72,6 @@ void AlphaArm::OnUpdate(units::second_t dt) {
   }
   _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
   _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
-  std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))
-            << std::endl;
   _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
   _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
   _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));
@@ -84,8 +81,6 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
   _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
   _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());
-
-  std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
 }
 
 void AlphaArm::SetState(AlphaArmState state) {

src/main/cpp/Robot.cpp

@@ -152,19 +152,7 @@ void Robot::TeleopInit() {
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
 
-  // frontLeft->SetVoltage(4_V);
-  // frontRight->SetVoltage(4_V);
-  // backLeft->SetVoltage(4_V);
-  // backRight->SetVoltage(4_V);
-
-  //  FMAP("fmap.fmap");
-
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
-
-  // reimplement when vision is reimplemented
-
-  // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
+  robotmap.controllers.triggerDriver->XButton()->SetTrueBehaviour(wom::make<wom::XDrivebase>(_swerveDrive));
 }
 
 void Robot::TeleopPeriodic() {}

src/main/include/RobotMap.h

@@ -26,12 +26,16 @@
 #include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
+#include "behaviour/TriggerXboxController.h"
+#include "utils/Encoder.h"
+#include "utils/PID.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
+    wom::TriggerXboxController* triggerDriver = new wom::TriggerXboxController(0);
   };
   Controllers controllers;
 

wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp

@@ -4,6 +4,8 @@
 
 #include "behaviour/BehaviourScheduler.h"
 
+#include "behaviour/Trigger.h"
+
 using namespace behaviour;
 
 BehaviourScheduler::BehaviourScheduler() {}
@@ -73,6 +75,10 @@ void BehaviourScheduler::Tick() {
       Schedule(sys->_default_behaviour_producer());
     }
   }
+
+  for (Trigger* trigger : m_triggers) {
+    trigger->OnTick();
+  }
 }
 
 void BehaviourScheduler::InterruptAll() {
@@ -83,3 +89,13 @@ void BehaviourScheduler::InterruptAll() {
     }
   }
 }
+
+void BehaviourScheduler::AddTrigger(Trigger* trigger) {
+  for (Trigger* i : m_triggers) {
+    if (i->GetName() == trigger->GetName()) {
+      throw std::invalid_argument("Cannot reuse Triggers!");
+      return;
+    }
+  }
+  m_triggers.emplace_back(trigger);
+}

wombat/src/main/cpp/behaviour/Trigger.cpp

@@ -0,0 +1,88 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "behaviour/Trigger.h"
+
+#include <iostream>
+
+#include "behaviour/BehaviourScheduler.h"
+
+using namespace behaviour;
+
+Trigger::Trigger(std::string name) : k_name{name}, m_condition{[]() { return false; }} {
+  behaviour::BehaviourScheduler::GetInstance()->AddTrigger(this);
+}
+
+Trigger::Trigger(std::function<bool()> condition, std::string name) : k_name{name}, m_condition(condition) {
+  behaviour::BehaviourScheduler::GetInstance()->AddTrigger(this);
+}
+
+Trigger::Trigger(std::function<bool()> condition, std::string name, Behaviour::ptr&& true_behaviour)
+    : k_name{name}, m_condition(condition), m_true_behaviour{true_behaviour} {
+  behaviour::BehaviourScheduler::GetInstance()->AddTrigger(this);
+}
+
+Trigger::Trigger(std::function<bool()> condition, std::string name, Behaviour::ptr&& true_behaviour,
+                 Behaviour::ptr&& false_behaviour)
+    : k_name{name},
+      m_condition(condition),
+      m_true_behaviour{true_behaviour},
+      m_false_behaviour{false_behaviour} {
+  behaviour::BehaviourScheduler::GetInstance()->AddTrigger(this);
+}
+
+std::string Trigger::GetName() {
+  return k_name;
+}
+
+void Trigger::OnTick() {
+  if (m_condition()) {
+    if (m_true_behaviour != nullptr) {
+      if (m_true_behaviour->IsFinished()) {
+        BehaviourScheduler::GetInstance()->Schedule(m_true_behaviour);
+      }
+      return;
+    }
+    std::cerr << "WARNING: No OnTrue behaviour specified for Trigger " << k_name << std::endl;
+  }
+  if (m_false_behaviour != nullptr) {
+    if (m_false_behaviour->IsFinished()) {
+      BehaviourScheduler::GetInstance()->Schedule(m_false_behaviour);
+    }
+    return;
+  }
+  std::cerr << "WARNING: No OnFalse behaviour specified for Trigger " << k_name << std::endl;
+}
+
+void Trigger::SetTrueBehaviour(Behaviour::ptr&& behaviour) {
+  m_true_behaviour = behaviour;
+}
+
+void Trigger::SetFalseBehaviour(Behaviour::ptr&& behaviour) {
+  m_false_behaviour = behaviour;
+}
+
+Trigger* Trigger::operator!() {
+  return new Trigger([condition = m_condition]() { return condition(); }, "Not" + k_name);
+}
+
+Trigger* Trigger::operator||(std::pair<std::function<bool()>, std::string> rhs) {
+  return new Trigger([condition = m_condition, rhs = std::move(rhs)]() { return condition() || rhs.first(); },
+                     rhs.second);
+}
+
+Trigger* Trigger::operator||(std::function<bool()> rhs) {
+  return new Trigger([condition = m_condition, rhs = std::move(rhs)]() { return condition() || rhs(); },
+                     "Logical or of " + k_name);
+}
+
+Trigger* Trigger::operator&&(std::pair<std::function<bool()>, std::string> rhs) {
+  return new Trigger([condition = m_condition, rhs = std::move(rhs)]() { return condition() && rhs.first(); },
+                     rhs.second);
+}
+
+Trigger* Trigger::operator&&(std::function<bool()> rhs) {
+  return new Trigger([condition = m_condition, rhs = std::move(rhs)]() { return condition() && rhs(); },
+                     "Logical and of " + k_name);
+}