Skip to content

对电机库增加控制器前馈 #67

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Metalxiaoxiao merged 1 commit into from
Jul 17, 2026
Merged

对电机库增加控制器前馈 #67

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion GSRL/Device/inc/dvc_motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,9 @@ class Motor
Controller *m_controller; // 符合 Controller 接口的控制器
fp32 m_controllerOutput;
bool m_controllerOutputPolarity;
fp32 m_feedForward; // 一次性前馈, 任一闭环方法调用后自动清零
uint16_t m_encoderOffset;

public:
virtual ~Motor() = default;
// 通信相关
Expand All @@ -88,6 +89,7 @@ class Motor
// 控制器相关
void setController(Controller *controller);
void setControllerOutputPolarity(bool polarity);
void setFeedForwardOnce(fp32 feedForward);
void openloopControl(fp32 controlValue);
fp32 angleClosedloopControl();
fp32 angleClosedloopControl(fp32 targetAngle);
Expand All @@ -106,6 +108,7 @@ class Motor
fp32 updateCurrentRevolutions();
inline void increaseMotorFeedbackErrorCount();
inline void clearMotorFeedbackErrorCount();
inline void applyControllerOutputPostProcess();
};

/**
Expand Down
48 changes: 33 additions & 15 deletions GSRL/Device/src/dvc_motor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -263,6 +263,18 @@ void Motor::openloopControl(fp32 controlValue)
convertControllerOutputToMotorControlData();
}

/**
* @brief 设置前馈值
* @param feedForward 前馈变量
* @note 请在每次控制电机前调用，电机控制方法被调用后会自动清零前馈值
* @note 只对闭环方法生效，对硬件控制无效
* @note 前馈在极性反转后叠加
*/
void Motor::setFeedForwardOnce(fp32 feedForward)
{
m_feedForward = feedForward;
}

/**
* @brief 角度闭环控制
* @return fp32 控制器输出
Expand All @@ -273,9 +285,7 @@ fp32 Motor::angleClosedloopControl()
fp32 angleError = GSRLMath::normalizeDeltaAngle(m_currentAngle - m_targetAngle);
fp32 feedBackData[2] = {angleError, m_currentAngularVelocity};
m_controllerOutput = m_controller->controllerCalculate(0.0f, feedBackData, 2);
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
applyControllerOutputPostProcess();
convertControllerOutputToMotorControlData();
return m_controllerOutput;
}
Expand All @@ -299,9 +309,7 @@ fp32 Motor::angularVelocityClosedloopControl()
{
fp32 feedBackData[1] = {m_currentAngularVelocity};
m_controllerOutput = m_controller->controllerCalculate(m_targetAngularVelocity, feedBackData, 1);
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
applyControllerOutputPostProcess();
convertControllerOutputToMotorControlData();
return m_controllerOutput;
}
Expand All @@ -326,9 +334,7 @@ fp32 Motor::revolutionsClosedloopControl()
{
fp32 feedBackData[2] = {m_currentRevolutions, m_currentAngularVelocity};
m_controllerOutput = m_controller->controllerCalculate(m_targetRevolutions, feedBackData, 2);
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
applyControllerOutputPostProcess();
convertControllerOutputToMotorControlData();
return m_controllerOutput;
}
Expand All @@ -352,9 +358,7 @@ int16_t Motor::torqueCurrentClosedloopControl()
{
fp32 feedBackData[1] = {(fp32)m_currentTorqueCurrent};
m_controllerOutput = m_controller->controllerCalculate((fp32)m_targetTorqueCurrent, feedBackData, 1);
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
applyControllerOutputPostProcess();
convertControllerOutputToMotorControlData();
return m_controllerOutput;
}
Expand All @@ -379,9 +383,7 @@ int16_t Motor::torqueCurrentClosedloopControl(int16_t targetTorqueCurrent)
fp32 Motor::externalClosedloopControl(fp32 setPoint, const fp32 *feedBackData, uint8_t feedBackSize)
{
m_controllerOutput = m_controller->controllerCalculate(setPoint, feedBackData, feedBackSize);
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
applyControllerOutputPostProcess();
convertControllerOutputToMotorControlData();
return m_controllerOutput;
}
Expand Down Expand Up @@ -415,6 +417,7 @@ Motor::Motor(uint32_t canControlID, uint32_t canFeedbackID, Controller *controll
m_controller(controller),
m_controllerOutput(0.0f),
m_controllerOutputPolarity(false),
m_feedForward(0.0f),
m_encoderOffset(encoderOffset)
{
m_motorControlHeader.DLC = 8;
Expand Down Expand Up @@ -466,6 +469,21 @@ inline void Motor::clearMotorFeedbackErrorCount()
m_isMotorConnected = true;
}

/**
* @brief 电机接收到控制器输出的后处理
* @note 前馈自动归零
* @note 前馈在控制器极性调换之后应用
*/
inline void Motor::applyControllerOutputPostProcess()
{
if (m_controllerOutputPolarity) {
m_controllerOutput = -m_controllerOutput;
}
m_controllerOutput += m_feedForward;
m_feedForward = 0.0f;
}


/******************************************************************************
* GM6020电机类实现
******************************************************************************/
Expand Down
Loading