Swerve.h

 
#pragma once
 
#include <deque>
#include <memory>
#include <string>
#include <vector>
 
#include <choreo/Choreo.h>
#include <Eigen/Core>
#include <ctre/phoenix6/Pigeon2.hpp>
#include <frc/DriverStation.h>
#include <frc/EigenCore.h>
#include <frc/Notifier.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <frc2/command/InstantCommand.h>
#include <networktables/StructArrayTopic.h>
#include <networktables/StructTopic.h>
#include <pathplanner/lib/config/RobotConfig.h>
#include <pathplanner/lib/util/swerve/SwerveSetpoint.h>
#include <pathplanner/lib/util/swerve/SwerveSetpointGenerator.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include "valkyrie/BaseSubsystem.h"
#include "valkyrie/CharMode.h"
#include "valkyrie/controllers/PIDF.h"
#include "valkyrie/drivetrain/SwerveModule.h"
#include "valkyrie/util/LockedAccess.h"
 
#define MODULE_COUNT 4
 
namespace valor {
 
class Swerve : public valor::BaseSubsystem {
   public:
    CharMode::MODE_OPTIONS selectedTest;
 
    Swerve(std::string name, std::vector<std::pair<BaseController*, BaseController*>> modules, units::meter_t module_radius,
           units::meter_t _wheelDiameter, std::vector<int> moduleCoordsX, std::vector<int> moduleCoordsY, units::meter_t driveBaseRadius);
    ~Swerve();
 
    void analyzeDashboard() override;
    void assignOutputs() override;
    void LoggablePeriodic() override;
    void reset() override;
 
    void SimulationPeriodic() override;
 
    frc::Pose2d getRawPose();
    frc::Pose2d getCalculatedPose();
 
    void setupGyro(int, const char*, units::degree_t, units::degree_t, units::degree_t);
    void resetGyro();
    frc::Rotation2d getGyro();
 
    void resetOdometry(frc::Pose2d pose);
    void resetEncoders();
 
    wpi::array<frc::SwerveModulePosition, MODULE_COUNT> getModulePositions();
 
    wpi::array<frc::SwerveModuleState, MODULE_COUNT> getModuleStates();
 
    wpi::array<frc::SwerveModuleState, MODULE_COUNT> calculateModuleStates(frc::ChassisSpeeds chassisSpeeds, bool isFOC);
 
    void updateAngularPosition();
 
    void drive(units::velocity::meters_per_second_t vx_mps, units::velocity::meters_per_second_t vy_mps,
               units::angular_velocity::radians_per_second_t omega_radps, bool isFOC);
 
    void drive(frc::ChassisSpeeds _chassisSpeeds, bool isFOC);
 
    void driveRobotRelative(frc::ChassisSpeeds speeds);
 
    double getSkiddingRatio();
    bool isRobotSkidding();
 
    frc::Pose2d cachedRawPose;
    frc::Pose2d cachedCalculatedPose;
    frc::Pose2d cachedPredictedPose;
 
    frc::ChassisSpeeds cachedChassisFieldSpeed;
    frc::ChassisSpeeds cachedRobotRelativeSpeed;
 
    void updateAngularAcceleration();
    units::angular_acceleration::radians_per_second_squared_t getSmoothedAngularAcceleration();
    double rotationLerping(double);
    void followTrajectory(const choreo::SwerveSample& sample);
 
    units::meter_t getDriveBaseRadius() { return driveBaseRadius; };
 
   protected:
    double xSpeed;
    double ySpeed;
    double rotSpeed;
 
    frc::ChassisSpeeds commandedSpeeds;
 
    units::meters_per_second_t maxDriveSpeed;
    units::radians_per_second_t maxRotationSpeed;
 
    std::unique_ptr<ctre::phoenix6::hardware::Pigeon2> pigeon;
    std::unique_ptr<frc::SwerveDriveKinematics<MODULE_COUNT>> kinematics;
    util::Locked<std::unique_ptr<frc::SwerveDrivePoseEstimator<MODULE_COUNT>>> rawEstimator;
    util::Locked<std::unique_ptr<frc::SwerveDrivePoseEstimator<MODULE_COUNT>>> calcEstimator;
 
    bool toast;
 
    void enableCarpetGrain(double grainMultiplier, bool roughTowardsRed);
 
    frc::ChassisSpeeds vectorLockedSpeeds(const frc::ChassisSpeeds speeds, const frc::Rotation2d heading);
 
    frc::ChassisSpeeds getRobotRelativeSpeeds();
    frc::ChassisSpeeds getFieldRelativeSpeeds();
    void setSwerveDesiredState(wpi::array<frc::SwerveModuleState, MODULE_COUNT> desiredStates, bool isDriveOpenLoop);
 
    units::radian_t angularPosition = 0_rad;
 
    wpi::array<frc::SwerveModuleState, MODULE_COUNT> testModeDesiredStates{wpi::empty_array};
 
    std::vector<valor::SwerveModule*> swerveModules;
 
    void startOdomThread(units::hertz_t freq);
    void updateOdometry();
 
    static frc::ChassisSpeeds reflectDriveSpeeds(const frc::ChassisSpeeds unreflectedSpeeds) {
        if (frc::DriverStation::GetAlliance().value_or(frc::DriverStation::Alliance::kBlue) == frc::DriverStation::Alliance::kRed) {
            return {-unreflectedSpeeds.vx, -unreflectedSpeeds.vy, unreflectedSpeeds.omega};
        }
        return unreflectedSpeeds;
    }
 
    frc::Timer resetOdom;
 
   private:
    std::deque<units::angular_acceleration::radians_per_second_squared_t> yawRateBuffer;  // TODO Replace with a moving average filter
 
    static constexpr size_t ACCEL_BUFFER_SIZE = 10;  // need to adjust // TODO: Replace with an actual moving average filter
    units::angular_velocity::radians_per_second_t lastYawRate = 0_rad_per_s;  // TODO: Replace with a moving average filter
 
    units::angular_acceleration::radians_per_second_squared_t angularAcceleration = 0_rad_per_s_sq;
 
    double _drivetrain_accel;
 
    bool useCarpetGrain;
    double carpetGrainMultiplier;
    bool roughTowardsRed;
    void calculateCarpetPose();
 
    bool rotTest;
    bool strLineTest;
 
    CharMode charac;  // TODO: Switch to WPI SysID
 
    units::meter_t driveBaseRadius;
 
    // Cached telemetry values (computed in analyzeDashboard, published in LoggablePeriodic)
    units::degree_t cachedGyroPitch{0_deg};
    units::degree_t cachedGyroYaw{0_deg};
    units::degree_t cachedGyroRoll{0_deg};
    wpi::array<frc::SwerveModuleState, MODULE_COUNT> cachedModuleStates{wpi::empty_array};
    units::degrees_per_second_t cachedAngularVelocity{0_deg_per_s};
 
    frc::Notifier odometryThread{[this] { updateOdometry(); }};
 
    frc::PIDController xChoreoController{10.0, 0.0, 0.0};
    frc::PIDController yChoreoController{10.0, 0.0, 0.0};
    frc::PIDController headingChoreoController{7.5, 0.0, 0.0};
 
    // SwerveSetpointGenerator setpointGenerator;
    // SwerveSetpoint previousSetpoint;
};
 
}  // namespace valor