ChassisSubsystem.h

Go to the documentation of this file.

#ifndef TR_EMBEDDED_CHASSIS_SUBSYSTEM_H
#define TR_EMBEDDED_CHASSIS_SUBSYSTEM_H
 
#include "mbed.h"
#include "util/peripherals/imu/IMU.h"
#include "util/communications/PwmIn.h"
#include "util/peripherals/encoder/MA4.h"
 
#include <util/motor/DJIMotor.h>
#include <util/communications/CANHandler.h>
#include <util/peripherals/oled/SSD1308.h>
// #include <subsystems/ChassisKalman.h>
// #include <algorithms/WheelKalman.h>
#include <util/algorithms/PID.h>
// #include <algorithms/WheelSpeeds.h>
// #include <algorithms/ChassisSpeeds.h>
 
#define CAN_BUS_TYPE CANHandler::CANBUS_1
#define MOTOR_TYPE M3508
#define M3508_POST_MAX_RPM 469
#define INPUT_THRESHOLD 0.01
 
#define PI 3.14159265
#define SECONDS_PER_MINUTE 60
#define TICKS_PER_ROTATION 8192.0
#define WHEEL_DIAMETER_METERS 0.146
#define WHEEL_TO_CHASSIS_CENTER_LX 0.21
#define WHEEL_TO_CHASSIS_CENTER_LY 0.14
 
#define MAX_BEYBLADE_SPEED 1800
#define BEYBLADE_ACCELERATION 0.05
#define MAX_VEL 2.92
 
struct OmniKinematics
{
    double r1x, r1y, r2x, r2y, r3x, r3y, r4x, r4y;
};
 
struct WheelSpeeds
{
    double LF;
    double RF;
    double LB;
    double RB;
 
    void operator*=(double scalar)
    {
        LF *= scalar;
        RF *= scalar;
        LB *= scalar;
        RB *= scalar;
    }
};
 
struct ChassisSpeeds
{
    double vX;
    double vY;
    double vOmega;
};
 
struct OmniKinematicsLimits
{
    double max_Vel;
    double max_vOmega;
};
 
class ChassisSubsystem
{
public:
    
    struct Config {
        int left_front_can_id;
        int right_front_can_id;
        int left_back_can_id;
        int right_back_can_id;
 
        const double radius;
        const double speed_pid_ff_ks;
 
        float yaw_initial_offset_ticks;
        IMU &imu;
        MA4 *encoder;  // External encoder for yaw position
    };
 
    ChassisSubsystem(const Config &config);
 
    enum BrakeMode
    {
        BRAKE,
        COAST
    };
 
    enum MotorLocation
    {
        LEFT_FRONT,
        RIGHT_FRONT,
        LEFT_BACK,
        RIGHT_BACK
    };
 
    enum SPEED_UNIT
    {
        RAD_PER_SECOND,
        RPM,
        METER_PER_SECOND,
    };
 
    enum DRIVE_MODE
    {
        YAW_ORIENTED,
        REVERSE_YAW_ORIENTED,
        ROBOT_ORIENTED,
        ODOM_ORIENTED,
        YAW_ALIGN
    };
 
    enum HOLONOMIC_MODE
    {
        OMNI,
        MECANUM
    };
 
    float previousRPM[4] = {0, 0, 0, 0};
 
    static float limitAcceleration(float desiredRPM, float previousRPM, int power);
 
    static double p_theory(int LeftFrontPower, int RightFrontPower, int LeftBackPower, int RightBackPower, int LeftFrontRpm, int RightFrontRpm, int LeftBackRpm, int RightBackRpm);
 
    float estimatePowerWatts(int torqueCounts);
 
    static double Bisection(int LeftFrontPower, int RightFrontPower, int LeftBackPower, int RightBackPower, int LeftFrontRpm, int RightFrontRpm, int LeftBackRpm, int RightBackRpm, float chassisPowerLimit);
 
    float power_limit;
 
    WheelSpeeds getWheelSpeeds() const;
 
    float setWheelSpeeds(WheelSpeeds wheelSpeeds);
 
    WheelSpeeds normalizeWheelSpeeds(WheelSpeeds wheelSpeeds) const;
 
    void setWheelPower(WheelSpeeds wheelPower);
 
    ChassisSpeeds getChassisSpeeds() const;
 
    float setChassisSpeeds(ChassisSpeeds desiredChassisSpeeds_, DRIVE_MODE mode = ROBOT_ORIENTED);
 
    ChassisSpeeds rotateChassisSpeed(ChassisSpeeds speeds, double yawCurrent);
 
    DJIMotor &getMotor(MotorLocation location);
 
    void setMotorSpeedPID(MotorLocation location, float kP, float kI, float kD);
 
    void setSpeedIntegralCap(MotorLocation location, double cap);
 
    void setSpeedFeedforward(MotorLocation location, double FF);
 
 
    BrakeMode getBrakeMode();
 
    void setBrakeMode(BrakeMode brakeMode);
 
    void initializeImu();
 
    void periodic(IMU::EulerAngles *imuCurr = nullptr);
 
    static double radiansToDegrees(double radians);
 
    static double degreesToRadians(double degrees);
 
    int getHeadingDegrees() const;
 
 
    double getMotorSpeed(MotorLocation location, SPEED_UNIT unit);
 
    void setOmniKinematicsLimits(double max_Vel, double max_vOmega);
 
    void readImu();
 
 
    bool setOdomReference();
 
    ChassisSpeeds desiredChassisSpeeds;
    WheelSpeeds desiredWheelSpeeds;
 
    OmniKinematicsLimits m_OmniKinematicsLimits;
    WheelSpeeds chassisSpeedsToWheelSpeeds(ChassisSpeeds chassisSpeeds);
    ChassisSpeeds wheelSpeedsToChassisSpeeds(WheelSpeeds wheelSpeeds);
 
    ChassisSpeeds m_chassisSpeeds;
    WheelSpeeds m_wheelSpeeds;
    
    int PEAK_POWER_ALL;
    int PEAK_POWER_SINGLE;
 
    PID pid_LF;
    PID pid_RF;
    PID pid_LB;
    PID pid_RB;
    
    PID pid_align;
    double yaw_velo_gain = 0;
    double yawPrior; // previous yaw for yaw_align
 
    uint32_t lastPIDTime = 0;
 
 
    double yawOdom;
    double imuOdom;
    int testData[300][4];
    int testDataIndex = 0;
 
    static double radiansToTicks(double radians);
    static double ticksToRadians(double ticks);
    
    // /**
    //  * Gets the yaw position from encoder (PWM) input in degrees (0-360)
    //  * @return yaw position in degrees, or -1 if encoder not available
    //  */
    // double getEncoderYawPosition();
 
    // /**
    //  * A helper method to calculate the moving average of the encoder readings for yaw position
    //  * @return the moving average of the encoder readings for yaw position
    //  */
    // double encoderMovingAverage();
    
    void updateYawPhaseFromEncoder();
 
private:
    DJIMotor LF, RF, LB, RB;
    DJIMotor *yaw = 0;
    MA4 *encoder = nullptr;
    double yawPhase;
    BrakeMode brakeMode;
 
    // double beybladeSpeed;
    // bool beybladeIncreasing;
    IMU &imu;
    IMU::EulerAngles imuAngles;
 
    static double rpmToRadPerSecond(double RPM);
    static double radPerSecondToRPM(double radPerSecond);
 
    OmniKinematics m_OmniKinematics;
    float chassis_radius;
    void setOmniKinematics(double radius, HOLONOMIC_MODE mode = OMNI);
    // Eigen::MatrixXd wheelSpeedsToChassisSpeeds(WheelSpeeds wheelSpeeds);
 
    double FF_Ks;
 
    void setMotorPower(MotorLocation location, double power);
    void setMotorSpeedRPM(MotorLocation location, double speed);
    // void setMotorSpeedTicksPerSecond(int index, double speed);
 
    double getMotorSpeedRPM(MotorLocation location);
    int motorPIDtoPower(MotorLocation location, double speed, uint32_t dt);
 
    // ChassisKalman chassisKalman;
    double testAngle;
    int lastTimeMs;
 
    unsigned long m_lastTorqueUs = 0;
};
 
#endif // TR_EMBEDDED_CHASSIS_SUBSYSTEM_H