generated from Template/H563ZI-HAL-CMake-Template
136 lines
3.0 KiB
C
136 lines
3.0 KiB
C
#include "encoder.h"
|
||
|
||
volatile int32_t tick_L = 0,tick_R = 0;
|
||
volatile int16_t rpm_L = 0,rpm_R = 0;
|
||
|
||
extern TIM_HandleTypeDef htim1;
|
||
extern TIM_HandleTypeDef htim8;
|
||
|
||
static TX_THREAD enc_thread;
|
||
static UCHAR enc_stack[512];
|
||
|
||
static void enc_thread_entry(ULONG);
|
||
|
||
UINT Encoder_ThreadCreate(void)
|
||
{
|
||
UINT status;
|
||
|
||
HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
|
||
HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
|
||
|
||
__HAL_TIM_SET_COUNTER(&htim1,0);
|
||
__HAL_TIM_SET_COUNTER(&htim8,0);
|
||
|
||
status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
|
||
enc_stack, sizeof(enc_stack),
|
||
6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
|
||
|
||
return status;
|
||
}
|
||
|
||
|
||
static void enc_thread_entry(ULONG arg)
|
||
{
|
||
const float msPerMin = 6000.0f;
|
||
const float ticksPerRev = 1024.0f; // ÏßÊý * 4
|
||
uint16_t lastL = 0, lastR = 0;
|
||
|
||
while (1)
|
||
{
|
||
uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
|
||
uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);
|
||
|
||
int16_t dL = (int16_t)(cntL - lastL);
|
||
int16_t dR = (int16_t)(cntR - lastR);
|
||
|
||
lastL = cntL;
|
||
lastR = cntR;
|
||
|
||
tick_L += dL;
|
||
tick_R += dR;
|
||
|
||
rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
|
||
rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));
|
||
|
||
tx_thread_sleep(10);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//
|
||
|
||
//ControlThread
|
||
//
|
||
|
||
static TX_THREAD control_thread;
|
||
static UCHAR control_stack[512];
|
||
|
||
volatile int16_t target_rpm_L = 0;
|
||
volatile int16_t target_rpm_R = 0;
|
||
|
||
static void control_thread_entry(ULONG arg);
|
||
|
||
|
||
UINT ControlThreadCreate(void)
|
||
{
|
||
UINT status;
|
||
status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
|
||
control_stack, sizeof(control_stack),
|
||
7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
|
||
|
||
return status;
|
||
}
|
||
|
||
|
||
static void control_thread_entry(ULONG arg)
|
||
{
|
||
static int16_t pwmL = 0, pwmR = 0;
|
||
static float integralL = 0, integralR = 0;
|
||
float Kp = 1.0f, Ki = 0.1f;
|
||
|
||
while (1)
|
||
{
|
||
int16_t errorL = target_rpm_L - rpm_L;
|
||
int16_t errorR = target_rpm_R - rpm_R;
|
||
|
||
integralL += errorL;
|
||
integralR += errorR;
|
||
|
||
// ÔÚ»ý·ÖÀÛ¼ÓºóÌí¼Ó¿¹±¥ºÍÂß¼
|
||
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;
|
||
// ͬÑù´¦ÀíintegralR...
|
||
|
||
pwmL += (int16_t)(Kp * errorL + Ki * integralL);
|
||
pwmR += (int16_t)(Kp * errorR + Ki * integralR);
|
||
|
||
if (pwmL < 0) pwmL = 0;
|
||
if (pwmL > 255) pwmL = 255;
|
||
if (pwmR < 0) pwmR = 0;
|
||
if (pwmR > 255) pwmR = 255;
|
||
|
||
DriveBOTH(pwmL, pwmR);
|
||
|
||
tx_thread_sleep(10);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
int16_t map_speed_to_rpm(int speed)
|
||
{
|
||
if (abs(speed) < 30) return 0;
|
||
return (speed - 60) * 1.5; // ¾ÙÀý
|
||
}
|
||
|