MADAO/ManGoWalk_STM32
1
0
Fork 0
generated from Template/H563ZI-HAL-CMake-Template
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Final

Browse Source
This commit is contained in:
MADAO
2025-07-08 23:30:16 +08:00
parent a635b6d5eb
commit a9884051f8
21 changed files with 7673 additions and 7958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
fun/encoder.c
View File

@@ -85,8 +85,8 @@ UINT ControlThreadCreate(void)
static void control_thread_entry(ULONG arg)
{
int16_t pwmL = 0, pwmR = 0;
float integralL = 0, integralR = 0;
static int16_t pwmL = 0, pwmR = 0;
static float integralL = 0, integralR = 0;
float Kp = 1.0f, Ki = 0.1f;
while (1)
@@ -96,7 +96,19 @@ static void control_thread_entry(ULONG arg)
integralL += errorL;
integralR += errorR;
// <20>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD>ۼӺ<DBBC><D3BA><EFBFBD><EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD>
if (errorL == 0) integralL = 0;
if (errorR == 0) integralR = 0;
const float max_integral = 1000.0f;
if(integralL > max_integral) integralL = max_integral;
if(integralL < -max_integral) integralL = -max_integral;
if(integralR > max_integral) integralR = max_integral;
if(integralR < -max_integral) integralR = -max_integral;
// ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>integralR...
pwmL += (int16_t)(Kp * errorL + Ki * integralL);
pwmR += (int16_t)(Kp * errorR + Ki * integralR);
@@ -111,9 +123,13 @@ static void control_thread_entry(ULONG arg)
}
}
int16_t map_speed_to_rpm(int speed)
{
if (speed < 60) return 0;
if (abs(speed) < 30) return 0;
return (speed - 60) * 1.5; // <20><><EFBFBD><EFBFBD>
}