SwerveModule.h

#pragma once
 
#include <memory>
 
#include <units/angle.h>
 
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
 
#include "frc/drive/RobotDriveBase.h"
#include "frc/geometry/Rotation2d.h"
#include "rmb/motorcontrol/AngularPositionController.h"
#include "rmb/motorcontrol/LinearVelocityController.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
#include "wpi/sendable/Sendable.h"
#include "wpi/sendable/SendableHelper.h"
 
namespace rmb {
 
struct SwerveModulePower {
  double power;          /* <- Power to drive swerve module at. */
  frc::Rotation2d angle; /* <- Angel to drive swerve module at. */
 
  static SwerveModulePower Optimize(const SwerveModulePower &desiredPower,
                                    const frc::Rotation2d &currentAngle);
};
 
class SwerveModule : public wpi::Sendable,
                     public wpi::SendableHelper<SwerveModule> {
public:
  SwerveModule(const SwerveModule &) = delete;
  SwerveModule(SwerveModule &&) = default;
 
  SwerveModule(std::unique_ptr<LinearVelocityController> velocityController,
               std::unique_ptr<AngularPositionController> angularController,
               const frc::Translation2d &moduleTranslation,
               bool breakMode = false);
 
  void setState(const units::meters_per_second_t &velocity,
                const frc::Rotation2d &angle);
 
  void smartdashboardDisplayTargetState(const std::string &name) const;
 
  void setState(const frc::SwerveModuleState &state);
 
  frc::SwerveModuleState getState() const;
 
  frc::SwerveModulePosition getPosition() const;
 
  frc::SwerveModuleState getTargetState() const;
 
  units::meters_per_second_t getTargetVelocity() const;
  frc::Rotation2d getTargetRotation() const;
 
  void setPower(double power, const frc::Rotation2d &angle);
 
  void setPower(const SwerveModulePower &power);
 
  void stop();
 
  SwerveModulePower getPower();
 
  const frc::Translation2d &getModuleTranslation() const;
 
  virtual void InitSendable(wpi::SendableBuilder &builder) override;
 
  double getAngle() {
    return ((units::angle::degree_t)angularController->getPosition())();
  }
 
private:
  std::unique_ptr<AngularPositionController> angularController;
 
  std::unique_ptr<LinearVelocityController> velocityController;
 
  frc::Translation2d moduleTranslation;
 
  bool breakMode;
};
} // namespace rmb