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

feat(FOC): 优化开环速度控制逻辑并提升性能

Browse Source 
refactor(ADC): 重构采样逻辑并封装为 sampler 类
refactor(CAN): 更新 CAN 发送器以使用新的sampler 接口
refactor(timer): 调整定时器中断结构并优化任务调度
This commit is contained in:
cbb
2025-10-19 23:20:28 +08:00
parent 34df732659
commit 7f0ca1857b
8 changed files with 309 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+33 -12
View File
@@ -8,13 +8,15 @@
constexpr double PI = 3.1415926535897932384626433832795f; constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float _3PI_2 = 4.71238898038f; constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11, DIR = 1; constexpr u8 PP = 11, DIR = 1;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
// 初始变量及函数定义 // 初始变量及函数定义
#define _constrain(amt, low, high) \ #define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define constrain(value, min, max, target_min, target_max) \ #define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min)) ((value - min) / (max - min) * (target_max - target_min))
float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
/** /**
* @brief 归一化角度 * @brief 归一化角度
@@ -22,7 +24,7 @@ float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
* @param angle * @param angle
* @return float * @return float
*/ */
float _normalizeAngle(float angle) { inline float _normalizeAngle(float angle) {
float a = fmod(angle, 2 * PI); float a = fmod(angle, 2 * PI);
// 取余运算可以用于归一化，列出特殊值例子算便知 // 取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2 * PI); return a >= 0 ? a : (a + 2 * PI);
@@ -78,18 +80,37 @@ void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
* @return float: q轴电压 * @return float: q轴电压
*/ */
float FOC_Controlroller::velocity_open_loop(float target_velocity) { float FOC_Controlroller::velocity_open_loop(float target_velocity) {
now_us = get_system_runtime_us();
float dt = (now_us - last_time_us) * 1e-6f;
// 限制时间间隔
if (dt <= 0.0f || dt > 0.5f)
dt = 1e-3f;
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * dt); // 预计算常量 1e-4 = 100微秒控制周期 / 1秒 = 100e-6
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
// 直接计算电气角度，避免函数调用开销
float angle_el = shaft_angle * PP + zero_electric_angle;
angle_el = _normalizeAngle(angle_el);
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = limit_voltage;
const float Ud = 0.0f;
// 帕克逆变换 (直接计算，避免函数调用开销)
const float sin_el = sin(angle_el);
const float cos_el = cos(angle_el);
Ualpha = -Uq * sin_el;
Ubeta = Uq * cos_el;
float uq = limit_voltage; // 设定一个固定的q轴电压 // 克拉克逆变换 (预计算常量值)
setPhaseVoltage(uq, 0, _electricalAngle(shaft_angle)); const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
last_time_us = now_us; Ua = Ualpha + v_half;
return uq; Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
// 计算占空比
const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
Set_3Pwm(dc_a, dc_b, dc_c);
return limit_voltage;
} }
/** /**
Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h
+50
View File
@@ -0,0 +1,50 @@
/*
* CSOLUTION generated file: DO NOT EDIT!
* Generated by: csolution version 2.11.0
*
* Project: 'Robot_V2.Release+GD32C103CB'
* Target: 'Release+GD32C103CB'
*/
#ifndef RTE_COMPONENTS_H
#define RTE_COMPONENTS_H
/*
* Define the Device Header File:
*/
#define CMSIS_device_header "gd32c10x.h"
/* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_ADC
/* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_CAN
/* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_DBG
/* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_DMA
/* GigaDevice::Device:GD32C10x_StdPeripherals:EXTI@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_EXTI
/* GigaDevice::Device:GD32C10x_StdPeripherals:FMC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_FMC
/* GigaDevice::Device:GD32C10x_StdPeripherals:FWDGT@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_FWDGT
/* GigaDevice::Device:GD32C10x_StdPeripherals:GPIO@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_GPIO
/* GigaDevice::Device:GD32C10x_StdPeripherals:MISC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_MISC
/* GigaDevice::Device:GD32C10x_StdPeripherals:PMU@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_PMU
/* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_RCU
/* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_SPI
/* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_TIMER
/* GigaDevice::Device:GD32C10x_StdPeripherals:USART@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_USART
/* GigaDevice::Device:GD32C10x_StdPeripherals:WWDGT@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_WWDGT
#endif /* RTE_COMPONENTS_H */
Robot_V2/device/ADC.cpp
+88 -46
View File
@@ -1,39 +1,55 @@
#include "adc.h" #include "adc.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <cstdio> #include <cstdio>
#include <stdint.h> #include <stdint.h>
#include "math.h"
constexpr float ERROR_ADC_VAL = 255.0f; constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f; constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f; constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f; constexpr float VCC_FACTOR = 11.0f;
constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f * 1000.0f; constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f; constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f; constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 50.0f; constexpr float MAX_CURRENT = 65.0f;
constexpr float MIN_CURRENT = -0.1f; constexpr float MIN_CURRENT = -0.1f;
constexpr float MAX_VCC = 65.0f; constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f; constexpr float MIN_VCC = -0.1f;
u16 adc_value[7] = {0}; // 显式初始化数组元素为0
u16 adc_value[7] = {0}; // 显式初始化数组元素为0 /**
* @brief 采样类初始化
*/
void sampler::init() {
for (int i = 0; i < 7; i++) {
mcu_temp_samples[i] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
currU_samples[i] = adc_value[2] * ADC_SCALE * CURRENT_FACTOR;
currV_samples[i] = adc_value[3] * ADC_SCALE * CURRENT_FACTOR;
currW_samples[i] = adc_value[4] * ADC_SCALE * CURRENT_FACTOR;
vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
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;
}
}
/** /**
* @brief 获取MCU温度 * @brief 获取MCU温度
* @return float * @return float
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/ */
float get_mcu_temp() { float sampler::get_mcu_temp() {
float temp_latest = ERROR_ADC_VAL; tempora_sample = medium_filter(mcu_temp_samples, 7);
// 迭代采样5次，直到获得有效温度值 // 检查温度读数是否在有效范围内
for (int i = 0; i < 5; i++) { if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
temp_latest = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; return ERROR_ADC_VAL;
if (temp_latest < MIN_TEMP || temp_latest > MAX_TEMP)
return ERROR_ADC_VAL;
} }
return temp_latest; return tempora_sample;
} }
/** /**
* @brief 获取电机驱动温度 * @brief 获取电机驱动温度
@@ -42,19 +58,13 @@ float get_mcu_temp() {
* 通过热敏电阻分压电路计算温度值，使用Steinhart-Hart方程进行温度转换。 * 通过热敏电阻分压电路计算温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。 * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/ */
float get_drv_temp() { float sampler::get_drv_temp() {
float temp_latest = ERROR_ADC_VAL; tempora_sample = medium_filter(drv_temp_samples, 7);
for (int i = 0; i < 5; i++) { // 检查温度读数是否在有效范围内
float ana_val = adc_value[1]; if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
float R_now = ana_val / (4096 - ana_val) * 4700; return ERROR_ADC_VAL;
temp_latest =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
if (temp_latest < MIN_TEMP || temp_latest > MAX_TEMP) {
return ERROR_ADC_VAL;
}
} }
return temp_latest; return tempora_sample;
} }
/** /**
@@ -64,31 +74,28 @@ float get_drv_temp() {
* *
* 通过ADC采样计算各相电流值 * 通过ADC采样计算各相电流值
*/ */
float get_current(char type) { float sampler::get_current(char type) {
float current_latest = ERROR_ADC_VAL;
switch (type) { switch (type) {
case 'U': { case 'U': {
current_latest = adc_value[2] * ADC_SCALE * CURRENT_FACTOR; tempora_sample = medium_filter(currU_samples, 7);
break; break;
} }
case 'V': { case 'V': {
current_latest = adc_value[3] * ADC_SCALE * CURRENT_FACTOR; tempora_sample = medium_filter(currV_samples, 7);
break; break;
} }
case 'W': { case 'W': {
current_latest = adc_value[4] * ADC_SCALE * CURRENT_FACTOR; tempora_sample = medium_filter(currW_samples, 7);
break; break;
} }
default: { default: {
return ERROR_ADC_VAL; return ERROR_ADC_VAL;
} }
} }
if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
// 检查电流读数是否在有效范围内
if (current_latest < MIN_CURRENT || current_latest > MAX_CURRENT) {
return ERROR_ADC_VAL; return ERROR_ADC_VAL;
} }
return current_latest; return tempora_sample;
} }
/** /**
@@ -98,16 +105,51 @@ float get_current(char type) {
* 通过ADC采样计算供电电压值，使用迭代方式确保获取有效的电压读数。 * 通过ADC采样计算供电电压值，使用迭代方式确保获取有效的电压读数。
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。 * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/ */
float get_vbus_voltage() { float sampler::get_vbus_voltage() {
float vcc_latest = ERROR_ADC_VAL; // 检查电压读数是否在有效范围内
for (int i = 0; i < 5; i++) { if (vbus_samples[4] > MAX_VCC || vbus_samples[4] < MIN_VCC) {
// 获取电压值 return ERROR_ADC_VAL;
vcc_latest = adc_value[5] * ADC_SCALE * VCC_FACTOR;
// 检查电压读数是否在有效范围内
if (vcc_latest > MAX_VCC || vcc_latest < MIN_VCC) {
return ERROR_ADC_VAL;
}
} }
return vcc_latest; return vbus_samples[4];
} }
/**
* @brief 更新MCU温度
*
* 通过ADC采样计算MCU温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
void sampler::update_mcu_temp() {
memmove(mcu_temp_samples, mcu_temp_samples + 1, 6 * sizeof(float));
mcu_temp_samples[6] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}
/**
* @brief 更新MCU温度
*/
void sampler::update_drv_temp() {
memmove(drv_temp_samples, drv_temp_samples + 1, 6 * sizeof(float));
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[6] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
/**
* @brief 更新相电流
*/
void sampler::update_current() {
memmove(currU_samples, currU_samples + 1, 6 * sizeof(float));
memmove(currV_samples, currV_samples + 1, 6 * sizeof(float));
memmove(currW_samples, currW_samples + 1, 6 * sizeof(float));
currU_samples[6] = adc_value[2] * ADC_SCALE;
currV_samples[6] = adc_value[3] * ADC_SCALE;
currW_samples[6] = adc_value[4] * ADC_SCALE;
}
/**
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
memmove(vbus_samples, vbus_samples + 1, 6 * sizeof(float));
vbus_samples[6] = adc_value[5] * ADC_SCALE;
}
Robot_V2/device/Can.cpp
+7 -6
View File
@@ -19,6 +19,7 @@
extern CAN_transmiter CAN0_transmiter; extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC; extern FOC_Controlroller FOC;
extern sampler sampler;
/*! /*!
\brief initialize CAN GPIO \brief initialize CAN GPIO
@@ -363,9 +364,9 @@ CAN_transmiter::CAN_transmiter() {
void CAN_transmiter::update(void) { void CAN_transmiter::update(void) {
limit_velocity_union.f = FOC.limit_velocity; limit_velocity_union.f = FOC.limit_velocity;
limit_voltage = FOC.limit_voltage * 1000; limit_voltage = FOC.limit_voltage * 1000;
mcu_temp = get_mcu_temp() * 100; mcu_temp = sampler.get_mcu_temp() * 100;
motor_temp = get_drv_temp() * 100; motor_temp = sampler.get_drv_temp() * 100;
Power_Voltage = get_vbus_voltage() * 1000; Power_Voltage = sampler.get_vbus_voltage() * 1000;
rt_angle_union.f = 0.0f; rt_angle_union.f = 0.0f;
rt_velocity_union.f = 0.0f; rt_velocity_union.f = 0.0f;
Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1; Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
@@ -375,9 +376,9 @@ void CAN_transmiter::update(void) {
target_angle_union.f = FOC.target_angle; target_angle_union.f = FOC.target_angle;
target_velocity_union.f = FOC.target_velocity; target_velocity_union.f = FOC.target_velocity;
u_current_union.f = get_current('U') * 1000; u_current_union.f = sampler.get_current('U') * 1000;
w_current_union.f = get_current('W') * 1000; w_current_union.f = sampler.get_current('W') * 1000;
v_current_union.f = get_current('V') * 1000; v_current_union.f = sampler.get_current('V') * 1000;
} }
/*! /*!
Robot_V2/device/Timer.cpp
+97 -74
View File
@@ -1,16 +1,16 @@
#include "Timer.h" #include "Timer.h"
#include "Can.h" #include "Can.h"
#include "DengFOC.h"
#include "Timer.h"
#include "adc.h" #include "adc.h"
#include "gd32c10x.h" #include "gd32c10x.h"
#include "gd32c10x_gpio.h" #include "gd32c10x_gpio.h"
#include "gd32c10x_rcu.h" #include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h" #include "gd32c10x_timer.h"
#include "DengFOC.h"
#include <cstdio> #include <cstdio>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include "Timer.h"
// 配置定时器每100微秒中断一次 // 配置定时器每100微秒中断一次
// 系统时钟为120MHz，要得到100微秒的中断周期 // 系统时钟为120MHz，要得到100微秒的中断周期
@@ -19,8 +19,8 @@
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 2999; // 120MHz / (0+1) / (5999+1) = 20kHz constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_CH0_PULSE = 0; constexpr int TIMER1_CH0_PULSE = 0;
constexpr int TIMER1_CH1_PULSE = 0; constexpr int TIMER1_CH1_PULSE = 0;
constexpr int TIMER1_CH2_PULSE = 0; constexpr int TIMER1_CH2_PULSE = 0;
@@ -33,6 +33,8 @@ static volatile std::u64 system_runtime_us = 0;
// 声明外部FOC控制器对象 // 声明外部FOC控制器对象
extern FOC_Controlroller FOC; extern FOC_Controlroller FOC;
// 声明外部采样器对象
extern sampler sampler;
/** /**
* @brief 设置引脚中断优先级 * @brief 设置引脚中断优先级
@@ -99,17 +101,13 @@ void User_timer_init(void) {
nvic_config(); nvic_config();
} }
/*! /*!
\brief 获取系统运行时间(毫秒) \brief 获取系统运行时间(毫秒)
\param[in] none \param[in] none
\param[out] none \param[out] none
\retval 系统运行时间，单位为毫秒 \retval 系统运行时间，单位为毫秒
*/ */
uint64_t get_system_runtime_us(void) uint64_t get_system_runtime_us(void) { return system_runtime_us; }
{
return system_runtime_us;
}
/** /**
* @brief TIMER2中断处理函数 * @brief TIMER2中断处理函数
@@ -120,30 +118,51 @@ void TIMER2_IRQHandler(void) {
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) { if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志 // 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 增加总计数器
// 增加总计数器 (使用更高效的位运算代替取模运算)
total_us100_counter++; total_us100_counter++;
system_runtime_us += 100; system_runtime_us += 100;
// 每100us执行一次的操作可以放在这里
// 在定时器中断中执行FOC控制，确保控制频率稳定 (最高频操作放在最前)
// 在定时器中断中执行FOC控制，确保控制频率稳定
FOC.velocity_open_loop(FOC.target_velocity); FOC.velocity_open_loop(FOC.target_velocity);
// 每100us执行一次的操作
sampler.update_current();
if (total_us100_counter % 10 == 0) { // 每1ms执行的代码 (100us * 10 = 1ms)
// 每1ms执行的代码可以放在这里 if (total_us100_counter >= 10) {
CAN0_transmiter.can0_send_flag = SET; total_us100_counter = 0; // 重置计数器
if (total_us100_counter % 100 == 0) {
// 每10ms执行的代码可以放在这里
}
if (total_us100_counter % 1000 == 0) { // 设置CAN发送标志
// 每100ms执行的代码可以放在这里 CAN0_transmiter.can0_send_flag = SET;
}
// 更新各种传感器数据
sampler.update_mcu_temp();
sampler.update_drv_temp();
sampler.update_vbus_voltage();
if (total_us100_counter % 10000 == 0) { // 每10ms执行的代码 (1ms * 10 = 10ms)
// 每1s执行的代码可以放在这里 static uint8_t ms10_counter = 0;
ms10_counter++;
// 重置计数器避免溢出 if (ms10_counter >= 10) {
total_us100_counter = 0; ms10_counter = 0;
// 可以添加每10ms执行的代码
// 每100ms执行的代码 (10ms * 10 = 100ms)
static uint8_t ms100_counter = 0;
ms100_counter++;
if (ms100_counter >= 10) {
ms100_counter = 0;
// 可以添加每100ms执行的代码
// 每1s执行的代码 (100ms * 10 = 1s)
static uint8_t sec_counter = 0;
sec_counter++;
if (sec_counter >= 10) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
}
} }
} }
} }
@@ -164,7 +183,8 @@ void timer1_gpio_config(void) {
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1); gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2); gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0|GPIO_PIN_12); gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
GPIO_PIN_0 | GPIO_PIN_12);
gpio_bit_set(GPIOB, GPIO_PIN_0); gpio_bit_set(GPIOB, GPIO_PIN_0);
gpio_bit_set(GPIOB, GPIO_PIN_12); gpio_bit_set(GPIOB, GPIO_PIN_12);
@@ -176,52 +196,53 @@ void timer1_gpio_config(void) {
* @brief TIMER1 配置函数 * @brief TIMER1 配置函数
*/ */
void timer1_pwm_config(void) { void timer1_pwm_config(void) {
timer_parameter_struct timer_initpara; timer_parameter_struct timer_initpara;
timer_oc_parameter_struct timer_ocinitpara; timer_oc_parameter_struct timer_ocinitpara;
// 使能TIMER1时钟 // 使能TIMER1时钟
rcu_periph_clock_enable(RCU_TIMER1); rcu_periph_clock_enable(RCU_TIMER1);
// 复位TIMER1 // 复位TIMER1
timer_deinit(TIMER1); timer_deinit(TIMER1);
// 配置TIMER1基本参数 // 配置TIMER1基本参数
timer_struct_para_init(&timer_initpara); timer_struct_para_init(&timer_initpara);
timer_initpara.prescaler = TIMER1_PRESCALER; // 预分频器 timer_initpara.prescaler = TIMER1_PRESCALER; // 预分频器
timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数 timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数
timer_initpara.period = TIMER1_PERIOD; // 计数周期 timer_initpara.period = TIMER1_PERIOD; // 计数周期
timer_initpara.clockdivision = TIMER_CKDIV_DIV1; timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
timer_init(TIMER1, &timer_initpara); timer_init(TIMER1, &timer_initpara);
// 配置通道输出参数 // 配置通道输出参数
timer_channel_output_struct_para_init(&timer_ocinitpara); timer_channel_output_struct_para_init(&timer_ocinitpara);
timer_ocinitpara.outputstate = TIMER_CCX_ENABLE; timer_ocinitpara.outputstate = TIMER_CCX_ENABLE;
timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH; timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
// 配置三个通道为PWM模式 // 配置三个通道为PWM模式
timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocinitpara); timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocinitpara);
timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara); timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara);
timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara); timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara);
// 设置PWM模式和占空比 // 设置PWM模式和占空比
timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0); timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0);
timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0); timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0);
timer_channel_output_mode_config(TIMER1, TIMER_CH_2, TIMER_OC_MODE_PWM0); timer_channel_output_mode_config(TIMER1, TIMER_CH_2, TIMER_OC_MODE_PWM0);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, TIMER1_CH0_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, TIMER1_CH1_PULSE); TIMER1_CH0_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2, TIMER1_CH2_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
TIMER1_CH1_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
TIMER1_CH2_PULSE);
// 使能主输出和自动重载 // 使能主输出和自动重载
timer_primary_output_config(TIMER1, ENABLE); timer_primary_output_config(TIMER1, ENABLE);
timer_auto_reload_shadow_enable(TIMER1); timer_auto_reload_shadow_enable(TIMER1);
// 启动TIMER1 // 启动TIMER1
timer_enable(TIMER1); timer_enable(TIMER1);
} }
/*! /*!
\brief set PWM \brief set PWM
\param[in] channel: TIMER_CH_0/TIMER_CH_1/TIMER_CH_2/TIMER_CH_3 (valid \param[in] channel: TIMER_CH_0/TIMER_CH_1/TIMER_CH_2/TIMER_CH_3 (valid
@@ -233,14 +254,13 @@ void timer1_pwm_config(void) {
*/ */
void Set_Pwm(u16 channel, float duty_cycle) { void Set_Pwm(u16 channel, float duty_cycle) {
float pulse = duty_cycle * TIMER1_PERIOD; float pulse = duty_cycle * TIMER1_PERIOD;
if (channel <= 3) { if (channel <= 3) {
timer_channel_output_pulse_value_config(TIMER1, channel, pulse); timer_channel_output_pulse_value_config(TIMER1, channel, pulse);
} else { } else {
} // Ensure channel is within valid range } // Ensure channel is within valid range
} }
/*! /*!
\brief set PWM \brief set PWM
\param[in] channel: TIMER_CH_0/TIMER_CH_1/TIMER_CH_2/TIMER_CH_3 (valid \param[in] channel: TIMER_CH_0/TIMER_CH_1/TIMER_CH_2/TIMER_CH_3 (valid
@@ -250,8 +270,11 @@ void Set_Pwm(u16 channel, float duty_cycle) {
\retval none \retval none
\note The channel parameter must be between 0 and 3 inclusive. \note The channel parameter must be between 0 and 3 inclusive.
*/ */
void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2) { void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, duty_cycle0 * TIMER1_PERIOD); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, duty_cycle1 * TIMER1_PERIOD); duty_cycle0 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2, duty_cycle2 * TIMER1_PERIOD); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
duty_cycle1 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
duty_cycle2 * TIMER1_PERIOD);
} }
Robot_V2/device/adc.h
+23 -5
View File
@@ -7,14 +7,32 @@ extern "C" {
#include "stdint.h" #include "stdint.h"
float get_mcu_temp(void); class sampler {
float get_drv_temp(void); private:
float get_vbus_voltage(void); float vbus_samples[7];
float get_current(char); float currU_samples[7];
float currV_samples[7];
float currW_samples[7];
float mcu_temp_samples[7];
float drv_temp_samples[7];
float tempora_sample;
public:
void init();
void update_mcu_temp(void);
void update_drv_temp(void);
void update_vbus_voltage(void);
void update_current(void);
float get_mcu_temp(void);
float get_drv_temp(void);
float get_vbus_voltage(void);
float get_current(char);
};
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
#endif #endif
Robot_V2/device/test.cpp
+7 -7
View File
@@ -13,7 +13,7 @@
#include <cstdio> #include <cstdio>
constexpr int status_print = 0; constexpr int status_print = 0;
extern sampler sampler;
void test_1111() { void test_1111() {
// 初始化CAN回环测试 // 初始化CAN回环测试
// can_loopback_init(); // can_loopback_init();
@@ -21,13 +21,13 @@ void test_1111() {
if (status_print) { if (status_print) {
/* value print */ /* value print */
printf(" the temperature data is %2.2f degrees Celsius\r\n", printf(" the temperature data is %2.2f degrees Celsius\r\n",
get_mcu_temp()); sampler.get_mcu_temp());
printf(" the drv_temperature data is %2.2f degrees Celsius\r\n", printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
get_drv_temp()); sampler.get_drv_temp());
printf(" the U current data is %2.2f mA\r\n", get_current('U')); printf(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
printf(" the V current data is %2.2f mA\r\n", get_current('V')); printf(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
printf(" the W current data is %2.2f mA\r\n", get_current('W')); printf(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
printf(" the VBUS is %2.2f V\r\n", get_vbus_voltage()); printf(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
// printf(" the angle is %2.2f\r\n", tle5012b_read()); // printf(" the angle is %2.2f\r\n", tle5012b_read());
printf("\n----------------------------------------------------\n"); printf("\n----------------------------------------------------\n");
Robot_V2/main.cpp
+4 -1
View File
@@ -18,6 +18,7 @@
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CAN_transmiter CAN0_transmiter; CAN_transmiter CAN0_transmiter;
FOC_Controlroller FOC; FOC_Controlroller FOC;
sampler sampler;
int main(void) { int main(void) {
systick_config(); // 系统时钟初始化 systick_config(); // 系统时钟初始化
@@ -25,16 +26,18 @@ int main(void) {
User_timer_init(); // Timer初始化 User_timer_init(); // Timer初始化
adc_config(); // ADC初始化 adc_config(); // ADC初始化
dma_config(); // DMA采样ADC初始化 dma_config(); // DMA采样ADC初始化
sampler.init(); // 采样器初始化
driver_init(); // 电机驱动初始化 driver_init(); // 电机驱动初始化
tle5012b_gpio_init(); // TLI5012B初始化 tle5012b_gpio_init(); // TLI5012B初始化
tle5012b_spi_init(); tle5012b_spi_init();
watchdog_init(5000); watchdog_init(5000);
while (1) { while (1) {
/* clear the FWDGT reset flag */ /* clear the FWDGT reset flag */
// std::printf("1\n");
fwdgt_counter_reload(); fwdgt_counter_reload();
printf("1\r\n");
CAN0_transmiter.communication_check(); CAN0_transmiter.communication_check();
} }
} }