add a simple scheduling example

CurtinFRC · Mar 2, 2024 · 4f4866b · 4f4866b
1 parent 4eb9aa2
commit 4f4866b
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 18 deletions.
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -21,7 +21,7 @@
 
 #include "behaviour/HasBehaviour.h"
 #include "networktables/NetworkTableInstance.h"
-
+#include "drivetrain/behaviours/SwerveBehaviours.h"
 
 static units::second_t lastPeriodic;
 
@@ -53,7 +53,7 @@ void Robot::RobotInit() {
   // simulation_timer = frc::Timer();
 
   // robotmap.swerveBase.gyro->Reset();
-  
+
 
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
@@ -154,20 +154,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
@@ -30,15 +30,19 @@
 #include "Wombat.h"
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.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;
-  
+
    struct AlphaArmSystem {
     rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
     rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless};
@@ -58,7 +62,7 @@ struct RobotMap {
   AlphaArmSystem alphaArmSystem;
 
 
-  
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};