cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(main): 统一外设初始化流程

Browse Source 
集中所有外设初始化于main函数起始处，通过返回值确认各模块是否成功启动；
refactor(adc): 修改采样器初始化返回值
将sampler构造函数替换为带返回值的init方法，在main函数中加入初始化结果判断，
确保系统启动时硬件配置正确加载。

feat(motor): 引入motor模块并重构电机控制逻辑
将GPIO操作封装进motor_sleep和motor_resume函数，提升代码可读性和维护性。
引入motor.h头文件，并在main.cpp中进行电机初始化统一管理。
修改FOC控制器中的shutdown与resume方法调用新接口。

refactor(filter): 优化卡尔曼滤波器实现
重命名状态变量xp为x，简化访问逻辑；移除冗余注释及未使用的getlastAngle方法；
调整getVelocity计算系数，提高速度估算精度；增加init方法支持初始状态设置。

refactor(angle_sensor): 移除冗余速度计算功能
删除传感器内部的速度估算相关数组和方法，交由外部滤波器处理以降低耦合度。
更新SPI片选宏定义以区分不同设备，并增强错误处理机制。

refactor(can): 调整CAN通信ID及初始化流程
更改状态与配置帧的标识符ID，添加对特定命令ID的响应处理如电机复位指令。
将CAN_transmiter构造函数改为init方法，配合主程序完成模块化初始化检查。
This commit is contained in:
cbb
2025-12-08 02:48:59 +08:00
parent 93cbf18f1f
commit 839a32ff04
16 changed files with 164 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+5 -1
View File
@@ -6,6 +6,7 @@
#include <cstring>
#include <gd32c10x_dma.h>
#include <stdint.h>
#include "systick.h"
constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
@@ -110,9 +111,10 @@ void adc_config(void) {
/**
* @brief 采样类初始化
*/
sampler::sampler() {
u8 sampler::init() {
dma_config(); // DMA采样ADC初始化
adc_config(); // ADC初始化
delay_1ms(1);
// 初始采样
for (int i = 0; i < FILTER_SIZE + 1; i++) {
mcu_temp_samples[i] =
@@ -130,7 +132,9 @@ sampler::sampler() {
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[i] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
delay_1ms(1);
}
return 0;
}
/**
* @brief 获取MCU温度
Robot_V2/device/Can.cpp
+19 -14
View File
@@ -6,6 +6,7 @@
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "motor.h"
#include "systick.h"
#include <cstdio>
#include <cstdlib>
@@ -21,7 +22,7 @@ constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
CAN_transmiter CAN0_transmiter; // CAN初始化
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -260,7 +261,7 @@ void can_loopback_init(void) {
can_parameter.prescaler = 6;
if (can_init(CANX, &can_parameter) != SUCCESS) {
u16 i = 200;
while (--i>0){
while (--i > 0) {
delay_1ms(10);
printf("can init error \r\n");
}
@@ -413,7 +414,7 @@ void can_loopback_test(void) {
\brief CAN_transmiter构造函数
\note 初始化CAN通信相关参数和硬件配置
*/
CAN_transmiter::CAN_transmiter() {
u8 CAN_transmiter::init() {
// 初始化CAN硬件
// can_config();
canfd_config();
@@ -426,6 +427,7 @@ CAN_transmiter::CAN_transmiter() {
// 初始化DMA缓冲区
memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
memset(can_tx_buffer, 0, sizeof(can_tx_buffer));
return 0;
}
/*!
@@ -440,7 +442,7 @@ void CAN_transmiter::update(void) {
rt_angle_union.f = FOC.rt_angle;
// rt_velocity_union.f = KTH7823.getVelocity();
rt_velocity_union.f = FOC.rt_velocity;
MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
@@ -618,19 +620,18 @@ u8 CAN_transmiter::CAN_all_Send() {
u8 CAN_transmiter::CANFD_all_Send() {
u32 success_flag = 0;
constexpr u32 STATUS_ID = 0x18FF0100;
constexpr u32 CONFIG_ID = 0x18FF0101;
constexpr u32 STATUS_ID = 0x00010100;
constexpr u32 CONFIG_ID = 0x00010101;
// 打包status数据
pack_u16_to_bytes(transmit_data, 0, mcu_temp);
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f));
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) |
((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2));
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2));
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
@@ -669,7 +670,7 @@ void CAN_transmiter::communication_check() {
// for (i = 0; i < receive_message.rx_dlen; i++) {
// printf(" %02x", receive_message.rx_data[i]);
// }
if (receive_message.rx_efid == 0x18ff0200) {
if (receive_message.rx_efid == 0x00010200) {
FOC.control_mode = receive_message.rx_data[0];
if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
FOC.limit_voltage) {
@@ -689,9 +690,13 @@ void CAN_transmiter::communication_check() {
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
FOC.target_velocity = target_velocity_union.f;
can0_receive_flag = RESET;
} else if (receive_message.rx_efid == 0x00010301) {
motor_reset();
}
else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
}
can0_receive_flag = RESET;
}
}
Robot_V2/device/Can.h
+1 -1
View File
@@ -101,7 +101,7 @@ class CAN_transmiter {
public:
// 添加构造函数声明
CAN_transmiter();
u8 init(void);
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
Robot_V2/device/adc.h
+1 -1
View File
@@ -21,7 +21,7 @@ class sampler {
float tempora_sample;
public:
sampler();
u8 init(void);
void update_mcu_temp(void);
void update_drv_temp(void);
void update_vbus_voltage(void);
Robot_V2/device/ang_sensor.cpp
+29 -49
View File
@@ -9,9 +9,10 @@
#include <cstring>
#include <stdint.h>
#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
@@ -204,37 +205,39 @@ void angle_sensor::KTH7823_init(void) {
/**
* @brief 角度传感器构造函数
*/
angle_sensor::angle_sensor(void) { KTH7823_init(); }
u8 angle_sensor::init(void) {
KTH7823_init();
return 0;
}
/**
* @brief 读取传感器数据
* @return 传感器数据
*/
inline float angle_sensor::read_sensorAngle(void) {
uint16_t raw_data;
uint16_t raw_data;
SPI_CS_ENABLE();
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
SPI_CS_DISABLE();
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
current_angle_deg = angle_deg;
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
return angle_deg;
}
#ifdef DEBUG
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
ANG_SPI_CS_ENABLE();
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
ANG_SPI_CS_DISABLE();
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
current_angle_deg = angle_deg;
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
return angle_deg;
}
#ifdef DEBUG
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
printf("Error reading angle from sensor.\n");
}
#endif
return (float)FAULT_RECEIVE;
}
#endif
return (float)FAULT_RECEIVE;
}
/**
* @brief 更新传感器角度（输出级）
@@ -262,9 +265,6 @@ float angle_sensor::update(void) {
delta += ANGLE_WRAP_VALUE;
}
std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1));
velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
m_cumulativeAngle += delta;
m_lastAngle = current_angle_deg;
@@ -285,26 +285,6 @@ float angle_sensor::getAngle(void) const {
*/
float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
/**
* @brief 获取速度值(输出级)
* @return 速度值(输出级)
*/
float angle_sensor::getVelocity(void) const {
return getRawVelocity() / REDUCTION_RATIO;
}
/**
* @brief 获取速度值(原始级)
* @return 速度值(原始级)
*/
float angle_sensor::getRawVelocity(void) const {
float velocity_filted = 0;
for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) {
velocity_filted += velocity[i];
}
return velocity_filted / (VEL_FILTER_SIZE - 1);
// return velocity[VEL_FILTER_SIZE - 1];
}
/**
* @brief 获取角度值(原始级)
* @return 角度值(原始级)
Robot_V2/device/ang_sensor.h
+1 -3
View File
@@ -14,14 +14,12 @@ constexpr u16 VEL_FILTER_SIZE = 4;
class angle_sensor {
public:
angle_sensor();
u8 init(void);
float read_sensorAngle(void);
float update(void);
void reset(void);
float getAngle(void) const;
float getRawAngle(void) const;
float getVelocity(void) const;
float getRawVelocity(void) const;
float getcurrent_sensorangle(void) const;
protected:
Robot_V2/device/motor.cpp
+38
View File
@@ -0,0 +1,38 @@
#include "motor.h"
#include "Timer.h"
#include "gd32c10x_eval.h"
#include <cstdio>
/**
* @brief driver输出初始化
*/
void motor_init(void) {
timer1_gpio_config();
timer1_pwm_config();
// 使能GPIOB时钟
rcu_periph_clock_enable(RCU_GPIOB);
// 配置PB2为输入引脚，用于读取电机状态
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
/* 配置GPIOB的PB14为推挽输出 SPICS */
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
GPIO_PIN_14 | GPIO_PIN_12);
}
/**
* @brief 电机复位
*/
void motor_reset(void) {
motor_sleep();
printf("motor reseting...\r\n");
motor_resume();
}
/**
* @brief 电机休眠
*/
void motor_sleep(void) { gpio_bit_reset(GPIOB, GPIO_PIN_12); }
/**
* @brief 电机启动
*/
void motor_resume(void) { gpio_bit_set(GPIOB, GPIO_PIN_12); }
Robot_V2/device/motor.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef _MOTOR_H
#define _MOTOR_H
void motor_init(void);
void motor_reset(void);
void motor_resume(void);
void motor_sleep(void);
#endif