mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-24 03:27:45 +08:00
266 lines
10 KiB
C++
266 lines
10 KiB
C++
/**
|
|
* @file PowerController.hpp
|
|
* @version 2.0
|
|
* @note The estimated power formula: P = τΩ + k1|Ω| + k2τ^2
|
|
*/
|
|
#pragma once
|
|
// #include "AppConfig.h"
|
|
|
|
#ifndef USE_POWER_CONTROLLER
|
|
#define USE_POWER_CONTROLLER FALSE
|
|
#endif
|
|
|
|
#if USE_POWER_CONTROLLER
|
|
|
|
// If the capacitor is plugged into the circuit, make sure you enable the super cap module successfully
|
|
// Otherwise, it will cause unexpected behavior of the RLS model
|
|
#ifndef USE_SUPER_CAPACITOR
|
|
#define USE_SUPER_CAPACITOR FALSE
|
|
#endif
|
|
|
|
#ifndef USE_REFEREE_SYSTEM_COMM
|
|
#define USE_REFEREE_SYSTEM_COMM FALSE
|
|
#endif
|
|
|
|
#if !(USE_REFEREE_SYSTEM_COMM)
|
|
#error Referee System Communication is not defined!
|
|
#endif
|
|
|
|
#if USE_SUPER_CAPACITOR
|
|
#include "SuperCapaManager.hpp"
|
|
#endif
|
|
|
|
#include "AbstractFeedbackMotor.hpp"
|
|
#include "Math.hpp"
|
|
#include "PID.hpp"
|
|
#include "RLS.hpp"
|
|
#include "RefereeSystemComm.hpp"
|
|
#include "RefereeSystemManager.hpp"
|
|
#include "RefereeSystemMessage.hpp"
|
|
|
|
namespace Core
|
|
{
|
|
namespace Control
|
|
{
|
|
namespace Power
|
|
{
|
|
|
|
constexpr static float refereeFullBuffSet = 60.0f;
|
|
constexpr static float refereeBaseBuffSet = 50.0f;
|
|
constexpr static float capFullBuffSet = 230.0f;
|
|
constexpr static float capBaseBuffSet = 30.0f;
|
|
constexpr static float error_powerDistribution_set = 20.0f;
|
|
constexpr static float prop_powerDistribution_set = 15.0f;
|
|
|
|
// constexpr float MIN_MAXPOWER_CONFIGURED = 15.0f;
|
|
constexpr float MAX_CAP_POWER_OUT = 300.0f;
|
|
constexpr float CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD = 43.0f;
|
|
constexpr float CAP_OFFLINE_ENERGY_TARGET_POWER = 37.0f;
|
|
constexpr float MAX_POEWR_REFEREE_BUFF = 60.0f;
|
|
constexpr float REFEREE_GG_COE = 0.95f;
|
|
constexpr float CAP_REFEREE_BOTH_GG_COE = 0.85f;
|
|
|
|
/**
|
|
* @brief The Power Limit and max HP enumeration attributed by division, chassis type and level
|
|
* @note Copy from RM2024 Official Rule Manual
|
|
* @attention The infantry data list only suits for standard infantry, but not balanced infantry
|
|
* @attention if the pilot changes the chassis type before the game officially start, and simultaneously the referee system is disconnected before
|
|
* chassis type changed, there will be problem of distinguishing the chassis type, so we choose HP_FIRST chassis type conservatively, except for
|
|
* sentry
|
|
*/
|
|
constexpr static uint8_t maxLevel = 10U;
|
|
constexpr static uint8_t HeroChassisPowerLimit_HP_FIRST[maxLevel] = {55U, 60U, 65U, 70U, 75U, 80U, 85U, 90U, 100U, 120U}
|
|
;
|
|
constexpr static uint8_t
|
|
|
|
InfantryChassisPowerLimit_HP_FIRST[maxLevel] = {45U, 50U, 55U, 60U, 65U, 70U, 75U, 80U, 90U,
|
|
100U};
|
|
constexpr static uint8_t SentryChassisPowerLimit = 100U;
|
|
|
|
enum class Division
|
|
{
|
|
INFANTRY = 0,
|
|
HERO,
|
|
SENTRY
|
|
};
|
|
|
|
struct Manager
|
|
{
|
|
enum RLSEnabled: bool
|
|
{
|
|
Disable = 0,
|
|
Enable = 1
|
|
} rlsEnabled;
|
|
|
|
enum ErrorFlags
|
|
{
|
|
MotorDisconnect = 1U,
|
|
RefereeDisConnect = 2U,
|
|
CAPDisConnect = 4U
|
|
};
|
|
|
|
struct Motors // For overloading
|
|
{
|
|
const AbstractFeedbackMotor * motorRf;
|
|
const AbstractFeedbackMotor * motorLf;
|
|
const AbstractFeedbackMotor * motorLb;
|
|
const AbstractFeedbackMotor * motorRb;
|
|
|
|
const AbstractFeedbackMotor * &operator[](int index)
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0:
|
|
return motorRf;
|
|
case 1:
|
|
return motorLf;
|
|
case 2:
|
|
return motorLb;
|
|
case 3:
|
|
return motorRb;
|
|
default:
|
|
return motorRf;
|
|
}
|
|
}
|
|
|
|
Motors(const AbstractFeedbackMotor * motorRf_,
|
|
const AbstractFeedbackMotor * motorLf_,
|
|
const AbstractFeedbackMotor * motorLb_,
|
|
const AbstractFeedbackMotor * motorRb_)
|
|
: motorRf(motorRf_), motorLf(motorLf_), motorLb(motorLb_), motorRb(motorRb_)
|
|
{
|
|
}
|
|
};
|
|
|
|
uint8_t error;
|
|
|
|
/**
|
|
* @remark In case of initialization without explicit datas
|
|
*/
|
|
Manager() = delete;
|
|
|
|
/**
|
|
* @brief The constructor of the power manager object
|
|
* @param motors_ The motor objects
|
|
* @todo This will change to the type of "AbstractFeedbackMotor*"
|
|
* @param division_ The type of robot
|
|
* @param rlsEnabled_ Enable or disable the RLS adaptive param mode
|
|
* @param torqueConst_ The torque const (KA) of the motor, measured by (N.m / A)
|
|
* @param k1_ The frequency-dissipate params on the power estimation motor
|
|
* @param k2_ The current-dissipate's square params on the power estimation motor
|
|
* @param k3_ The constant power loss
|
|
* @param lambda_ The RLS update forgetting factor
|
|
*/
|
|
Manager(const Motors & motors_,
|
|
const Division division_,
|
|
RLSEnabled rlsEnabled_ = Enable,
|
|
const float k1_ = 0.22f,
|
|
const float k2_ = 1.2f,
|
|
const float k3_ = 2.78f,
|
|
const float lambda_ = 0.9999f);
|
|
|
|
Motors motors;
|
|
Division division;
|
|
|
|
float powerBuff;
|
|
float fullBuffSet;
|
|
float baseBuffSet;
|
|
float fullMaxPower;
|
|
float baseMaxPower;
|
|
|
|
float powerUpperLimit;
|
|
float refereeMaxPower;
|
|
|
|
float userConfiguredMaxPower;
|
|
float(*callback)(void);
|
|
|
|
float measuredPower;
|
|
float estimatedPower;
|
|
float estimatedCapEnergy;
|
|
|
|
float torqueConst;
|
|
|
|
float k1;
|
|
float k2;
|
|
float k3;
|
|
|
|
TickType_t lastUpdateTick;
|
|
Math::RLS < 2 > rls;
|
|
}
|
|
;
|
|
|
|
struct PowerObj
|
|
{
|
|
public:
|
|
float pidOutput; // torque current command, [-maxOutput, maxOutput], no unit
|
|
float curAv; // Measured angular velocity, [-maxAv, maxAv], rad/s
|
|
float setAv; // target angular velocity, [-maxAv, maxAv], rad/s
|
|
float pidMaxOutput; // pid max output
|
|
}
|
|
;
|
|
|
|
/**
|
|
* @brief Storing the power status of the chassis
|
|
*/
|
|
struct PowerStatus
|
|
{
|
|
public:
|
|
float userConfiguredMaxPower;
|
|
float maxPowerLimited;
|
|
float sumPowerCmd_before_clamp;
|
|
float effectivePower;
|
|
float powerLoss;
|
|
float efficiency;
|
|
uint8_t estimatedCapEnergy;
|
|
Manager::ErrorFlags error;
|
|
}
|
|
;
|
|
|
|
// return the latest feedback referee power limit(before referee disconnected), according to the robot level
|
|
float getLatestFeedbackJudgePowerLimit();
|
|
|
|
/**
|
|
* @brief Get the controlled output torque current based on current model
|
|
* @param objs The collections of power objects from four wheels, recording the necessary data from the PID controller
|
|
* @retval The controlled output torque current
|
|
*/
|
|
float *getControlledOutput(PowerObj * objs[4]);
|
|
|
|
/**
|
|
* @brief return the power status of the chassis
|
|
* @retval The power status object
|
|
*/
|
|
const volatile PowerStatus & getPowerStatus();
|
|
|
|
/**
|
|
* @brief The power controller module initialization function
|
|
* @param manager The manager object
|
|
* @note This function should be called before the scheduler starts
|
|
*/
|
|
void init(const Manager & manager);
|
|
|
|
/**
|
|
* @brief set the user configured max power
|
|
* @param maxPower The max power value
|
|
* @note The max power configured by this function will compete with the basic energy limitation, to ensure system does not die
|
|
*/
|
|
void setMaxPowerConfigured(float maxPower);
|
|
|
|
void setMode(uint8_t mode);
|
|
|
|
void registerPowerCallbackFunc(float(*callback)(void));
|
|
|
|
/**
|
|
* @brief Enable for disable the automatically parameters update process
|
|
* @param isUpdate disable with 0, enable with 1
|
|
* @note The system will automatically disable the update when both referee system and cap is disconnect from the power module
|
|
* @retval None
|
|
*/
|
|
void setRLSEnabled(uint8_t isUpdate);
|
|
|
|
} // namespace Power
|
|
} // namespace Control
|
|
} // namespace Core
|
|
|
|
#endif
|