auto_aim_command.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <argos_lib/general/debouncer.h>
#include <argos_lib/subsystems/swappable_controllers_subsystem.h>
#include <frc2/command/Command.h>
#include <frc2/command/CommandHelper.h>
 
#include "subsystems/elevator_subsystem.h"
#include "subsystems/shooter_subsystem.h"
#include "subsystems/simple_led_subsystem.h"
#include "subsystems/swerve_drive_subsystem.h"
#include "subsystems/vision_subsystem.h"
 
class AutoAimCommand : public frc2::CommandHelper<frc2::Command, AutoAimCommand> {
 public:
  AutoAimCommand(SwerveDriveSubsystem* swerveDrive,
                 ShooterSubsystem* shooter,
                 ElevatorSubsystem* elevator,
                 VisionSubsystem* vision,
                 argos_lib::SwappableControllersSubsystem* controllers,
                 SimpleLedSubsystem* leds,
                 bool endWhenAimed = false);
 
  void Initialize() override;
 
  void Execute() override;
 
  void End(bool interrupted) override;
 
  bool IsFinished() override;
 
 private:
  SwerveDriveSubsystem* m_pSwerveDrive;
  ShooterSubsystem* m_pShooter;
  ElevatorSubsystem* m_pElevator;
  VisionSubsystem* m_pVision;
  argos_lib::SwappableControllersSubsystem* m_pControllers;
  SimpleLedSubsystem* m_pLeds;
  bool m_endWhenAimed;
  argos_lib::Debouncer m_aimedDebouncer;
  std::chrono::time_point<std::chrono::steady_clock> m_startTime;
 
  struct AimParams {
    units::degree_t carriageAngle;
    units::angular_velocity::revolutions_per_minute_t shooterSpeed;
    units::degree_t targetAngleOffset;
    units::degree_t cameraAngleOffset;
  };
 
  [[nodiscard]] std::optional<AimParams> GetAimParams();
 
  [[nodiscard]] std::optional<units::degree_t> GetAdjustmentOffset(const std::optional<AimParams>& params);
 
  [[nodiscard]] bool Aimed(const std::optional<AimParams>& params);
 
  [[nodiscard]] bool Aim(const std::optional<AimParams>& params);
 
  void SetOperatorFeedback(const std::optional<AimParams>& params);
};