SkyLi
77 lines
2.2 KiB
C
77 lines
2.2 KiB
C
#include "MotorEncoder.h"
|
|
#include "ti_msp_dl_config.h"
|
|
#include "ti/driverlib/dl_timerg.h"
|
|
#include "zf_common_interrupt.h"
|
|
#include "zf_driver_exti.h"
|
|
#include "zf_driver_gpio.h"
|
|
|
|
#define MOTOR_ENCODER_LEFT_A_PIN A9
|
|
#define MOTOR_ENCODER_LEFT_B_PIN A12
|
|
|
|
volatile motor_encoder_data_t motor_speed_right = {.raw = 0};
|
|
volatile motor_encoder_data_t motor_speed_left = {.raw = 0};
|
|
/*
|
|
由于MSPM0G3507只有一个QEI，因此只能测量一个电机的编码器速度，另一个电机的速度需要通过GPIO中断模拟正交编码器来测量。
|
|
分配如下：
|
|
- QEI0：右电机编码器
|
|
- GPIO中断：左电机编码器
|
|
*/
|
|
|
|
//用中断模拟一个正交编码器(或者用输入捕获)
|
|
volatile int16_t left_encoder_count = 0;
|
|
//A相中断
|
|
void motor_encoder_exit_handler_A(uint32 state, void *ptr)
|
|
{
|
|
// 读取A相和B相的状态
|
|
uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
|
|
uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
|
|
// 根据A相和B相的状态更新计数
|
|
if(A_state == B_state)
|
|
{
|
|
left_encoder_count++;
|
|
}
|
|
else
|
|
{
|
|
left_encoder_count--;
|
|
}
|
|
|
|
}
|
|
//B相中断
|
|
void motor_encoder_exit_handler_B(uint32 state, void *ptr)
|
|
{
|
|
// 读取A相和B相的状态
|
|
uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
|
|
uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
|
|
// 根据A相和B相的状态更新计数
|
|
if(A_state == B_state)
|
|
{
|
|
left_encoder_count--;
|
|
}
|
|
else
|
|
{
|
|
left_encoder_count++;
|
|
}
|
|
}
|
|
|
|
void motor_encoder_init(void)
|
|
{
|
|
//使用QEI
|
|
DL_TimerG_startCounter(QEI_0_INST);
|
|
//使用GPIO中断模拟正交编码器
|
|
exti_init(MOTOR_ENCODER_LEFT_A_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_A, NULL);
|
|
exti_init(MOTOR_ENCODER_LEFT_B_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_B, NULL);
|
|
interrupt_set_priority(GPIOA_INT_IRQn, 0);
|
|
}
|
|
|
|
void motor_encoder_update(void)
|
|
{
|
|
// 读取右电机编码器
|
|
motor_speed_right.raw = (uint16_t)DL_TimerG_getTimerCount(QEI_0_INST);
|
|
DL_TimerG_setTimerCount(QEI_0_INST, 0); // 读取后清零
|
|
|
|
// 读取左电机编码器
|
|
motor_speed_left.value = left_encoder_count;
|
|
left_encoder_count = 0; // 读取后清零
|
|
}
|
|
|