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

FOC开环调试成功

Browse Source
This commit is contained in:
cbb
2025-10-16 21:43:19 +08:00
parent 2d807260c5
commit 35801d6f48
13 changed files with 704 additions and 509 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/Filter.cpp
+4 -1
View File
@@ -77,7 +77,10 @@ CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
: x(initial_current), P(1.0f), // 初始误差协方差
Q(Q), // 过程噪声
R(R_noise), // 测量噪声
L(L), R_m(R), T(T) {}
L(L), R_m(R), T(T) {
CircularBuffer buffer;
}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_current 测量电流值
Robot_V2/Algorithm/Filter.h
-3
View File
@@ -66,9 +66,6 @@ class CurrentKalman {
void setMeasurementNoise(float r);
};
extern CurrentKalman kalman;
extern CircularBuffer buffer;
#endif // __cplusplus
#endif // _FILTER_H
Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c
+2 -1
View File
@@ -978,7 +978,8 @@ void can_message_receive(uint32_t can_periph, uint8_t fifo_number, can_receive_m
receive_message->rx_data[7] = (uint8_t)(GET_RFIFOMDATA1_DB7(CAN_RFIFOMDATA1(can_periph, fifo_number)));
} else {
CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
printf("CAN FD function disabled but FD frame received\r\n");
// CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
}
} else {
Robot_V2/Robot_V2.cproject.yml
+1 -1
View File
@@ -1,4 +1,3 @@
# A project translates into one executable or library.
project:
groups:
- group: Source Files
@@ -21,6 +20,7 @@ project:
- file: device/adc.h
- file: device/Timer.cpp
- file: device/Timer.h
- file: device/test.cpp
- group: Algorithm
files:
- file: Algorithm/Filter.cpp
Robot_V2/device/ADC.cpp
+2 -2
View File
@@ -7,7 +7,7 @@ constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f;
constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f;
constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f * 1000.0f;
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 50.0f;
@@ -102,7 +102,7 @@ float get_vbus_voltage() {
float vcc_latest = ERROR_ADC_VAL;
for (int i = 0; i < 5; i++) {
// 获取电压值
vcc_latest = adc_value[6] * ADC_SCALE * VCC_FACTOR;
vcc_latest = adc_value[5] * ADC_SCALE * VCC_FACTOR;
// 检查电压读数是否在有效范围内
if (vcc_latest > MAX_VCC || vcc_latest < MIN_VCC) {
Robot_V2/device/Can.cpp
+264 -134
View File
@@ -1,26 +1,41 @@
#include "Can.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_eval.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <inttypes.h>
#include <random>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#define DEV_CAN0_ID 0x0000
#define DEV_CAN0_MASK 0x0000
#define CANX CAN0
#define DEV_CAN_LOOPBACK_ID 0x0000
#define DEV_CAN0_ID 0xaabb
#define DEV_CAN0_MASK 0x0000
extern CAN_transmiter CAN0_transmiter;
/*!
\brief initialize CAN GPIO
\param[in] none
\param[out] none
\retval ErrStatus: SUCCESS or ERROR
*/
u8 can_gpio_init() {
/* enable CAN clock */
rcu_periph_clock_enable(RCU_CAN0);
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_AF);
/* configure CAN0 GPIO */
gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_MAX, GPIO_PIN_11);
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_MAX, GPIO_PIN_12);
/* enable the EN GPIO clock */
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_AF);
/* configure CAN0 GPIO */
gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_11); // tx
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12); // rx
gpio_pin_remap_config(GPIO_CAN0_FULL_REMAP, DISABLE);
/* enable the EN GPIO clock */
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13);
gpio_bit_reset(GPIOB, GPIO_PIN_13);
return 0;
@@ -33,6 +48,7 @@ u8 can_gpio_init() {
\retval none
*/
void can_config(void) {
can_gpio_init();
can_parameter_struct can_parameter;
can_filter_parameter_struct can_filter;
@@ -52,31 +68,31 @@ void can_config(void) {
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 12;
can_parameter.prescaler = 6;
/* initialize CAN */
can_init(CAN0, &can_parameter);
if (can_init(CAN0, &can_parameter) != SUCCESS) {
while (1)
printf("can init error \r\n");
}
/* initialize filter */
/* configure filter mode */
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
/* configure filter ID */
can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U);
can_filter.filter_list_low = ((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2);
/* configure filter mask */
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
/* select receiver fifo */
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
/* configure filter number */
can_filter.filter_number = 0;
can_filter_init(&can_filter);
/* configure CAN0 NVIC */
nvic_irq_enable(CAN0_RX0_IRQn, 0, 0);
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
}
/*!
@@ -86,18 +102,16 @@ void can_config(void) {
\retval none
*/
void canfd_config(void) {
can_gpio_init();
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
can_filter_parameter_struct can_filter;
/* enable CAN clock */
rcu_periph_clock_enable(RCU_CAN0);
/* initialize CAN register */
can_deinit(CAN0);
/* initialize CAN parameters */
/* initialize CAN */
can_parameter.time_triggered = DISABLE;
can_parameter.auto_bus_off_recovery = DISABLE;
can_parameter.auto_wake_up = DISABLE;
@@ -109,8 +123,7 @@ void canfd_config(void) {
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 12;
/* initialize CAN */
can_parameter.prescaler = 6;
can_init(CAN0, &can_parameter);
can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter);
@@ -129,15 +142,27 @@ void canfd_config(void) {
can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ;
/* CAN-FD data segement prescaler should be the same as non-data segement
* prescaler */
can_fd_parameter.data_prescaler = 12;
can_fd_parameter.data_prescaler = 6;
can_fd_init(CAN0, &can_fd_parameter);
/* initialize filter */
can1_filter_start_bank(14);
can_filter_mask_mode_init(DEV_CAN0_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0);
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
/* configure filter ID */
can_filter.filter_list_high = 0x0000;
can_filter.filter_list_low = 0x0000;
/* configure filter mask */
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
/* select receiver fifo */
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
/* configure filter number */
can_filter.filter_number = 0;
can_filter_init(&can_filter);
/* configure CAN0 NVIC */
nvic_irq_enable(CAN0_RX0_IRQn, 0, 0);
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
}
@@ -149,103 +174,100 @@ void canfd_config(void) {
\retval none
*/
void can_loopback_init(void) {
can_gpio_init();
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
can_filter_parameter_struct can_filter;
/* enable CAN clock */
rcu_periph_clock_enable(RCU_CAN0);
/* initialize CAN register */
can_deinit(CAN0);
can_deinit(CANX);
/* initialize CAN parameters */
/* initialize CAN */
can_parameter.time_triggered = DISABLE;
can_parameter.auto_bus_off_recovery = DISABLE;
can_parameter.auto_wake_up = DISABLE;
can_parameter.auto_retrans = ENABLE;
can_parameter.rec_fifo_overwrite = ENABLE;
can_parameter.trans_fifo_order = ENABLE;
can_parameter.working_mode = CAN_LOOPBACK_MODE;
can_parameter.auto_retrans = DISABLE;
can_parameter.rec_fifo_overwrite = DISABLE;
can_parameter.trans_fifo_order = DISABLE;
can_parameter.working_mode = CAN_NORMAL_MODE;
/* baudrate 1Mbps, sample piont at 80% */
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 12;
/* initialize CAN */
can_init(CAN0, &can_parameter);
can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter);
can_fd_parameter.fd_frame = ENABLE;
can_fd_parameter.excp_event_detect = ENABLE;
can_fd_parameter.delay_compensation = ENABLE;
can_fd_tdc_parameter.tdc_filter = 0x04;
can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET;
can_fd_tdc_parameter.tdc_offset = 0x04;
can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter;
can_fd_parameter.iso_bosch = CAN_FDMOD_ISO;
can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE;
/* FD mode data baudrate 2Mbps, sample piont at 80% */
can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ;
can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ;
can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ;
/* CAN-FD data segement prescaler should be the same as non-data segement
* prescaler */
can_fd_parameter.data_prescaler = 12;
can_fd_init(CAN0, &can_fd_parameter);
can_parameter.prescaler = 6;
if (can_init(CANX, &can_parameter) != SUCCESS) {
while (1)
printf("can init error \r\n");
}
/* initialize filter */
can1_filter_start_bank(14);
can_filter_mask_mode_init(DEV_CAN_LOOPBACK_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0);
/* configure filter mode */
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
/* configure filter ID */
can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U);
can_filter.filter_list_low =
((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2);
/* configure filter mask */
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
/* select receiver fifo */
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
/* configure filter number */
can_filter.filter_number = 0;
can_filter_init(&can_filter);
}
/*!
\brief CAN0 loopback test
\brief CAN0 FD回环测试
\param[in] none
\param[out] none
\retval ErrStatus
*/
void can_loopback_test(void) {
void canfd_loopback_test(void) {
can_trasnmit_message_struct transmit_message;
can_receive_message_struct receive_message;
uint32_t timeout = 0xFFFF;
uint8_t transmit_mailbox = 0;
std::mt19937 gen;
std::uniform_int_distribution<int> dis(0, 255);
can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message);
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
transmit_message.tx_efid = 0x00;
transmit_message.tx_ft = CAN_FT_DATA;
transmit_message.tx_ff = CAN_FF_EXTENDED;
transmit_message.tx_dlen = 12;
transmit_message.tx_dlen = 7;
transmit_message.fd_flag = 1;
transmit_message.fd_brs = 1;
transmit_message.fd_esi = 0;
int i = 0;
while (i++<transmit_message.tx_dlen) {
transmit_message.tx_data[i] = i;
while (i++ < transmit_message.tx_dlen) {
transmit_message.tx_data[i] = dis(gen);
std::printf("%x\n", transmit_message.tx_data[i]);
}
/* transmit message */
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
/* waiting for transmit completed */
while((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && (0 != timeout)) {
while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) &&
(0 != timeout)) {
timeout--;
}
timeout = 0xFFFF;
/* waiting for receive completed */
while((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) && (0 != timeout)) {
while ((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) &&
(0 != timeout)) {
timeout--;
}
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
can_message_receive(CAN0, CAN_FIFO0, &receive_message);
/* check the receive message */
if((transmit_message.tx_sfid == receive_message.rx_sfid) && (transmit_message.tx_ff == receive_message.rx_ff)
&& (transmit_message.tx_dlen == receive_message.rx_dlen) ) {
if ((transmit_message.tx_sfid == receive_message.rx_sfid) &&
(transmit_message.tx_ff == receive_message.rx_ff) &&
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
/* loopback test is success */
std::printf("CAN LO0PBACK SUCCESS\r\n");
} else {
@@ -254,38 +276,84 @@ void can_loopback_test(void) {
}
}
/**
\brief CAN0 回环测试
\param[in] none
\param[out] none
\retval none
*/
void can_loopback_test(void) {
can_trasnmit_message_struct transmit_message;
can_receive_message_struct receive_message;
uint32_t timeout = 0xF;
uint8_t transmit_mailbox = 0;
std::mt19937 gen(rand()); // 使用随机设备生成的种子
std::uniform_int_distribution<int> dis(0, 0xff);
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
/* initialize transmit message */
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
transmit_message.tx_ft = CAN_FT_DATA;
transmit_message.tx_ff = CAN_FF_STANDARD;
transmit_message.tx_dlen = 7;
int i = 0;
while (i++ < transmit_message.tx_dlen) {
// transmit_message.tx_data[i] = dis(gen);
transmit_message.tx_data[i] = i;
}
/* transmit message */
transmit_mailbox = can_message_transmit(CANX, &transmit_message);
/* waiting for transmit completed */
while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) &&
(0 != timeout)) {
printf("sending:%x...\n", can_transmit_states(CANX, transmit_mailbox));
timeout--;
}
timeout = 0xFF;
/* waiting for receive completed */
while ((can_receive_message_length_get(CANX, CAN_FIFO0) < 1) &&
(0 != timeout)) {
timeout--;
}
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
can_message_receive(CANX, CAN_FIFO0, &receive_message);
/* check the receive message */
if ((DEV_CAN_LOOPBACK_ID == receive_message.rx_sfid) &&
(CAN_FF_STANDARD == receive_message.rx_ff) &&
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
i = transmit_message.tx_dlen;
while (i-- > 0) {
std::printf("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
receive_message.rx_data[i]);
}
if (std::memcmp(receive_message.rx_data, transmit_message.tx_data,
transmit_message.tx_dlen) == 0) {
/* loopback test is success */
std::printf("loopback success\n");
} else {
std::printf("loopback failed\n");
}
} else {
/* loopback test is failed */
std::printf("loopback failed\n");
}
}
/*!
\brief CAN_transmiter构造函数
\note 初始化CAN通信相关参数和硬件配置
*/
CAN_transmiter::CAN_transmiter() {
// 初始化成员变量
controller_Mode = 0;
limit_velocity_union.f = 0;
limit_voltage = 0;
mcu_temp = 0;
motor_temp = 0;
Power_Voltage = 0;
rt_angle_union.f = 0;
Status_code = 0;
target_angle_union.f = 0;
Target_Control_Mode = 0;
target_velocity_union.f = 0;
u_current_union.f = 0;
w_current_union.f = 0;
v_current_union.f = 0;
// 初始化CAN硬件
can_gpio_init();
can_config();
printf("CAN_transmiter initialized successfully\r\n");
transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
timeout = 0xFFFF; // 默认超时时间 0xFFFF
// can_config();
canfd_config();
// 初始化发送消息结构体
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
/* initialize receive message */
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
}
/*!
@@ -293,25 +361,26 @@ CAN_transmiter::CAN_transmiter() {
*/
void CAN_transmiter::update(void) {
limit_velocity_union.f = 0;
limit_voltage = 0;
mcu_temp = get_mcu_temp();
motor_temp = get_drv_temp();
Power_Voltage = get_vbus_voltage();
limit_voltage = 0 * 1000;
mcu_temp = get_mcu_temp() * 100;
motor_temp = get_drv_temp() * 100;
Power_Voltage = get_vbus_voltage() * 1000;
rt_angle_union.f = 0;
rt_velocity_union.f = 0;
Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 1 : 0;
target_angle_union.f = 0;
Target_Control_Mode = 0;
target_velocity_union.f = 0;
u_current_union.f = get_current('U');
w_current_union.f = get_current('W');
v_current_union.f = get_current('V');
u_current_union.f = get_current('U') * 1000;
w_current_union.f = get_current('W') * 1000;
v_current_union.f = get_current('V') * 1000;
}
/*!
\brief CAN 数据发送
*/
u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
timeout = 0xFFFF;
// 检查数据长度是否符合CAN规范
if (length > 8) {
printf("CAN data length exceeds maximum of 8 bytes\r\n");
@@ -326,7 +395,7 @@ u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式
transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_dlen = length; // 设置数据长度
@@ -334,7 +403,6 @@ u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
for (int i = 0; i < length && i < 8; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
@@ -359,11 +427,11 @@ u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
return 1;
}
/*!
\brief CANFD 数据发送
*/
u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
timeout = 0xFFFF;
// 检查CANFD模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
printf("CANFD mode not enabled\r\n");
@@ -385,7 +453,8 @@ u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
transmit_message.tx_sfid = id; // 设置帧ID
// transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_efid = id; // 扩展帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 设置CANFD特定参数
@@ -397,7 +466,6 @@ u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
for (int i = 0; i < length && i < 64; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
@@ -416,19 +484,19 @@ u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
// 超时检查
if (0 == timeout) {
printf("CANFD transmission timeout for mailbox %d\r\n", transmit_mailbox);
printf("CANFD transmission timeout for mailbox %d\r\n",
transmit_mailbox);
return 0;
}
return 1;
}
/**
* @brief CAN发送函数
* @retval 0: 发送失败
* 1: 发送成功
*/
u8 CAN_transmiter::CAN_all_Send(){
u8 CAN_transmiter::CAN_all_Send() {
u32 success_flag = 0;
constexpr u32 RT_Vel_Ang_FEED = 0x101;
@@ -437,25 +505,32 @@ u8 CAN_transmiter::CAN_all_Send(){
constexpr u32 Status_FEED = 0x118;
constexpr u32 Limit_Vel_Vol_FEED = 0x119;
//打包RT_Vel_Ang_FEED
// 打包RT_Vel_Ang_FEED
pack_float_to_bytes(transmit_data, 0, rt_angle_union);
pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
//打包SET_Vel_Ang_FEED
success_flag += CAN_Send(RT_Vel_Ang_FEED, 8, transmit_data);
// 打包SET_Vel_Ang_FEED
pack_float_to_bytes(transmit_data, 0, target_angle_union);
pack_float_to_bytes(transmit_data, 4, target_velocity_union);
//打包Current_FEED
pack_float_to_bytes(transmit_data, 0, u_current_union);
pack_float_to_bytes(transmit_data, 4, w_current_union);
pack_float_to_bytes(transmit_data, 8, v_current_union);
//打包Status_FEED
success_flag += CAN_Send(SET_Vel_Ang_FEED, 8, transmit_data);
// 打包Current_FEED
pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
success_flag += CAN_Send(Current_FEED, 6, transmit_data);
// 打包Status_FEED
transmit_data[0] = Status_code;
transmit_data[1] = controller_Mode;
pack_short_to_bytes(transmit_data, 2, motor_temp);
pack_short_to_bytes(transmit_data, 4, mcu_temp);
pack_short_to_bytes(transmit_data, 6, Power_Voltage);
//打包Limit_Vel_Vol_FEED
success_flag += CAN_Send(Status_FEED, 8, transmit_data);
// 打包Limit_Vel_Vol_FEED
pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
pack_short_to_bytes(transmit_data, 4, limit_voltage);
success_flag += CAN_Send(Limit_Vel_Vol_FEED, 6, transmit_data);
return success_flag;
}
@@ -464,24 +539,22 @@ u8 CAN_transmiter::CAN_all_Send(){
* @retval 0: 发送失败
* 1: 发送成功
*/
u8 CAN_transmiter::CANFD_all_Send(){
u8 CAN_transmiter::CANFD_all_Send() {
u32 success_flag = 0;
constexpr u32 STATUS_ID = 0x18FF100;
constexpr u32 CONFIG_ID = 0x18FF101;
constexpr u32 STATUS_ID = 0x18FF0100;
constexpr u32 CONFIG_ID = 0x18FF0101;
// 打包status数据
pack_short_to_bytes(transmit_data, 0, mcu_temp);
pack_short_to_bytes(transmit_data, 2, motor_temp);
pack_float_to_bytes(transmit_data, 4, u_current_union);
pack_float_to_bytes(transmit_data, 8, w_current_union);
pack_float_to_bytes(transmit_data, 12, v_current_union);
transmit_data[16] = Status_code;
pack_u16_to_bytes(transmit_data, 17, Power_Voltage);
pack_float_to_bytes(transmit_data, 19, rt_angle_union);
pack_float_to_bytes(transmit_data, 23, rt_velocity_union);
success_flag += CANFD_Send(STATUS_ID, 27, transmit_data);
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f));
transmit_data[10] = Status_code;
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);
success_flag += CANFD_Send(STATUS_ID, 20, transmit_data);
// 打包config数据
transmit_data[0] = controller_Mode;
@@ -489,8 +562,65 @@ u8 CAN_transmiter::CANFD_all_Send(){
pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
pack_float_to_bytes(transmit_data, 7, target_angle_union);
pack_float_to_bytes(transmit_data, 11, target_velocity_union);
success_flag += CANFD_Send(CONFIG_ID, 15, transmit_data);
success_flag += CANFD_Send(CONFIG_ID, 16, transmit_data);
return success_flag;
}
/**
* @brief CAN通信检查函数
* @retval 0: 无新消息
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
uint8_t i = 0;
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
CAN0_transmiter.update();
// CAN0_transmiter.CAN_all_Send();
CAN0_transmiter.CANFD_all_Send();
can0_send_flag = RESET;
}
// CAN0 接收数据标志
if (SET == can0_receive_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
// for (i = 0; i < receive_message.rx_dlen; i++) {
// printf(" %02x", receive_message.rx_data[i]);
// }
can0_receive_flag = RESET;
}
}
/**
* @brief CAN0中断服务程序
* @retval 无
*/
void CAN0_RX0_IRQHandler(void) {
// 检查是否是FIFO0消息挂起中断
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) {
// 清除中断标志
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
// 读取接收到的数据
can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message);
// 接收标志位置1
CAN0_transmiter.can0_receive_flag = SET;
}
// 检查其他可能的中断源
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) {
// FIFO0满中断
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);
printf("CAN FIFO0 full\n");
}
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) {
// FIFO0溢出中断
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0);
printf("CAN FIFO0 overflow\n");
}
}
Robot_V2/device/Can.h
+40 -35
View File
@@ -18,28 +18,31 @@ typedef union {
} float_union_t;
// 添加辅助函数用于将short类型数据打包到u8数组中
static inline void pack_short_to_bytes(u8* buffer, u16 offset, short value) {
static inline void pack_short_to_bytes(u8 *buffer, u16 offset, short value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
}
// 添加辅助函数用于将u16类型数据打包到u8数组中
static inline void pack_u16_to_bytes(u8* buffer, u16 offset, u16 value) {
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
}
// 添加辅助函数用于将float类型数据打包到u8数组中
static inline void pack_float_to_bytes(u8* buffer, u16 offset, float_union_t& float_union) {
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
float_union_t &float_union) {
buffer[offset] = float_union.b[0];
buffer[offset + 1] = float_union.b[1];
buffer[offset + 2] = float_union.b[2];
buffer[offset + 3] = float_union.b[3];
}
u8 can_gpio_init(void);
void can_config(void);
void canfd_config(void);
u8 can_gpio_init(void); // CAN引脚初始化
void can_config(void); // CAN初始化
void canfd_config(void);// CANFD初始化
void CAN0_RX0_IRQHandler(void);// CAN接收中断处理函数
void can_loopback_init(void);
void can_loopback_test(void);
@@ -50,42 +53,44 @@ void can_loopback_test(void);
class CAN_transmiter {
private:
u8 controller_Mode;
float_union_t limit_velocity_union;
u16 limit_voltage;
short mcu_temp;
short motor_temp;
u16 Power_Voltage;
float_union_t rt_angle_union;
float_union_t rt_velocity_union;
u8 Status_code;
float_union_t target_angle_union;
u8 Target_Control_Mode;
float_union_t target_velocity_union;
float_union_t u_current_union;
float_union_t w_current_union;
float_union_t v_current_union;
u8 controller_Mode = 0; // 电机模式
float_union_t limit_velocity_union = {0}; // 限制速度
u16 limit_voltage = 0; // 限制电压
short mcu_temp = 0; // MCU温度
short motor_temp = 0; // 电机温度
u16 Power_Voltage = 0; // 电源电压
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
u8 Status_code = 0; // 状态码
float_union_t target_angle_union = {0}; // 目标角度
u8 Target_Control_Mode = 0; // 目标控制模式
float_union_t target_velocity_union = {0}; // 目标速度
float_union_t u_current_union = {0}; // U相电流
float_union_t w_current_union = {0}; // W相电流
float_union_t v_current_union = {0}; // V相电流
u8 transmit_mailbox; // 初始化为无mailbox状态
u32 timeout; // 默认超时为0xFFFF
can_trasnmit_message_struct transmit_message; // 发送消息结构体
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
u32 timeout = 0XFFFF; // 默认超时为0xFFFF
u8 transmit_data[64]; // CANFD最大64字节
can_trasnmit_message_struct transmit_message;
can_receive_message_struct receive_message;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
void update(); // 更新数据
u8 CAN_Receive(); // CAN接收
u8 CANFD_Receive(); // CANFD接收
u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送
u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送
u8 CAN_all_Send(); // CAN发送
u8 CANFD_all_Send(); // CANFD发送
public:
// 添加构造函数声明
CAN_transmiter();
u8 CAN_Send(u32 ID, u8 length, u8 *data);
u8 CANFD_Send(u32 ID, u8 length, u8 *data);
void update();
u8 CAN_all_Send();
u8 CANFD_all_Send();
u8 CAN_Receive();
u8 CANFD_Receive();
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
void communication_check(); // 通信检查
};
#ifdef __cplusplus
Robot_V2/device/Timer.cpp
+28 -27
View File
@@ -1,22 +1,31 @@
#include "Timer.h"
#include "Can.h"
#include "adc.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_timer.h"
#include "initial.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
constexpr int TIMER1_PRESCALER = 1199;
constexpr int TIMER1_PERIOD = 9;
// 配置定时器每100微秒中断一次
// 系统时钟为120MHz，要得到100微秒的中断周期
// 可以使用预分频器119，定时器时钟为120MHz/(119+1) = 1MHz
// 然后设置周期为99，即计数100次，1MHz/100 = 10kHz = 100微秒
constexpr int TIMER1_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER1_PERIOD = 99; // 1MHz / 100 = 100us
constexpr int TIMER0_PRESCALER = 5999;
constexpr int TIMER0_PERIOD = 15999;
constexpr int TIMER0_CH0_PULSE = 4000;
constexpr int TIMER0_CH1_PULSE = 6000;
constexpr int TIMER0_CH2_PULSE = 8000;
static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern CAN_transmiter CAN0_transmiter;
/**
* @brief 设置引脚中断优先级
@@ -40,8 +49,8 @@ void nvic_config(void) {
void timer1_config(void) {
/* ----------------------------------------------------------------------------
TIMER1 Configuration:
TIMER1CLK = SystemCoreClock/12000 = 10KHz, the period is 1s(10000/10000 =
1s).
TIMER1CLK = SystemCoreClock/120 = 1MHz, the period is 100us(100/1000000 =
100us).
----------------------------------------------------------------------------
*/
timer_parameter_struct timer_struct;
@@ -55,11 +64,11 @@ void timer1_config(void) {
timer_struct_para_init(&timer_struct);
/* TIMER1配置参数设置 */
timer_struct.prescaler =
TIMER1_PRESCALER; /* 预分频值：12000分频(从0开始计数) */
TIMER1_PRESCALER; /* 预分频值：120分频(从0开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER1_PERIOD; /* 自动重装载值：10000次计数(从0开始计数) */
TIMER1_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
timer_init(TIMER1, &timer_struct);
@@ -92,35 +101,27 @@ void TIMER1_IRQHandler(void) {
if (timer_interrupt_flag_get(TIMER1, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER1, TIMER_INT_UP);
// 增加总计数器
total_us100_counter++;
// 使用静态变量来跟踪定时器事件
static u8 ms_counter = 0;
static u8 tens_counter = 0;
static u8 hundreds_counter = 0;
static u16 seconds_counter = 0;
// 每100us执行一次的操作可以放在这里
// 增加计数器
ms_counter++;
if (ms_counter >= 10) {
ms_counter = 0;
tens_counter++;
if (total_us100_counter % 10 == 0) {
// 每1ms执行的代码可以放在这里
if (tens_counter >= 10) {
tens_counter = 0;
hundreds_counter++;
CAN0_transmiter.can0_send_flag = SET;
if (total_us100_counter % 100 == 0) {
// 每10ms执行的代码可以放在这里
}
if (hundreds_counter >= 10) {
hundreds_counter = 0;
seconds_counter++;
if (total_us100_counter % 1000 == 0) {
// 每100ms执行的代码可以放在这里
}
if (seconds_counter >= 10) {
seconds_counter = 0;
if (total_us100_counter % 10000 == 0) {
// 每1s执行的代码可以放在这里
}
}
// 重置计数器避免溢出
total_us100_counter = 0;
}
}
}
Robot_V2/device/test.cpp
+38
View File
@@ -0,0 +1,38 @@
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "core_cm4.h"
#include "device/tle5012b.h"
#include "initial.h"
#include "main.h"
#include "sys/initial.h"
#include "systick.h"
#include "tle5012b.h"
#include <cinttypes>
#include <cstdio>
constexpr int status_print = 0;
void test_1111() {
// 初始化CAN回环测试
// can_loopback_init();
// can_config();
if (status_print) {
/* value print */
printf(" the temperature data is %2.2f degrees Celsius\r\n",
get_mcu_temp());
printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
get_drv_temp());
printf(" the U current data is %2.2f mA\r\n", get_current('U'));
printf(" the V current data is %2.2f mA\r\n", get_current('V'));
printf(" the W current data is %2.2f mA\r\n", get_current('W'));
printf(" the VBUS is %2.2f V\r\n", get_vbus_voltage());
// printf(" the angle is %2.2f\r\n", tle5012b_read());
printf("\n----------------------------------------------------\n");
// TLI5012B测试
Test_TLI5012B();
}
// can_loopback_test();
}
Robot_V2/device/tle5012b.cpp
+112 -87
View File
@@ -1,6 +1,9 @@
#include "tle5012b.h"
#include "core_cmInstr.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "stdbool.h"
#include "systick.h"
#define SPI_TX_OFF() \
do { \
@@ -69,8 +72,6 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
/* ---------- Send data ---------- */
to = 200;
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to);
if (to == 0) ok = false;
@@ -89,7 +90,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
data = spi_i2s_data_receive(spi_periph); /* throw Dummy */
}
/* ---------- wait bus idle ---------- */
/* ---------- wait bsu idle ---------- */
if (ok)
{
to = 200;
@@ -100,6 +101,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
return ok ? data : 0xFFFF;
}
/**
* @brief Write data to TLI5012B register
* @param i_Cmd Command word to send
@@ -111,12 +113,16 @@ uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data)
{
uint16_t result = 0xFFFF; /* Default error code */
SPI_CS_ENABLE();
SPI_TX_ON();
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
result = spi_rw_half_word(SPI0, i_Cmd);
result = spi_rw_half_word(SPI0, i_Data);
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
// result = spi_rw_half_word(SPI0, result);
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
result = spi_rw_half_word(SPI0, DUMMY_BYTE);
SPI_CS_DISABLE();
SPI_TX_ON(); /* Restore MOSI as output */
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
return result;
}
@@ -131,12 +137,16 @@ uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data)
{
uint16_t result = 0xFFFF; /* Default error code */
SPI_CS_ENABLE();
SPI_TX_ON();
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
*i_Data = spi_rw_half_word(SPI0, i_Cmd);
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
// *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);
result = spi_rw_half_word(SPI0, result);
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
*i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);
result = spi_rw_half_word(SPI0, DUMMY_BYTE);
SPI_CS_DISABLE();
SPI_TX_ON(); /* Restore MOSI as output */
__NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
return result;
}
@@ -146,16 +156,17 @@ void tle5012b_gpio_init(void)
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_SPI0);
rcu_periph_clock_enable(RCU_AF);
/* JTAG-DP disabled and SW-DP enabled, so SPI0 can use PB3 and PB4 */
/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
/* SPI0 GPIO config: NSS/PA15, SCK/PB3, MOSI/PB5 */
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
/* PA15 → NSS, 推挽输出 */
gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5);
/* SPI0 GPIO config: MISO/PB4 */
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4);
/* PB4 → MISO, 浮空输入 */
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4 | GPIO_PIN_10);
}
/**
@@ -168,15 +179,15 @@ void tle5012b_spi_init(void)
spi_parameter_struct spi_init_str;
spi_struct_para_init(&spi_init_str);
/* SPI0 parameter config */
/* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */
spi_init_str.device_mode = SPI_MASTER;
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
spi_init_str.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;
spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
spi_init_str.nss = SPI_NSS_SOFT;
spi_init_str.endian = SPI_ENDIAN_MSB;
spi_init_str.prescale = SPI_PSC_128;
spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */
spi_init(SPI0, &spi_init_str);
spi_nss_output_enable(SPI0);
spi_nssp_mode_enable(SPI0);
@@ -185,7 +196,6 @@ void tle5012b_spi_init(void)
/**
* @brief Initialize TLI5012B magnetic angle sensor
* @note This function configures the sensor registers for proper operation
@@ -201,114 +211,129 @@ void cdd_TLI5012_Init(void)
/* Read STAT register to get current status */
u16Command = READ_STAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write STAT register, set Slave_Number = 00 */
u16Command = 0x0001;
u16Data &= 0x9FFF; /* Clear bits 13-14 (Slave_Number) */
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Read STAT register again to verify write operation */
u16Command = READ_STAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 2: Read ACSTAT register ---------- */
u16Command = READ_ACSTAT_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 3: Configure MOD1 register ---------- */
/* Read current MOD1 register value */
u16Command = RD_REG | WRITE_MOD1_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD1 register with configuration value */
// u16Command = WRITE_MOD1_VALUE;
// u16Data = MOD1_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD1 register with configuration value */
u16Command = WRITE_MOD1_VALUE;
u16Data = MOD1_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD1 register write operation */
// u16Command = RD_REG | WRITE_MOD1_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD1 register write operation */
u16Command = RD_REG | WRITE_MOD1_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 4: Configure MOD2 register ---------- */
// /* Read current MOD2 register value */
// u16Command = RD_REG | WRITE_MOD2_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 4: Configure MOD2 register ---------- */
/* Read current MOD2 register value */
u16Command = RD_REG | WRITE_MOD2_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD2 register with configuration value */
// u16Command = WRITE_MOD2_VALUE;
// u16Data = MOD2_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD2 register with configuration value */
u16Command = WRITE_MOD2_VALUE;
u16Data = MOD2_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD2 register write operation */
// u16Command = RD_REG | WRITE_MOD2_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD2 register write operation */
u16Command = RD_REG | WRITE_MOD2_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 5: Configure MOD3 register ---------- */
// /* Read current MOD3 register value */
// u16Command = RD_REG | WRITE_MOD3_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* ---------- Step 5: Configure MOD3 register ---------- */
/* Read current MOD3 register value */
u16Command = RD_REG | WRITE_MOD3_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD3 register with configuration value */
// u16Command = WRITE_MOD3_VALUE;
// u16Data = MOD3_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD3 register with configuration value */
u16Command = WRITE_MOD3_VALUE;
u16Data = MOD3_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD3 register write operation */
// u16Command = RD_REG | WRITE_MOD3_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD3 register write operation */
u16Command = RD_REG | WRITE_MOD3_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Optional: Configure MOD3 register with SSC_OD = 1 (commented out) */
/* ---------- Step 6: Configure MOD4 register ---------- */
/* Read current MOD4 register value */
u16Command = RD_REG | WRITE_MOD4_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// u16Command = 0x5091;
// u16Data |= 0x0004; // Set SSC_OD bit
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Write MOD4 register with configuration value */
u16Command = WRITE_MOD4_VALUE;
u16Data = MOD4_VALUE;
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
printf("\r\n @Joe [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* ---------- Step 6: Configure MOD4 register ---------- */
// /* Read current MOD4 register value */
// u16Command = RD_REG | WRITE_MOD4_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Write MOD4 register with configuration value */
// u16Command = WRITE_MOD4_VALUE;
// u16Data = MOD4_VALUE;
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
// printf("\r\n [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
// /* Verify MOD4 register write operation */
// u16Command = RD_REG | WRITE_MOD4_VALUE;
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
/* Verify MOD4 register write operation */
u16Command = RD_REG | WRITE_MOD4_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
}
void Test_TLI5012B(void)
{
uint16_t u16Command = READ_ANGLE_VALUE;
uint16_t u16Command = 0xFFFF;
uint16_t u16Data = 0x0000;
uint16_t u16Safe = 0x0000;
float angle = 0;
u16Command = READ_ANGLE_VALUE;
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
// 输出原始读取的数据，用于诊断
printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// 检查是否有错误标志位
if (u16Safe != 0xFFFF && u16Safe != 0x0000) {
printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n");
}
// 检查错误位（bit 15）
if (u16Data & 0x8000) {
printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n");
}
// 提取角度值(15位)
u16Data = u16Data & 0x7FFF;
printf ( "\r\n AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// angle = 360*( ((int16_t)(u16Data << 1)) >> 1)/32768;
angle = ( 360*( ( (int16_t)(u16Data << 1) ) >> 1 ) ) >> 15;
printf ( "\r\n angle = %f, AVAL = %d \r\n", angle, (((int16_t)(u16Data << 1)) >> 1));
printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
// 将15位角度值转换为角度(0-360度)
// 0x7FFF = 32767 对应 360度
angle = ((float)u16Data * 360.0f) / 32767.0f;
printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data);
// 添加额外的传感器状态检查
uint16_t status_data, status_safe;
u16Command = READ_STAT_VALUE;
status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data);
printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe);
}
Robot_V2/main.cpp
+5 -24
View File
@@ -3,11 +3,11 @@
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "core_cm4.h"
#include "device/tle5012b.h"
#include "initial.h"
#include "sys/initial.h"
#include "systick.h"
#include "core_cm4.h"
#include "tle5012b.h"
#include <cinttypes>
#include <cstdio>
@@ -15,9 +15,7 @@
// 全局变量定义
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CircularBuffer buffer;
CAN_transmiter CAN0_transmiter;
int main(void) {
systick_config(); // 系统时钟初始化
USART0_init(); // 调试串口初始化
@@ -27,32 +25,15 @@ int main(void) {
tle5012b_gpio_init(); // TLI5012B初始化
tle5012b_spi_init();
cdd_TLI5012_Init(); // 初始化TLE5012B传感器
// cdd_TLI5012_Init(); // 初始化TLE5012B传感器
watchdog_init(1000);
// 初始化CAN回环测试
// can_loopback_init();
watchdog_init(5000);
while (1) {
/* clear the FWDGT reset flag */
fwdgt_counter_reload();
// can_loopback_test();
/* value print */
// printf(" the temperature data is %2.2f degrees Celsius\r\n",
// get_mcu_temp());
// printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
// get_drv_temp());
// printf(" the U current data is %2.2f mA\r\n",get_current('U'));
// printf(" the V current data is %2.2f mA\r\n",get_current('V'));
// printf(" the W current data is %2.2f mA\r\n",get_current('W'));
// printf(" the VBUS is %2.2f V\r\n",get_vbus_voltage());
// printf(" the angle is %2.2f\r\n", tle5012b_read());
printf("\n----------------------------------------------------\n");
delay_1ms(700);
// TLI5012B测试
Test_TLI5012B();
CAN0_transmiter.communication_check();
// delay_1ms(2);
Set_Pwm(0, 1500);
}
Robot_V2/sys/systick.cpp
+14 -2
View File
@@ -56,7 +56,19 @@ void systick_config(void)
/* configure the systick handler priority */
NVIC_SetPriority(SysTick_IRQn, 0x00U);
}
/*!
\brief delay a time in seconds
\param[in] count: count in seconds
\param[out] none
\retval none
*/
void delay_1s(uint32_t count)
{
uint32_t i;
for(i = 0; i < count; i++){
delay_1ms(1000);
}
}
/*!
\brief delay a time in milliseconds
\param[in] count: count in milliseconds
@@ -66,8 +78,8 @@ void systick_config(void)
void delay_1ms(uint32_t count)
{
delay = count;
while(0U != delay){
fwdgt_counter_reload();
}
}
Robot_V2/sys/systick.h
+2
View File
@@ -44,6 +44,8 @@ extern "C" {
void systick_config(void);
/* delay a time in milliseconds */
void delay_1ms(uint32_t count);
/* delay a time in seconds */
void delay_1s(uint32_t count);
/* delay decrement */
void delay_decrement(void);