mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-24 03:27:45 +08:00
videotransmiter
This commit is contained in:
TuxMonkey
416
User_Code/module/software/powercontrol/Martix.hpp
Normal file
416
User_Code/module/software/powercontrol/Martix.hpp
Normal file
@@ -0,0 +1,416 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file matrix.cpp/h
|
||||
* @brief Matrix/vector calculation. 矩阵/向量运算
|
||||
* @author Spoon Guan
|
||||
******************************************************************************
|
||||
* Copyright (c) 2023 Team JiaoLong-SJTU
|
||||
* All rights reserved.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#include "AppConfig.h"
|
||||
#include "EnableIf.hpp"
|
||||
#include "arm_math.h"
|
||||
#pragma once
|
||||
|
||||
template<int _rows, int _cols>
|
||||
|
||||
class Matrixf
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor without input data
|
||||
* @param
|
||||
*/
|
||||
constexpr Matrixf (
|
||||
void
|
||||
)
|
||||
:
|
||||
rows_(_rows), cols_(_cols) {
|
||||
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor with input data
|
||||
* @param data A 2-D array buffer that stores the data
|
||||
*/
|
||||
constexpr Matrixf (
|
||||
float data[_rows * _cols]
|
||||
)
|
||||
:
|
||||
Matrixf() {
|
||||
memcpy(this->data_, data, _rows * _cols * sizeof(float));
|
||||
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Copy Constructor
|
||||
* @param mat The copied matrix
|
||||
*/
|
||||
constexpr Matrixf (
|
||||
const Matrixf<_rows, _cols>
|
||||
&mat
|
||||
)
|
||||
:
|
||||
Matrixf() {
|
||||
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
|
||||
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
~Matrixf(void) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief returns the row size of the matrix
|
||||
* @return _rows The row size of the matrix
|
||||
*/
|
||||
uint32_t rows(void) const {
|
||||
return _rows;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief return the column size of the matrix
|
||||
* @return _cols The column size of the matrix
|
||||
*/
|
||||
uint32_t cols(void) const {
|
||||
return _cols;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return the element of the matrix
|
||||
* @param row The row
|
||||
*/
|
||||
float *operator[](const int&row) {
|
||||
return &this->data_[row * _cols];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Copy assignment of the matrix(row * size) instance
|
||||
* @param mat The copied prototype
|
||||
* @return *this matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > &operator = (const Matrixf<_rows, _cols>
|
||||
mat
|
||||
) {
|
||||
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Additional operator of two matrices(row * size)
|
||||
* @param mat The matrix on the right hand side
|
||||
* @note This function returns itself as the result
|
||||
* @return The sum of two matrices
|
||||
*/
|
||||
Matrixf < _rows, _cols > &operator += (const Matrixf<_rows, _cols>
|
||||
mat
|
||||
) {
|
||||
arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Substraction operator of two matrices(row * size)
|
||||
* @param mat The matrix on the left hand side
|
||||
* @note This function returns itself as the result
|
||||
* @return The difference of two matrices
|
||||
*/
|
||||
Matrixf < _rows, _cols > &operator -= (const Matrixf<_rows, _cols>
|
||||
mat
|
||||
) {
|
||||
arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scalar operator of the matrix and a scaling factor
|
||||
* @param val The scaling factor
|
||||
* @note This function returns itself as the result
|
||||
* @return THe scaled matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > &operator *= (const float & val
|
||||
) {
|
||||
arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scalar operator of the matrix and a division factor
|
||||
* @param val The division factor
|
||||
* @note This function returns itself as the result
|
||||
* @retval matrix / val
|
||||
* @return The scaled matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > &operator /= (const float & val
|
||||
) {
|
||||
arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Additonal operator
|
||||
* @note This function doesn't return itself but instead a new matrix instance
|
||||
* @param mat The matrix on the right hand side
|
||||
* @return The sum of the additional matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > operator + (const Matrixf<_rows, _cols> & mat
|
||||
)
|
||||
const {
|
||||
Matrixf < _rows, _cols > res;
|
||||
arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Substraction matrix
|
||||
* @note This function does not return itself but instead a new matrix instance
|
||||
* @param mat matrix on the right hand side
|
||||
* @return The sum of the substracted matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > operator - (const Matrixf<_rows, _cols> & mat
|
||||
)
|
||||
const {
|
||||
Matrixf < _rows, _cols > res;
|
||||
arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scalar operator of the matrix and a scaling factor
|
||||
* @param val The scaling factor
|
||||
* @note This function does not return itself
|
||||
* @return THe scaled matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > operator * (const float & val
|
||||
)
|
||||
const {
|
||||
Matrixf < _rows, _cols > res;
|
||||
arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scalar operator of the matrix and a scaling factor
|
||||
* @param val The scaling factor on the left hand side
|
||||
* @note This function does not return itself
|
||||
* @note This time the scaling factor is on the left hand side
|
||||
* @return THe scaled matrix
|
||||
*/
|
||||
friend Matrixf<_rows, _cols> operator*(const float&val, const Matrixf<_rows, _cols>&mat) {
|
||||
arm_status s;
|
||||
Matrixf < _rows, _cols > res;
|
||||
s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scalar operator of the matrix and a division factor
|
||||
* @param val The division factor
|
||||
* @note This function returns itself as the result
|
||||
* @retval matrix / val
|
||||
* @return The scaled matrix
|
||||
*/
|
||||
Matrixf < _rows, _cols > operator / (const float & val
|
||||
)
|
||||
const {
|
||||
Matrixf < _rows, _cols > res;
|
||||
arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The matrix multiplication
|
||||
* @param mat1 the matrix on the LHS
|
||||
* @param mat2 the matrix on the RHS
|
||||
* @return The multiplication result
|
||||
*/
|
||||
template < int
|
||||
cols2 >
|
||||
friend Matrixf<_rows, cols2> operator*(const Matrixf<_rows, _cols>&mat1, const Matrixf<_cols, cols2>&mat2) {
|
||||
Matrixf < _rows, cols2 > res;
|
||||
arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compare whether two matrices are identical
|
||||
*
|
||||
*/
|
||||
bool operator
|
||||
==
|
||||
(const Matrixf<_rows, _cols>
|
||||
&mat
|
||||
)
|
||||
const {
|
||||
for (int i = 0; i < _rows * _cols; i++)
|
||||
{
|
||||
if (this->data_[i] != mat.data_[i])
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Submatrix
|
||||
template < int
|
||||
rows, int
|
||||
cols >
|
||||
Matrixf < rows, cols > block(const int &start_row, const int &start_col)
|
||||
const {
|
||||
Matrixf < rows, cols > res;
|
||||
for (int row = start_row; row < start_row + rows; row++)
|
||||
{
|
||||
memcpy((float *) res[0] + (row - start_row) * cols, (float *) this->data_ + row * _cols + start_col,
|
||||
cols * sizeof(float));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return the specific row of the matrix
|
||||
* @param row The row index
|
||||
* @retval The row vector presented in the matrix from
|
||||
*/
|
||||
Matrixf < 1, _cols > row(const int &row)
|
||||
const {
|
||||
return block < 1, _cols > (row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return the specific row of the matrix
|
||||
* @param col The column index
|
||||
* @retval The column vector presented in the matrix from
|
||||
*/
|
||||
Matrixf < _rows, 1 > col(const int &col)
|
||||
const {
|
||||
return block<_rows, 1>(0, col);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the transpose of the matrix
|
||||
* @param
|
||||
* @retval the transposed matrix
|
||||
*/
|
||||
Matrixf < _cols, _rows > trans(void)
|
||||
const {
|
||||
Matrixf < _cols, _rows > res;
|
||||
arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
// Trace
|
||||
|
||||
/**
|
||||
* @brief Get the trace of the matrix
|
||||
* @param
|
||||
* @retval The trace of the matrix
|
||||
*/
|
||||
float trace(void) const {
|
||||
float res = 0;
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++)
|
||||
{
|
||||
res += (*this)[i][i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the norm of the matrix
|
||||
* @param
|
||||
* @retval The norm of the matrix
|
||||
*/
|
||||
float norm(void) const {
|
||||
return sqrtf((this->trans() * *this)[0][0]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the inverse of the matrix
|
||||
* @param
|
||||
* @retval The inverse of the matrix
|
||||
*/
|
||||
Matrixf < _cols, _rows > inv(void)
|
||||
const {
|
||||
if (_cols != _rows)
|
||||
return Matrixf < _cols, _rows > ::zeros();
|
||||
|
||||
Matrixf < _cols, _rows > res;
|
||||
arm_status status = arm_mat_inverse_f32(&this->arm_mat_, &res);
|
||||
|
||||
if (status == ARM_MATH_SINGULAR)
|
||||
return Matrixf < _cols, _rows > ::zeros();
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/*==============================================================*/
|
||||
// Static function
|
||||
/**
|
||||
* @brief Returns a _rows x _cols zero matrix
|
||||
* @tparam _rows The row size
|
||||
* @tparam _cols The column size
|
||||
* @retval The zero matrix
|
||||
*/
|
||||
static Matrixf<_rows, _cols> zeros(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
return Matrixf < _rows, _cols > (data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a _rows x _cols one matrix
|
||||
* @tparam _rows The row size
|
||||
* @tparam _cols The column size
|
||||
* @retval The one matrix
|
||||
*/
|
||||
static Matrixf<_rows, _cols> ones(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
for (int i = 0; i < _rows * _cols; i++)
|
||||
{
|
||||
data[i] = 1;
|
||||
}
|
||||
return Matrixf < _rows, _cols > (data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a _rows * columns matrix
|
||||
* @tparam _rows The row size
|
||||
* @tparam _cols The column size
|
||||
* @retval The identity matrix
|
||||
*/
|
||||
static Matrixf<_rows, _cols> eye(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++)
|
||||
{
|
||||
data[i * _cols + i] = 1;
|
||||
}
|
||||
return Matrixf < _rows, _cols > (data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a _rows x _cols diagonal matrix
|
||||
* @tparam _rows The row size
|
||||
* @tparam _cols The column size
|
||||
* @param vec The diagnoal entries
|
||||
* @retval The diagnoanl matrix
|
||||
*/
|
||||
static Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
|
||||
Matrixf < _rows, _cols > res = Matrixf < _rows, _cols > ::zeros();
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++)
|
||||
{
|
||||
res[i][i] = vec[i][0];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
public:
|
||||
arm_matrix_instance_f32 arm_mat_; // The arm math instance
|
||||
|
||||
protected:
|
||||
// The size
|
||||
int rows_, cols_;
|
||||
// Data buffer
|
||||
float data_[_rows * _cols];
|
||||
};
|
||||
|
||||
// namespace matrixf
|
||||
128
User_Code/module/software/powercontrol/RLS.hpp
Normal file
128
User_Code/module/software/powercontrol/RLS.hpp
Normal file
@@ -0,0 +1,128 @@
|
||||
#include "FreeRTOS.h"
|
||||
#include "Matrix.hpp"
|
||||
#include "task.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace Core
|
||||
{
|
||||
namespace Control
|
||||
{
|
||||
namespace Math
|
||||
{
|
||||
|
||||
template < uint32_t dim >
|
||||
class RLS
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Delete the default constructor
|
||||
*/
|
||||
RLS() = delete;
|
||||
|
||||
/**
|
||||
* @brief The constructor
|
||||
* @param delta_ The intialized non-singular value of the transfer matrix
|
||||
* @param lambda_ The forgotten index
|
||||
*/
|
||||
constexpr RLS(float delta_, float lambda_)
|
||||
: dimension(dim), lambda(lambda_), delta(delta_), lastUpdate(0), updateCnt(0),
|
||||
defaultParamsVector(Matrixf<dim, 1>::zeros())
|
||||
{
|
||||
this->reset();
|
||||
this->validate();
|
||||
}
|
||||
|
||||
constexpr RLS(float delta_, float lambda_, Matrixf<dim, 1> initParam): RLS(delta_, lambda_)
|
||||
{defaultParamsVector = initParam; }
|
||||
|
||||
/**
|
||||
* @brief Reset the RLS module
|
||||
* @retval None
|
||||
*/
|
||||
void reset()
|
||||
{
|
||||
transMatrix = Matrixf<dim, dim>::eye() * delta;
|
||||
gainVector = Matrixf<dim, 1>::zeros();
|
||||
paramsVector = Matrixf<dim, 1>::zeros();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess a cycle of RLS update
|
||||
* @param sampleVector The new samples input expressed in n x 1 dimensionasl vector form
|
||||
* @param actualOutput The actual feedback real output
|
||||
* @retval paramsVector
|
||||
*/
|
||||
const Matrixf<dim, 1> & update(Matrixf<dim, 1> & sampleVector, float actualOutput)
|
||||
{
|
||||
gainVector =
|
||||
(transMatrix * sampleVector) / (
|
||||
1.0f + (sampleVector.trans() * transMatrix * sampleVector)[0][0] / lambda) /
|
||||
lambda; // Get gain vector
|
||||
paramsVector += gainVector * (
|
||||
actualOutput - (sampleVector.trans() * paramsVector)[0][0])
|
||||
; // Get params vector
|
||||
transMatrix =
|
||||
(transMatrix - gainVector * sampleVector.trans() * transMatrix) / lambda
|
||||
; // Get transferred matrix
|
||||
|
||||
updateCnt++;
|
||||
lastUpdate = xTaskGetTickCount();
|
||||
return paramsVector;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the default regression parameters
|
||||
* @param updatedParams
|
||||
* @retval None
|
||||
*/
|
||||
void setParamVector(const Matrixf<dim, 1> & updatedParams)
|
||||
{
|
||||
paramsVector = updatedParams;
|
||||
defaultParamsVector = updatedParams;
|
||||
}
|
||||
/**
|
||||
* @brief The getter function of the params vector
|
||||
* @param None
|
||||
* @retval paramsVector
|
||||
*/
|
||||
constexpr Matrixf<dim, 1> & getParamsVector() const{return paramsVector; }
|
||||
|
||||
/**
|
||||
* @brief The getter function of the output vector
|
||||
* @param None
|
||||
* @retval The estimated / filterd output of the RLS module
|
||||
*/
|
||||
const float &getOutput() const{return output; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Lambda and delta validate check
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void validate() const
|
||||
{
|
||||
configASSERT(lambda >= 0.0f || lambda <= 1.0f);
|
||||
configASSERT(delta > 0);
|
||||
}
|
||||
|
||||
uint32_t dimension; // Dimension of the RLS space
|
||||
float lambda; // The forget index
|
||||
float delta; // Intialized value of the transferred matrix
|
||||
|
||||
TickType_t lastUpdate; // Last update tick
|
||||
uint32_t updateCnt; // Total update Count
|
||||
|
||||
/*RLS relvant matrix*/
|
||||
Matrixf<dim, dim > transMatrix; // Transfer matrix instance
|
||||
Matrixf<dim, 1 > gainVector; // Gain vector for params update
|
||||
Matrixf<dim, 1 > paramsVector; // Params vector
|
||||
Matrixf<dim, 1 > defaultParamsVector;
|
||||
float output; // Estimated / filtered output
|
||||
}
|
||||
;
|
||||
|
||||
} // namespace Math
|
||||
} // namespace Control
|
||||
} // namespace Core
|
||||
@@ -2,4 +2,596 @@
|
||||
// Created by tux on 2025/11/5.
|
||||
//
|
||||
|
||||
#include "powercontrol.h"
|
||||
#include "powercontrol.h"
|
||||
|
||||
// #define USE_POWER_CONTROLLER
|
||||
|
||||
#if USE_POWER_CONTROLLER
|
||||
|
||||
namespace Core
|
||||
{
|
||||
namespace Control
|
||||
{
|
||||
namespace Power
|
||||
{
|
||||
|
||||
#define POWER_PD_KP 50.0f
|
||||
|
||||
Manager manager(Manager::Motors(nullptr, nullptr, nullptr, nullptr), Division::HERO);
|
||||
PowerStatus powerStatus;
|
||||
static uint8_t LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL;
|
||||
static bool isCapEnergyOut = false;
|
||||
static uint16_t motorDisconnectCounter[4] = {0U, 0U, 0U, 0U};
|
||||
static float MIN_MAXPOWER_CONFIGURED = 30.0f;
|
||||
|
||||
static inline bool floatEqual(float a, float b) {return fabs(a - b) < 1e-5f; }
|
||||
|
||||
static inline float rpm2av(float rpm) {return rpm * (float) M_PI / 30.0f; }
|
||||
|
||||
static inline float av2rpm(float av) {return av * 30.0f / (float) M_PI; }
|
||||
|
||||
static inline void setErrorFlag(uint8_t & curFlag, Manager::ErrorFlags setFlag)
|
||||
{curFlag |= static_cast<uint8_t>(setFlag); }
|
||||
|
||||
static inline void clearErrorFlag(uint8_t & curFlag, Manager::ErrorFlags clearFlag)
|
||||
{curFlag &= (~static_cast<uint8_t>(clearFlag)); }
|
||||
|
||||
static inline bool isFlagged(uint8_t & curFlag, Manager::ErrorFlags flag)
|
||||
{return (curFlag & static_cast<uint8_t>(flag)) != 0; }
|
||||
|
||||
static inline bool isMotorConnected(const AbstractFeedbackMotor * motor)
|
||||
{return motor != nullptr && motor->getDisconnectCounter() < 10; }
|
||||
|
||||
static inline bool isAllMotorConnected()
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
if (not isMotorConnected(manager.motors[i]))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
Manager::Manager(
|
||||
const Motors & motors_, const Division division_, RLSEnabled rlsEnabled_, const float k1_, const float k2_,
|
||||
const float k3_, const float lambda_)
|
||||
|
||||
: rlsEnabled(rlsEnabled_),
|
||||
error(0UL),
|
||||
motors(motors_),
|
||||
division(division_),
|
||||
powerBuff(0.0f),
|
||||
fullBuffSet(0.0f),
|
||||
baseBuffSet(0.0f),
|
||||
fullMaxPower(0.0f),
|
||||
baseMaxPower(0.0f),
|
||||
powerUpperLimit(0.0f),
|
||||
refereeMaxPower(0.0f),
|
||||
userConfiguredMaxPower(0.0f),
|
||||
callback(nullptr),
|
||||
torqueConst(0.0f),
|
||||
k1(k1_),
|
||||
k2(k2_),
|
||||
k3(k3_),
|
||||
lastUpdateTick(0),
|
||||
rls(1e-5f, 0.99999f)
|
||||
{
|
||||
configASSERT(k1_ >= 0);
|
||||
configASSERT(k2_ >= 0);
|
||||
configASSERT(k3_ >= 0);
|
||||
|
||||
float initParams[2] = {k1_, k2_};
|
||||
rls.setParamVector(Matrixf < 2, 1 > (initParams));
|
||||
}
|
||||
|
||||
static bool isInitialized;
|
||||
StackType_t xPowerTaskStack[1024];
|
||||
StaticTask_t uxPowerTaskTCB;
|
||||
|
||||
using namespace Core::Communication;
|
||||
RefereeSystem::RefereeSystemMessageReceive<RefereeSystem::RefereePowerHeatMessageData> powerMessage;
|
||||
RefereeSystem::RefereeSystemMessageReceive<RefereeSystem::RefereeRobotStatusMessageData> robotMessage;
|
||||
|
||||
#if USE_SUPER_CAPACITOR
|
||||
const Core::Control::SuperCapacitor::CapacitorStatus&capStatus = Core::Control::SuperCapacitor::getStatus();
|
||||
#endif
|
||||
|
||||
Core::Control::PID::Param powerPDParam(POWER_PD_KP, 0.0f, 0.2f, 0.0f, MAX_CAP_POWER_OUT, 0.0f, 0.0f, 0.2f, 100UL);
|
||||
Core::Control::PID powerPD_base(powerPDParam); // ensure the system does not die
|
||||
Core::Control::PID powerPD_full(powerPDParam); // ensure the capacitor's energy keeps under 90%
|
||||
|
||||
/**
|
||||
* @implements
|
||||
*/
|
||||
void setMaxPowerConfigured(float maxPower)
|
||||
{
|
||||
manager.userConfiguredMaxPower = Utils::Math::clamp(maxPower, MIN_MAXPOWER_CONFIGURED, manager.powerUpperLimit);
|
||||
}
|
||||
|
||||
void setMode(uint8_t mode)
|
||||
{
|
||||
setMaxPowerConfigured(mode == 1 ? manager.powerUpperLimit : manager.refereeMaxPower);
|
||||
}
|
||||
|
||||
void registerPowerCallbackFunc(float (*callback)(void))
|
||||
{
|
||||
manager.callback = callback;
|
||||
}
|
||||
|
||||
/**
|
||||
* @implements
|
||||
*/
|
||||
void setRLSEnabled(uint8_t enable)
|
||||
{
|
||||
manager.rlsEnabled = static_cast<Manager::RLSEnabled>(enable);
|
||||
}
|
||||
|
||||
const volatile PowerStatus&getPowerStatus()
|
||||
{
|
||||
return powerStatus;
|
||||
}
|
||||
|
||||
float getLatestFeedbackJudgePowerLimit()
|
||||
{
|
||||
return manager.refereeMaxPower;
|
||||
}
|
||||
|
||||
/**
|
||||
* @implements
|
||||
*/
|
||||
|
||||
float *getControlledOutput(PowerObj *objs[4])
|
||||
{
|
||||
const float k0 = manager.torqueConst * manager.motors[0]->getCurrentLimit() /
|
||||
manager.motors[0]->getOutputLimit(); // torque current rate of the motor, defined as Nm/Output
|
||||
|
||||
static float newTorqueCurrent[4];
|
||||
|
||||
float sumCmdPower = 0.0f;
|
||||
float cmdPower[4];
|
||||
|
||||
float sumError = 0.0f;
|
||||
float error[4];
|
||||
|
||||
float maxPower = Utils::Math::clamp(manager.userConfiguredMaxPower, manager.fullMaxPower, manager.baseMaxPower);
|
||||
|
||||
float allocatablePower = maxPower;
|
||||
float sumPowerRequired = 0.0f;
|
||||
#if USE_DEBUG
|
||||
static float newCmdPower;
|
||||
#endif
|
||||
|
||||
for (int i = 0; i<4; i++)
|
||||
{
|
||||
if (isMotorConnected(manager.motors[i]))
|
||||
{
|
||||
PowerObj *p = objs[i];
|
||||
cmdPower[i] = p->pidOutput * k0 * p->curAv + fabs(p->curAv) * manager.k1 + p->pidOutput * k0 * p->pidOutput * k0 * manager.k2 +
|
||||
manager.k3 / static_cast<float>(4);
|
||||
sumCmdPower += cmdPower[i];
|
||||
error[i] = fabs(p->setAv - p->curAv);
|
||||
if (floatEqual(cmdPower[i], 0.0f) || cmdPower[i] < 0.0f)
|
||||
{
|
||||
allocatablePower += -cmdPower[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
sumError += error[i];
|
||||
sumPowerRequired += cmdPower[i];
|
||||
}
|
||||
}
|
||||
else if (motorDisconnectCounter[i] < 1000U)
|
||||
{
|
||||
cmdPower[i] = manager.motors[i]->getTorqueFeedback() * rpm2av(manager.motors[i]->getRPMFeedback()) +
|
||||
fabs(rpm2av(manager.motors[i]->getRPMFeedback())) * manager.k1 +
|
||||
manager.motors[i]->getTorqueFeedback() * manager.motors[i]->getTorqueFeedback() * manager.k2 + manager.k3 / 4.0f;
|
||||
error[i] = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmdPower[i] = 0.0f;
|
||||
error[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// update power status
|
||||
powerStatus.maxPowerLimited = maxPower;
|
||||
powerStatus.sumPowerCmd_before_clamp = sumCmdPower;
|
||||
|
||||
if (sumCmdPower> maxPower)
|
||||
{
|
||||
float errorConfidence;
|
||||
if (sumError > error_powerDistribution_set)
|
||||
{
|
||||
errorConfidence = 1.0f;
|
||||
}
|
||||
else if (sumError > prop_powerDistribution_set)
|
||||
{
|
||||
errorConfidence =
|
||||
Utils::Math::clamp((sumError - prop_powerDistribution_set) / (error_powerDistribution_set - prop_powerDistribution_set), 0.0f, 1.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
errorConfidence = 0.0f;
|
||||
}
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
PowerObj *p = objs[i];
|
||||
if (isMotorConnected(manager.motors[i]))
|
||||
{
|
||||
if (floatEqual(cmdPower[i], 0.0f) || cmdPower[i] < 0.0f)
|
||||
{
|
||||
newTorqueCurrent[i] = p->pidOutput;
|
||||
continue;
|
||||
}
|
||||
float powerWeight_Error = fabs(p->setAv - p->curAv) / sumError;
|
||||
float powerWeight_Prop = cmdPower[i] / sumPowerRequired;
|
||||
float powerWeight = errorConfidence * powerWeight_Error + (1.0f - errorConfidence) * powerWeight_Prop;
|
||||
float delta = p->curAv * p->curAv -
|
||||
4.0f * manager.k2 * (manager.k1 * fabs(p->curAv) + manager.k3 / static_cast<float>(4) - powerWeight * allocatablePower);
|
||||
if (floatEqual(delta, 0.0f)) // repeat roots
|
||||
{
|
||||
newTorqueCurrent[i] = -p->curAv / (2.0f * manager.k2) / k0;
|
||||
}
|
||||
else if (delta > 0.0f) // distinct roots
|
||||
{
|
||||
newTorqueCurrent[i] = p->pidOutput > 0.0f ? (-p->curAv + sqrtf(delta)) / (2.0f * manager.k2) / k0
|
||||
: (-p->curAv - sqrtf(delta)) / (2.0f * manager.k2) / k0;
|
||||
}
|
||||
else // imaginary roots
|
||||
{
|
||||
newTorqueCurrent[i] = -p->curAv / (2.0f * manager.k2) / k0;
|
||||
}
|
||||
newTorqueCurrent[i] = Utils::Math::clamp(newTorqueCurrent[i], p->pidMaxOutput);
|
||||
}
|
||||
else
|
||||
{
|
||||
newTorqueCurrent[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
if (isMotorConnected(manager.motors[i]))
|
||||
{
|
||||
newTorqueCurrent[i] = objs[i]->pidOutput;
|
||||
}
|
||||
else
|
||||
{
|
||||
newTorqueCurrent[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if USE_DEBUG
|
||||
newCmdPower = 0.0f;
|
||||
for (int i = 0; i<4; i++)
|
||||
{
|
||||
PowerObj *p = objs[i];
|
||||
newCmdPower += newTorqueCurrent[i] * k0 * p->curAv + fabs(p->curAv) * manager.k1 +
|
||||
newTorqueCurrent[i] * k0 * newTorqueCurrent[i] * k0 * manager.k2 + manager.k3 / 4.0f;
|
||||
}
|
||||
#endif
|
||||
|
||||
return newTorqueCurrent;
|
||||
}
|
||||
|
||||
static inline void setErrorFlag()
|
||||
{
|
||||
|
||||
|
||||
|
||||
/*Judge the error status*/
|
||||
#if USE_SUPER_CAPACITOR
|
||||
if (not capStatus.isConnected|| not capStatus.capacitorTx.enableDCDC|| not capStatus.capacitorRx.errorCode== 0)
|
||||
setErrorFlag(manager.error, Manager::CAPDisConnect);
|
||||
else
|
||||
clearErrorFlag(manager.error, Manager::CAPDisConnect);
|
||||
#else
|
||||
setErrorFlag(manager.error, Manager::CAPDisConnect);
|
||||
#endif
|
||||
|
||||
if (not RefereeSystem::isConnected())
|
||||
setErrorFlag(manager.error, Manager::RefereeDisConnect);
|
||||
else
|
||||
clearErrorFlag(manager.error, Manager::RefereeDisConnect);
|
||||
|
||||
if (not isAllMotorConnected())
|
||||
setErrorFlag(manager.error, Manager::MotorDisconnect);
|
||||
else
|
||||
clearErrorFlag(manager.error, Manager::MotorDisconnect);
|
||||
}
|
||||
|
||||
void powerDaemon [[noreturn]](void *pvParam)
|
||||
{
|
||||
static Matrixf<2, 1> samples;
|
||||
static Matrixf<2, 1> params;
|
||||
static float effectivePower = 0;
|
||||
|
||||
manager.torqueConst = manager.motors[0]->getKA() * manager.motors[0]->getReductionRatio();
|
||||
isInitialized = true;
|
||||
|
||||
vTaskDelay(1000);
|
||||
|
||||
manager.lastUpdateTick = xTaskGetTickCount();
|
||||
|
||||
while (true)
|
||||
{
|
||||
setErrorFlag();
|
||||
TickType_t now = xTaskGetTickCount();
|
||||
|
||||
// update rls state and check whether cap energy is out even when cap disconnect to utilize credible data from referee system for the rls
|
||||
// model
|
||||
// estimate the cap energy if cap disconnect
|
||||
// estimated cap energy = cap energy feedback when cap is connected
|
||||
#if USE_SUPER_CAPACITOR
|
||||
// If super capacitor is disconnected from the circuit, disable the rls update
|
||||
if (isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
{
|
||||
// Judge whether the cap energy is used-up
|
||||
if (not isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
{
|
||||
if (powerMessage.getData().bufferEnergy < MAX_POEWR_REFEREE_BUFF &&
|
||||
powerMessage.getData().chassisPower > CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD)
|
||||
{
|
||||
isCapEnergyOut = true;
|
||||
manager.estimatedCapEnergy = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
isCapEnergyOut = false;
|
||||
manager.rlsEnabled = Manager::Disable;
|
||||
if (powerMessage.getData().chassisPower < MIN_MAXPOWER_CONFIGURED && powerMessage.getData().bufferEnergy ==
|
||||
60U)
|
||||
{
|
||||
manager.estimatedCapEnergy = 2100.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
manager.estimatedCapEnergy += (powerMessage.getData().chassisPower - manager.estimatedPower) *
|
||||
static_cast<float>((now - manager.lastUpdateTick) / configTICK_RATE_HZ);
|
||||
manager.estimatedCapEnergy = Utils::Math::clamp(manager.estimatedCapEnergy, 0.0f, 2100.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
isCapEnergyOut = false;
|
||||
manager.rlsEnabled = Manager::Disable;
|
||||
manager.estimatedCapEnergy += (CAP_OFFLINE_ENERGY_TARGET_POWER - manager.estimatedPower) *
|
||||
static_cast<float>((now - manager.lastUpdateTick) / configTICK_RATE_HZ);
|
||||
manager.estimatedCapEnergy = Utils::Math::clamp(manager.estimatedCapEnergy, 0.0f, 2100.0f);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
isCapEnergyOut = false;
|
||||
manager.estimatedCapEnergy = capStatus.capacitorRx.capEnergy / 255.0f * 2100.0f;
|
||||
}
|
||||
#else // Only Use Referee System, disable the rls update if referee data is invalid
|
||||
if (isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
{
|
||||
manager.rlsEnabled = Manager::Disable;
|
||||
}
|
||||
isCapEnergyOut = false;
|
||||
manager.estimatedCapEnergy = 0.0f
|
||||
#endif
|
||||
|
||||
// Set the power buff and buff set based on the current state
|
||||
// Take cap message as priority
|
||||
// If disconnect from cap or disable the cap, then take the referee system's power buffer as feedback
|
||||
// If referee system is disconnected, then we need to disable the energy loop and treat power loop conservatively
|
||||
// When both cap and referee are disconnected, we disable the energy loop and therefore no need to update the powerBuff and buffSet
|
||||
//
|
||||
// Set the energy feedback based on the current error status
|
||||
#if USE_SUPER_CAPACITOR
|
||||
if (not isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
manager.powerBuff = capStatus.capacitorRx.capEnergy;
|
||||
else if (not isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
manager.powerBuff = powerMessage.getData().bufferEnergy;
|
||||
#else
|
||||
if (not isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
manager.powerBuff = powerMessage.getData().bufferEnergy;
|
||||
#endif
|
||||
// Set the energy target based on the current error status
|
||||
#if USE_SUPER_CAPACITOR
|
||||
// If the Super Capacitor is in the circuit
|
||||
if (not isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
{
|
||||
manager.fullBuffSet = capFullBuffSet;
|
||||
manager.baseBuffSet = capBaseBuffSet;
|
||||
}
|
||||
else
|
||||
{
|
||||
// if referee data is not valid, we do not enable the energy loop, so that we do not have to update fullbuffset and basebuffset
|
||||
manager.fullBuffSet = refereeFullBuffSet;
|
||||
manager.baseBuffSet = refereeBaseBuffSet;
|
||||
}
|
||||
#else
|
||||
// Only Use Referee System
|
||||
// if referee data is not valid, we do not enable the energy loop, so that we do not have to updating fullbuffset and basebuffset
|
||||
manager.fullBuffSet = refereeFullBuffSet;
|
||||
manager.baseBuffSet = refereeBaseBuffSet;
|
||||
#endif
|
||||
|
||||
// Update the referee maximum power limit and user configured power limit
|
||||
// If disconnected, then restore the last robot level and find corresponding chassis power limit
|
||||
if (not isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
{
|
||||
manager.refereeMaxPower = fmax(robotMessage.getData().chassis_power_limit,
|
||||
CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD);
|
||||
if (robotMessage.getData().robot_level > 10U)
|
||||
LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = 1U;
|
||||
else
|
||||
LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = fmax(1U, robotMessage.getData().robot_level);
|
||||
#if USE_SUPER_CAPACITOR
|
||||
if (isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
manager.powerUpperLimit = manager.refereeMaxPower + POWER_PD_KP * (
|
||||
sqrtf(refereeFullBuffSet) - sqrtf(refereeBaseBuffSet));
|
||||
else
|
||||
manager.powerUpperLimit = manager.refereeMaxPower + MAX_CAP_POWER_OUT;
|
||||
#else
|
||||
manager.powerUpperLimit = manager.refereeMaxPower;
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (manager.division)
|
||||
{
|
||||
case Division::HERO:
|
||||
manager.refereeMaxPower = HeroChassisPowerLimit_HP_FIRST[LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL - 1U];
|
||||
break;
|
||||
case Division::INFANTRY:
|
||||
manager.refereeMaxPower = InfantryChassisPowerLimit_HP_FIRST[LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL - 1U];
|
||||
break;
|
||||
case Division::SENTRY:
|
||||
manager.refereeMaxPower = SentryChassisPowerLimit;
|
||||
break;
|
||||
default:
|
||||
configASSERT(0) break;
|
||||
}
|
||||
// Since we have less available feedback, we constrain the power conservatively
|
||||
#if USE_SUPER_CAPACITOR
|
||||
if (isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
{
|
||||
manager.powerUpperLimit = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE;
|
||||
}
|
||||
else
|
||||
{
|
||||
manager.powerUpperLimit = manager.refereeMaxPower + MAX_CAP_POWER_OUT;
|
||||
}
|
||||
#else
|
||||
manager.powerUpperLimit = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE;
|
||||
#endif
|
||||
}
|
||||
|
||||
MIN_MAXPOWER_CONFIGURED = manager.refereeMaxPower * 0.8f;
|
||||
|
||||
// energy loop
|
||||
// if cap and referee both gg, set the max power to latest power limit * 0.85 and disable energy loop
|
||||
// if referee gg, set the max power to latest power limit * 0.95, enable energy loop when cap energy out
|
||||
if (isFlagged(manager.error, Manager::CAPDisConnect) && isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
{
|
||||
manager.baseMaxPower = manager.fullMaxPower = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE;
|
||||
powerPD_base.reset();
|
||||
powerPD_full.reset();
|
||||
}
|
||||
else
|
||||
{
|
||||
manager.baseMaxPower =
|
||||
fmax(manager.refereeMaxPower - powerPD_base(sqrtf(manager.baseBuffSet), sqrtf(manager.powerBuff)), MIN_MAXPOWER_CONFIGURED);
|
||||
manager.fullMaxPower =
|
||||
fmax(manager.refereeMaxPower - powerPD_full(sqrtf(manager.fullBuffSet), sqrtf(manager.powerBuff)), MIN_MAXPOWER_CONFIGURED);
|
||||
}
|
||||
|
||||
// if user has self defined power curve, use it
|
||||
if (manager.callback!= nullptr)
|
||||
setMaxPowerConfigured(manager.callback());
|
||||
|
||||
// Estimate the power based on the current model
|
||||
effectivePower = 0;
|
||||
samples[0][0] = 0;
|
||||
samples[1][0] = 0;
|
||||
for (int i = 0; i<4; i++)
|
||||
{
|
||||
if (isMotorConnected(manager.motors[i]))
|
||||
{
|
||||
motorDisconnectCounter[i] = 0U;
|
||||
}
|
||||
else
|
||||
{
|
||||
motorDisconnectCounter[i]++;
|
||||
}
|
||||
if (motorDisconnectCounter[i] < 1000U) // We consider motor that is just disconnected as still using power, by assuming the motor keep
|
||||
// latest output and rpm by 1 second, otherwise it is not safe if we only use energy loop
|
||||
{
|
||||
effectivePower += manager.motors[i]->getTorqueFeedback() * rpm2av(manager.motors[i]->getRPMFeedback());
|
||||
samples[0][0] += fabsf(rpm2av(manager.motors[i]->getRPMFeedback()));
|
||||
samples[1][0] += manager.motors[i]->getTorqueFeedback() * manager.motors[i]->getTorqueFeedback();
|
||||
}
|
||||
else
|
||||
{
|
||||
motorDisconnectCounter[i] = 1000U;
|
||||
}
|
||||
}
|
||||
manager.estimatedPower = manager.k1 * samples[0][0] + manager.k2 * samples[1][0] + effectivePower + manager.k3;
|
||||
|
||||
// Get the measured power from cap
|
||||
// If cap is disconnected, get measured power from referee feedback if cap energy is out
|
||||
// Otherwise, set it to estimated power
|
||||
#if USE_SUPER_CAPACITOR
|
||||
if (not isFlagged(manager.error, Manager::CAPDisConnect))
|
||||
{
|
||||
manager.measuredPower = capStatus.capacitorRx.chassisPower;
|
||||
}
|
||||
else if (not isFlagged(manager.error, Manager::RefereeDisConnect) && isCapEnergyOut)
|
||||
{
|
||||
// If the capacitor energy is used up, we could trust the data from the referee system
|
||||
manager.measuredPower = powerMessage.getData().chassisPower;
|
||||
}
|
||||
else
|
||||
{
|
||||
manager.measuredPower = manager.estimatedPower;
|
||||
}
|
||||
#else
|
||||
if (not isFlagged(manager.error, Manager::RefereeDisConnect))
|
||||
{
|
||||
manager.measuredPower = powerMessage.getData().chassisPower;
|
||||
}
|
||||
else
|
||||
{
|
||||
manager.measuredPower = manager.estimatedPower;
|
||||
}
|
||||
#endif
|
||||
|
||||
// update power status
|
||||
powerStatus.userConfiguredMaxPower = manager.userConfiguredMaxPower;
|
||||
powerStatus.effectivePower = effectivePower;
|
||||
powerStatus.powerLoss = manager.measuredPower - effectivePower;
|
||||
powerStatus.efficiency = Utils::Math::clamp(effectivePower / manager.measuredPower, 0.0f, 1.0f);
|
||||
powerStatus.estimatedCapEnergy = static_cast<uint8_t>(manager.estimatedCapEnergy / 2100.0f * 255.0f);
|
||||
powerStatus.error = static_cast<Manager::ErrorFlags>(manager.error);
|
||||
|
||||
// Update the RLS parameters AND
|
||||
// Add dead zone AND
|
||||
// The Referee System could not detect negative power, leading to failure of real measurement.
|
||||
// So use estimated power to evaluate this situtation
|
||||
if (manager.rlsEnabled== Manager::Enable&& fabs(manager.measuredPower)> 5.0f &&
|
||||
not(isFlagged(manager.error, Manager::CAPDisConnect) && manager.estimatedPower<0))
|
||||
{
|
||||
params = manager.rls.update(samples, manager.measuredPower - effectivePower - manager.k3);
|
||||
manager.k1 = fmax(params[0][0], 1e-5f); // In case the k1 diverge to negative number
|
||||
manager.k2 = fmax(params[1][0], 1e-5f); // In case the k2 diverge to negative number
|
||||
}
|
||||
|
||||
manager.lastUpdateTick = now;
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(1));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @implements
|
||||
*/
|
||||
void init(const Manager&mana)
|
||||
{
|
||||
if (isInitialized)
|
||||
return;
|
||||
manager = mana;
|
||||
|
||||
// default value
|
||||
LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = manager.division == Division::SENTRY ? 10U : 1U;
|
||||
MIN_MAXPOWER_CONFIGURED = 30.0f;
|
||||
manager.powerUpperLimit = CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD + MAX_CAP_POWER_OUT;
|
||||
|
||||
Core::Communication::RefereeSystem::subscribeMessage(&powerMessage);
|
||||
Core::Communication::RefereeSystem::subscribeMessage(&robotMessage);
|
||||
|
||||
xTaskCreateStatic(powerDaemon, "power", 1024, nullptr, 10, xPowerTaskStack, &uxPowerTaskTCB);
|
||||
}
|
||||
|
||||
} // namespace Power
|
||||
} // namespace Control
|
||||
} // namespace Core
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,8 +1,265 @@
|
||||
//
|
||||
// Created by tux on 2025/11/5.
|
||||
//
|
||||
/**
|
||||
* @file PowerController.hpp
|
||||
* @version 2.0
|
||||
* @note The estimated power formula: P = τΩ + k1|Ω| + k2τ^2
|
||||
*/
|
||||
#pragma once
|
||||
// #include "AppConfig.h"
|
||||
|
||||
#ifndef TRONONEH7_SCAFFOLD_POWERCONTROL_H
|
||||
#define TRONONEH7_SCAFFOLD_POWERCONTROL_H
|
||||
#ifndef USE_POWER_CONTROLLER
|
||||
#define USE_POWER_CONTROLLER FALSE
|
||||
#endif
|
||||
|
||||
#endif //TRONONEH7_SCAFFOLD_POWERCONTROL_H
|
||||
#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
|
||||
|
||||
2
User_Code/module/software/powercontrol/powercontrol.md
Normal file
2
User_Code/module/software/powercontrol/powercontrol.md
Normal file
@@ -0,0 +1,2 @@
|
||||
# 功率控制模块
|
||||
|
||||
Reference in New Issue
Block a user