SwerveController.h

#pragma once
 
#include "../kinematics/DiffDriveKinematics.h"
#include "../kinematics/SwerveDriveKinematics.h"
 
#include <map>
 
namespace control {
enum class DriveMode {
    Normal,

    TurnInPlace,

    Crab,
};

struct swerve_rots_t {
    int lfRot;
    int rfRot;
    int lbRot;
    int rbRot;
};

struct drive_commands_t {
    double lfPower;
    double rfPower;
    double lbPower;
    double rbPower;
};

class SwerveController {
public:
    SwerveController(double baseWidth, double baseLength, int epsilon);

    drive_commands_t setTurnInPlaceCmdVel(double dtheta,
                                          const swerve_rots_t& wheel_rots) const;

    drive_commands_t setTurnInPlaceCmdVel(double dtheta, const swerve_rots_t& wheel_rots,
                                          double& scaleFactor) const;

    drive_commands_t setCrabCmdVel(double dtheta, double dy,
                                   const swerve_rots_t& wheel_rots) const;

    drive_commands_t setCrabCmdVel(double dtheta, double dy, const swerve_rots_t& wheel_rots,
                                   double& scaleFactor) const;

    const kinematics::SwerveDriveKinematics& swerveKinematics() const;

    void setOverride(bool override);

    bool isOverridden() const;

    DriveMode getDriveMode() const;

    swerve_rots_t setDriveMode(DriveMode mode);

    bool checkWheelRotation(DriveMode mode, const swerve_rots_t& wheel_rots) const;

    swerve_rots_t getSteerRots(DriveMode mode) const;
 
private:
    const kinematics::SwerveDriveKinematics swerve_kinematics;
    const kinematics::DiffDriveKinematics crab_kinematics;
    const double steer_epsilon;

    DriveMode driveMode;

    bool override_steer_check;
};
} // namespace control
 
namespace util {
std::string to_string(control::DriveMode mode);
} // namespace util