Merge pull request #2 from Superbro525Alt/align

Added auto align

src/main/cpp/behaviour/LimelightBehaviours.cpp

@@ -1,2 +1,40 @@
 #include "behaviour/LimelightBehaviours.h"
 
+
+AlignBehaviour::AlignBehaviour(Limelight *limelight, frc::XboxController &driver, wom::SwerveDrive *swerve)
+    : _limelight(limelight), _driver(driver), _swerve(swerve) {
+  Controls(limelight);
+  Controls(swerve);
+}
+
+void AlignBehaviour::AlignToTarget(frc::Pose3d pose) {
+  //swerve->SetPose(pose);
+
+  _state = poseState::notAtPose;
+  _setPoint = pose;
+}
+
+void AlignBehaviour::CancelAlign() {
+  _state = poseState::atPose;
+}
+
+void AlignBehaviour::OnTick(units::second_t dt) {
+    if (_driver.GetAButton()) {
+        AlignToTarget(_SetPoints[0]);
+    } else if (_driver.GetXButton()) {
+        AlignToTarget(_SetPoints[1]);
+    } else if (_driver.GetYButton()) {
+        AlignToTarget(_SetPoints[2]);
+    } else if (_driver.GetBButton()) {
+        AlignToTarget(_SetPoints[3]);
+    } else if (_driver.GetBackButtonPressed()) {
+        CancelAlign();
+    }
+
+    if (_state == poseState::notAtPose && !_limelight->IsAtSetPoseVision(_setPoint, dt)) {
+        _swerve->SetPose(_setPoint.ToPose2d());
+    }
+    else {
+        _state = poseState::atPose;
+    }
+}

src/main/include/behaviour/LimelightBehaviours.h

@@ -5,12 +5,44 @@
 
 // wom includes
 #include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
 
 // std lib includes
+#include <vector>
+#include <memory>
+
 
 // units includes
 
 // wpilib includes
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Pose2d.h>
+#include "frc/XboxController.h"
 
 // other external includes
 
+enum class poseState {
+  atPose,
+  notAtPose
+};
+
+class AlignBehaviour : public behaviour::Behaviour {
+ public:
+  AlignBehaviour(Limelight *limelight, frc::XboxController &driver, wom::SwerveDrive *swerve);
+
+  void OnTick(units::second_t dt) override;
+
+  void AlignToTarget(frc::Pose3d pose);
+  void CancelAlign();
+
+private:
+  Limelight *_limelight;
+  frc::XboxController &_driver;
+  wom::SwerveDrive *_swerve;
+
+  std::vector<frc::Pose3d> _SetPoints;
+  frc::Pose3d _setPoint;
+
+
+  poseState _state = poseState::notAtPose;
+};