Finished LED code v1 (#135)

* Finished LED code v1 Co-authored-by: Harry C <[email protected]> * Formatting fixes --------- Co-authored-by: Harry C <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
CurtinFRC · Feb 24, 2024 · a08258f · a08258f
1 parent 6ab3a53
commit a08258f
Show file tree

Hide file tree

Showing 8 changed files with 100 additions and 6 deletions.
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,8 +6,8 @@
 
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
-    : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver, LED* led)
+    : _intake(intake), _codriver(codriver), _led(led) {
   Controls(intake);
 }
 
@@ -33,24 +33,30 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     if (_codriver.GetRightBumperPressed()) {
       if (_intake->getState() == IntakeState::kIntake) {
         _intake->setState(IntakeState::kIdle);
+        _led->SetState(LEDState::kIdle);
       } else {
         _intake->setState(IntakeState::kIntake);
+        _led->SetState(LEDState::kIntaking);
       }
     }
 
     if (_codriver.GetLeftBumper()) {
       if (_intake->getState() == IntakeState::kEject) {
         _intake->setState(IntakeState::kIdle);
+        _led->SetState(LEDState::kIdle);
       } else {
         _intake->setState(IntakeState::kEject);
+        _led->SetState(LEDState::kIdle);
       }
     }
 
     if (_codriver.GetAButtonPressed()) {
       if (_intake->getState() == IntakeState::kPass) {
         _intake->setState(IntakeState::kIdle);
+        _led->SetState(LEDState::kIdle);
       } else {
         _intake->setState(IntakeState::kPass);
+        _led->SetState(LEDState::kIntaking);
       }
     }
   }

diff --git a/src/main/cpp/LED.cpp b/src/main/cpp/LED.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "LED.h"
+
+LED::LED() {}
+
+void LED::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case LEDState::kIdle: {
+      _led.frc::PWM::SetSpeed(0);
+    } break;
+
+    case LEDState::kAiming: {
+      _led.frc::PWM::SetSpeed(0.27);
+    } break;
+    case LEDState::kAmpReady: {
+      _led.frc::PWM::SetSpeed(0.87);
+    } break;
+    case LEDState::kHold: {
+      _led.frc::PWM::SetSpeed(0.57);
+    } break;
+    case LEDState::kIntaking: {
+      _led.frc::PWM::SetSpeed(0.07);
+    } break;
+    case LEDState::kShooterReady: {
+      _led.frc::PWM::SetSpeed(0.77);
+    } break;
+
+    default:
+      break;
+  }
+}
+
+void LED::SetState(LEDState state) {
+  _state = state;
+}
+
+LEDState LED::GetState() {
+  return _state;
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -102,6 +102,8 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
+  _led = new LED();
+
   lastPeriodic = wom::now();
 }
 
@@ -126,6 +128,9 @@ void Robot::RobotPeriodic() {
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
+
+  _led->OnUpdate(dt);
+
   _swerveDrive->OnUpdate(dt);
 }
 

diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
@@ -4,8 +4,8 @@
 
 #include "ShooterBehaviour.h"
 
-ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester)
-    : _shooter(shooter), _codriver(tester) {
+ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester, LED* led)
+    : _shooter(shooter), _codriver(tester), _led(led) {
   Controls(shooter);
 }
 
@@ -33,17 +33,22 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     if (_codriver->GetXButton()) {
       _shooter->SetPidGoal(150_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
+      _led->SetState(LEDState::kAiming);
     } else if (_codriver->GetYButton()) {
       _shooter->SetPidGoal(300_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
+      _led->SetState(LEDState::kAiming);
     } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
       _shooter->SetState(ShooterState::kRaw);
+      _led->SetState(LEDState::kIdle);
       _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
     } else if (_codriver->GetRightTriggerAxis() > 0.1) {
       _shooter->SetState(ShooterState::kRaw);
       _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
+      _led->SetState(LEDState::kIdle);
     } else {
       _shooter->SetState(ShooterState::kIdle);
+      _led->SetState(LEDState::kIdle);
     }
   }
 }
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
@@ -7,17 +7,19 @@
 #include <frc/XboxController.h>
 
 #include "Intake.h"
+#include "LED.h"
 #include "Wombat.h"
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
-  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
+  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver, LED* led);
 
   void OnTick(units::second_t dt) override;
 
  private:
   Intake* _intake;
   frc::XboxController& _codriver;
+  LED* _led;
 
   units::volt_t _rawVoltage;
   units::volt_t _setVoltage;
@@ -31,4 +33,6 @@ class IntakeAutoControl : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
+
+  LED* _led;
 };
diff --git a/src/main/include/LED.h b/src/main/include/LED.h
@@ -0,0 +1,26 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/PWM.h>
+
+#include "Wombat.h"
+
+enum class LEDState { kIntaking, kHold, kAiming, kShooterReady, kAmpReady, kIdle };
+
+class LED : public behaviour::HasBehaviour {
+ public:
+  LED();
+
+  void OnUpdate(units::second_t dt);
+
+  void SetState(LEDState state);
+  LEDState GetState();
+
+ private:
+  LEDState _state = LEDState::kIdle;
+
+  frc::PWM _led{0};
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
@@ -19,6 +19,7 @@
 #include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
+#include "LED.h"
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
@@ -58,6 +59,8 @@ class Robot : public frc::TimedRobot {
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
+  LED* _led;
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;

diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
@@ -8,19 +8,22 @@
 
 #include <memory>
 
+#include "LED.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
 class ShooterManualControl : public behaviour::Behaviour {
  public:
-  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver);
+  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver, LED* led);
 
   void OnTick(units::second_t dt) override;
 
  private:
   Shooter* _shooter;
   frc::XboxController* _codriver;
 
+  LED* _led;
+
   bool _rawControl = false;
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");