generated from Template/H563ZI-HAL-CMake-Template
120 lines
2.5 KiB
C
120 lines
2.5 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)
|
|
{
|
|
int16_t pwmL = 0, pwmR = 0;
|
|
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;
|
|
|
|
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 (speed < 60) return 0;
|
|
return (speed - 60) * 1.5; // ¾ÙÀý
|
|
}
|
|
|