92 lines
2.3 KiB
C++
92 lines
2.3 KiB
C++
#include "../2025_include_user/Encoder.h"
|
||
|
||
#define RADIUS_OF_TYRE 30 // 轮胎半径，单位：mm
|
||
#define LINE_SPEED_C (2.0f * 3.1415926f * RADIUS_OF_TYRE) // 一圈对应的线长，mm
|
||
#define SPEED_RATIO 50
|
||
|
||
Encoder::Speed Encoder::ticks = {0, 0};
|
||
Encoder::Speed Encoder::direction = {0, 0};
|
||
Encoder::Speed Encoder::speed = {0, 0};
|
||
Encoder::Speed Encoder::real_speed = {0, 0};
|
||
|
||
extern "C" {
|
||
void Encoder1_TIM_INST_IRQHandler(void)
|
||
{
|
||
switch (DL_TimerG_getPendingInterrupt(Encoder1_TIM_INST))
|
||
{
|
||
case DL_TIMERG_IIDX_LOAD:
|
||
Encoder::ticks.left++;
|
||
Encoder::direction.left = DL_GPIO_readPins(Encoder1_GPIO_PORT, Encoder1_GPIO_Encoder1B_GPIO_PIN) ? -1 : 1;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void Encoder2_TIM_INST_IRQHandler(void)
|
||
{
|
||
switch (DL_TimerA_getPendingInterrupt(Encoder2_TIM_INST))
|
||
{
|
||
case DL_TIMERG_IIDX_LOAD:
|
||
Encoder::ticks.right++;
|
||
Encoder::direction.right = DL_GPIO_readPins(Encoder2_GPIO_PORT, Encoder2_GPIO_Encoder2B_GPIO_PIN) ? -1 : 1;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void Encoder_Count_INST_IRQHandler(void) {
|
||
Encoder::inject();
|
||
}
|
||
}
|
||
|
||
void Encoder::init() {
|
||
NVIC_EnableIRQ(Encoder1_TIM_INST_INT_IRQN);
|
||
DL_TimerG_startCounter(Encoder1_TIM_INST);
|
||
|
||
NVIC_EnableIRQ(Encoder2_TIM_INST_INT_IRQN);
|
||
DL_TimerA_startCounter(Encoder2_TIM_INST);
|
||
|
||
NVIC_EnableIRQ(Encoder_Count_INST_INT_IRQN);
|
||
DL_TimerG_startCounter(Encoder_Count_INST);
|
||
}
|
||
|
||
void Encoder::inject() {
|
||
speed = {ticks.left * SPEED_RATIO, ticks.right * SPEED_RATIO};
|
||
real_speed = {ticks.left * SPEED_RATIO * direction.left, ticks.right * SPEED_RATIO * direction.right};
|
||
ticks = {0, 0};
|
||
}
|
||
|
||
Encoder::Speed Encoder::getSpeeds() {
|
||
return speed;
|
||
}
|
||
|
||
Encoder::Speed Encoder::getRealSpeeds() {
|
||
return real_speed;
|
||
}
|
||
|
||
int Encoder::getRealLeftSpeed() {
|
||
return real_speed.left;
|
||
}
|
||
|
||
int Encoder::getRealRightSpeed() {
|
||
return real_speed.right;
|
||
}
|
||
|
||
int Encoder::getLeftSpeed() {
|
||
return speed.left;
|
||
}
|
||
|
||
int Encoder::getRightSpeed() {
|
||
return speed.right;
|
||
}
|
||
|
||
int Encoder::getLeftDirection() {
|
||
return direction.left;
|
||
}
|
||
|
||
int Encoder::getRightDirection() {
|
||
return direction.right;
|
||
}
|