From 5a388c04e2128b865ea3ab9dc23ea39480a3e512 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 11 Feb 2024 11:57:55 +0800
Subject: [PATCH] add a simple scheduling example

---
 src/main/cpp/Robot.cpp      | 15 ++-------------
 src/main/include/RobotMap.h |  4 ++++
 2 files changed, 6 insertions(+), 13 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index af9d93d2..cfdf9328 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -152,19 +152,8 @@ 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::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d(1_m, 0_m, frc::Rotation2d(0_rad))));
 }
 
 void Robot::TeleopPeriodic() {}
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e8cb215b..8caea92f 100644
--- a/src/main/include/RobotMap.h
+++ b/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;