finished intake PID

prawny-boy · Feb 29, 2024 · fe36dd3 · fe36dd3
1 parent abec6e5
commit fe36dd3
Show file tree

Hide file tree

Showing 3 changed files with 71 additions and 5 deletions.
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
@@ -54,9 +54,9 @@ void Intake::OnUpdate(units::second_t dt) {
       }
       _pid.SetSetpoint(0);
       units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
+          // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
       // units::volt_t pidCalculate =
-      //     units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+          units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       _setVoltage = pidCalculate;
       _stringStateName = "Hold";
     }

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
@@ -58,4 +58,43 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
-void AutoIntake::OnTick(units::second_t dt) {}
+
+void AutoIntake::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kIntake);
+}
+
+IntakeNote::IntakeNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void IntakeNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kIntake);
+
+  if (_intake->GetState() == IntakeState::kHold) {
+    SetDone();
+  }
+}
+
+PassNote::PassNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void PassNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kPass);
+
+  if (_intake->GetState() == IntakeState::kIdle) {
+    SetDone();
+  }
+}
+
+EjectNote::EjectNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void EjectNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kEject);
+
+  if (_intake->GetState() == IntakeState::kIdle) {
+    SetDone();
+  }
+}
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
@@ -9,8 +9,6 @@
 #include "Intake.h"
 #include "Wombat.h"
 
-enum class IntakeBehaviourState { kIntaking, kEjecting, kPassing, kIdleing};
-
 class IntakeManualControl : public behaviour::Behaviour {
  public:
   explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
@@ -30,5 +28,34 @@ class AutoIntake : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
+};
+
+class IntakeNote : public behaviour::Behaviour {
+ public:
+  explicit IntakeNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+
+};
+
+class PassNote : public behaviour::Behaviour {
+ public:
+  explicit PassNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+
+};
+
+class EjectNote : public behaviour::Behaviour {
+ public:
+  explicit EjectNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
 
 };