diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp index 128cdd2c..401cd0f8 100644 --- 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 index 05b3bf5e..3ba5814a 100644 --- 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(); + } +} \ No newline at end of file diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h index 2e6e5ebb..642d20b2 100644 --- 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; };