btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat 更新角度闭环相关功能

Browse Source
This commit is contained in:
井明江
2024-01-16 20:03:21 +08:00
parent 16ead48bba
commit 3bc5e3cf93
19 changed files with 908 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
app/by_buzzer.c
View File

@@ -29,14 +29,14 @@ void queue_pop_element(void)
void queue_pop_read(void)
{
while (queue_long != 0) {
pwm_init(TIM8_PWM_MAP0_CH1_C6, a[0], 5000);
pwm_init(BUZZER_PIN, a[0], 5000);
queue_pop_element();
system_delay_ms(100);
pwm_set_duty(TIM8_PWM_MAP0_CH1_C6, 0);
pwm_set_duty(BUZZER_PIN, 0);
}
}
void by_buzzer_init(void)
{
pwm_init(TIM8_PWM_MAP0_CH1_C6, 2000, 0);
pwm_init(BUZZER_PIN, 2000, 0);
}

2
app/by_buzzer.h
View File

@@ -10,7 +10,7 @@
#define BY_PRESS_LONG 2500
#define BY_FORWARD 1500
#define BY_BACKWARD 1800
#define BUZZER_PIN TIM3_PWM_MAP0_CH2_A7
extern void by_buzzer_init(void);
extern void queue_init(void);
extern void queue_add_element(int element);

60
app/by_fan_control.c
View File

@@ -1,28 +1,58 @@
#include "by_fan_control.h"
#include "zf_common_headfile.h"
#define Fan_pwm_up1 TIM8_PWM_MAP0_CH1_C6
#define Fan_pwm_up2 TIM8_PWM_MAP0_CH2_C7
#define Fan_pwm_power1 TIM4_PWM_MAP1_CH3_D14
#define Fan_pwm_power2 TIM4_PWM_MAP1_CH4_D15
#define Fan_pwm_power3 TIM4_PWM_MAP1_CH2_D13
#define Fan_pwm_power4 TIM4_PWM_MAP1_CH1_D12
uint32_t myclip(uint32_t x, uint32_t low, uint32_t up)
{
return (x > up ? up : x < low ? low
: x);
}
void by_pwm_init(void)
{
pwm_init(TIM4_PWM_MAP1_CH1_D12, 10000, 0); // 浮力风扇左
pwm_init(TIM4_PWM_MAP1_CH2_D13, 10000, 0); // 浮力风扇右
pwm_init(Fan_pwm_up1, 50, 500); // 浮力风扇左
pwm_init(Fan_pwm_up2, 50, 500); // 浮力风扇右
pwm_init(TIM4_PWM_MAP1_CH3_D14, 10000, 0); // 动力风扇左
pwm_init(TIM4_PWM_MAP1_CH4_D15, 10000, 0); // 动力风扇右
pwm_init(Fan_pwm_power1, 50, 500); // 动力风扇左1
pwm_init(Fan_pwm_power2, 50, 500); // 动力风扇右1
pwm_init(Fan_pwm_power3, 50, 500); // 动力风扇左2
pwm_init(Fan_pwm_power4, 50, 500); // 动力风扇右2
// system_delay_ms(3000);
// // pwm_init(Fan_pwm_power1, 50, 1000); // 动力风扇左1
// // pwm_init(Fan_pwm_power2, 50, 1000); // 动力风扇右1
// // pwm_init(Fan_pwm_power3, 50, 1000); // 动力风扇左2
// // pwm_init(Fan_pwm_power4, 50, 1000); // 动力风扇右2
// // system_delay_ms(5000);
// pwm_set_duty(Fan_pwm_power1, 600);
// pwm_set_duty(Fan_pwm_power2, 600);
// pwm_set_duty(Fan_pwm_power3, 600);
// pwm_set_duty(Fan_pwm_power4, 600);
// while(1);
}
void by_pwm_update_duty(uint32_t update_pwm_duty)
void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2)
{
if (4000UL < update_pwm_duty) {
update_pwm_duty = 4000UL;
}
pwm_set_duty(TIM4_PWM_MAP1_CH1_D12, update_pwm_duty);
pwm_set_duty(TIM4_PWM_MAP1_CH2_D13, update_pwm_duty);
// pwm_set_duty(TIM4_PWM_MAP1_CH3_D14, update_pwm_duty);
// pwm_set_duty(TIM4_PWM_MAP1_CH4_D15, update_pwm_duty);
update_pwm_duty1=myclip(update_pwm_duty1, 500, 900);
update_pwm_duty2=myclip(update_pwm_duty2, 500, 900);
pwm_set_duty(Fan_pwm_up1, update_pwm_duty1);
pwm_set_duty(Fan_pwm_up2, update_pwm_duty2);
}
void by_pwm_power_duty(uint32_t power_pwm_duty_l, uint32_t power_pwm_duty_r)
void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1, uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2)
{
pwm_set_duty(TIM4_PWM_MAP1_CH3_D14, power_pwm_duty_l);
pwm_set_duty(TIM4_PWM_MAP1_CH4_D15, power_pwm_duty_r);
power_pwm_duty_l1=myclip(power_pwm_duty_l1, 500, 900);
power_pwm_duty_r1=myclip(power_pwm_duty_r1, 500, 900);
power_pwm_duty_l2=myclip(power_pwm_duty_l2, 500, 900);
power_pwm_duty_r2=myclip(power_pwm_duty_r2, 500, 900);
pwm_set_duty(Fan_pwm_power1, power_pwm_duty_l1);
pwm_set_duty(Fan_pwm_power2, power_pwm_duty_r1);
pwm_set_duty(Fan_pwm_power3, power_pwm_duty_l2);
pwm_set_duty(Fan_pwm_power4, power_pwm_duty_r2);
}

4
app/by_fan_control.h
View File

@@ -5,7 +5,7 @@
#include "ch32v30x.h"
extern void by_pwm_init(void);
extern void by_pwm_update_duty(uint32_t update_pwm_duty);
extern void by_pwm_power_duty(uint32_t power_pwm_duty_l, uint32_t power_pwm_duty_r);
void by_pwm_update_duty(uint32_t update_pwm_duty1,uint32_t update_pwm_duty2);
void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1,uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2);
#endif

18
app/by_imu.c
View File

@@ -14,8 +14,8 @@ gyro_param_t GyroOffset;
bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
float param_Kp = 0.17f; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004f; // 陀螺仪收敛速率的积分增益 0.004
float fast_sqrt(float x)
{
@@ -170,9 +170,9 @@ void ICM_getEulerianAngles(void)
float q3 = Q_info.q3;
// 四元数计算欧拉角
eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch
eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw
eulerAngle.pitch = asinf(-2 * q1 * q3 + 2 * q0 * q2) * 180.0f / M_PI; // pitch
eulerAngle.roll = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180.0f / M_PI; // roll
eulerAngle.yaw = atan2f(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180.0f / M_PI; // yaw
/* 姿态限制*/
if (eulerAngle.roll > 90 || eulerAngle.roll < -90) {
@@ -184,9 +184,9 @@ void ICM_getEulerianAngles(void)
}
}
if (eulerAngle.yaw > 360) {
eulerAngle.yaw -= 360;
} else if (eulerAngle.yaw < 0) {
eulerAngle.yaw += 360;
if (eulerAngle.yaw > 180) {
eulerAngle.yaw -= 180;
} else if (eulerAngle.yaw < -180) {
eulerAngle.yaw += 180;
}
}

7
app/isr.c
View File

@@ -38,6 +38,7 @@
#include "by_imu.h"
#include "jj_blueteeth.h"
#include "by_buzzer.h"
#include "jj_motion.h"
void NMI_Handler(void) __attribute__((interrupt()));
void HardFault_Handler(void) __attribute__((interrupt()));
@@ -92,8 +93,8 @@ void USART1_IRQHandler(void)
void USART2_IRQHandler(void)
{
if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
uart_query_byte(UART_2,&bt_buffer);
bt_rx_flag=true;
uart_query_byte(UART_2, &bt_buffer);
bt_rx_flag = true;
USART_ClearITPendingBit(USART2, USART_IT_RXNE);
}
}
@@ -272,6 +273,8 @@ void TIM1_UP_IRQHandler(void)
{
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
sport_motion2(bt_fly);
}
}

59
app/jj_blueteeth.c
View File

@@ -1,12 +1,23 @@
#include "jj_blueteeth.h"
#include "jj_motion.h"
#include "by_fan_control.h"
bool bt_rx_flag = false;
uint8 bt_buffer;//接收字符存入
uint8 bt_buffer; // 接收字符存入
float bt_angle = 0.0f;
uint32_t bt_run_flag = 0;
uint8_t bt_flow_flag = 0;
uint32_t bt_fly = 0;
uint32_t bt_flow = 0;
enum bt_order {
Start_work = 0x01,
Turn_Left = 0x02,
Turn_Right = 0x03,
Speed_up = 0x04,
Speed_down = 0x05,
Stop_fly = 0x06,
Flow_on = 0x08,
Flow_up = 0x09,
Flow_down = 0x10,
};
/**
* @brief 蓝牙初始化
@@ -15,7 +26,7 @@ enum bt_order {
void jj_bt_init()
{
uart_init(UART_2, 115200, UART2_MAP1_TX_D5, UART2_MAP1_RX_D6);
// uart_tx_interrupt(UART_2, 1);
// uart_tx_interrupt(UART_2, 1);
uart_rx_interrupt(UART_2, ENABLE);
}
/**
@@ -26,24 +37,56 @@ void jj_bt_run()
if (bt_rx_flag) {
switch (bt_buffer) {
case Start_work:
printf("1\r\n");
bt_angle = 0.0f;
break;
case Turn_Left:
printf("2\r\n");
bt_angle = 10.0f;
break;
case Turn_Right:
printf("3\r\n");
bt_angle = -10.0f;
break;
case Speed_down:
printf("5\r\n");
bt_fly -= 10;
break;
case Speed_up:
printf("4\r\n");
bt_fly += 10;
break;
case Stop_fly:
bt_run_flag = !bt_run_flag;
break;
case Flow_on:
bt_flow_flag = !bt_flow_flag;
break;
case Flow_up:
bt_flow += 20;
break;
case Flow_down:
bt_flow -= 20;
break;
default:
break;
}
uart_write_byte(UART_2,bt_buffer);
if (bt_flow_flag == 0) {
bt_flow = 0;
}
by_pwm_update_duty(bt_flow + 500, bt_flow + 500);
bt_rx_flag = false;
}
}
void bt_printf(const char *format, ...)
{
char sbuf[40];
va_list args;
va_start(args, format);
vsnprintf(sbuf, 40, format, args);
va_end(args);
for (uint16_t i = 0; i < strlen(sbuf); i++) {
while (USART_GetFlagStatus((USART_TypeDef *)uart_index[UART_2], USART_FLAG_TC) == RESET)
;
USART_SendData((USART_TypeDef *)uart_index[UART_2], sbuf[i]);
}
}

9
app/jj_blueteeth.h
View File

@@ -1,10 +1,13 @@
#ifndef _JJ_BLUETEETH_H_
#define _JJ_BLUETEETH_H_
#include "zf_common_headfile.h"
#include "stdio.h"
#include "zf_driver_uart.h"
extern bool bt_rx_flag;
extern uint8 bt_buffer;
extern uint8_t bt_buffer;
extern uint32_t bt_fly;
extern uint32_t bt_run_flag;
extern float bt_angle;
void jj_bt_init();
void jj_bt_run();
void bt_printf(const char *format, ...);
#endif

60
app/jj_motion.c Normal file
View File

@@ -0,0 +1,60 @@
#include "jj_motion.h"
#include "by_fan_control.h"
#include "by_imu.h"
#include "../3rd-lib/PID-Library/pid.h"
#include "jj_blueteeth.h"
#include "gl_common.h"
PID_TypeDef angle_pid;
PID_TypeDef gyro_z_pid;
PID_TypeDef img_x_pid;
float an_Kp = 8.0f;
float an_Ki = 0.0f;
float an_Kd = 2.0f;
float im_Kp = 1.0f;
float im_Ki = 0.0f;
float im_Kd = 0.0f;
float set_x = 0.0f;
float img_x = 40.0f;
float yaw0 = 0;
float out_M = 0;
float out_yaw;
uint32_t cnt1 = 0;
/**
* @brief
*
* @param fy y轴的加速度
*/
void sport_motion2(uint32_t fy)
{
cnt1++;
yaw0 = imu660ra_gyro_z;
if (cnt1 >= 50) {
PID_Compute(&angle_pid);
cnt1 = 0;
}
PID_Compute(&gyro_z_pid);
if (bt_run_flag == 1) {
by_pwm_power_duty((int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M),
(int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M));
} else {
by_pwm_power_duty(500, 500, 500, 500);
}
}
/**
* @brief 结构体初始化
*
*/
void sport_pid_init()
{
PID(&angle_pid, &bt_angle, &out_yaw, &set_x, im_Kp, im_Ki, im_Kd, 0, 0);
PID(&gyro_z_pid, &yaw0, &out_M, &out_yaw, an_Kp, an_Ki, an_Kd, 1, 1);
PID_Init(&angle_pid);
PID_Init(&gyro_z_pid);
PID_SetOutputLimits(&angle_pid, -500.0f, 500.0f);
PID_SetOutputLimits(&gyro_z_pid, -400.0f, 400.0f);
PID_SetMode(&gyro_z_pid, 1);
PID_SetMode(&angle_pid, 1);
}

18
app/jj_motion.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef _JJ_MOTION_H_
#define _JJ_MOTION_H_
#include "ch32v30x.h"
#include "../3rd-lib/PID-Library/pid.h"
extern PID_TypeDef angle_pid;
extern PID_TypeDef gyro_z_pid;
extern float an_Kp;
extern float an_Ki;
extern float an_Kd;
extern float im_Kp;
extern float im_Ki;
extern float im_Kd;
extern float out_M;
extern float out_yaw;
extern float yaw0;
void sport_pid_init();
void sport_motion2(uint32_t fy);
#endif

33
app/jj_param.c
View File

@@ -1,32 +1,25 @@
#include "jj_param.h"
#include "./page/page_ui_widget.h"
#include "./page/page.h"
#include "zf_common_headfile.h"
#include "jj_motion.h"
PARAM_INFO Param_Data[DATA_NUM];
soft_iic_info_struct eeprom_param;
TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM];
float data0 = 10.0f;
float data1 = 10.0f;
float data2 = 15;
float data3 = 100.01f;
float data4 = 1.04f;
float data5 = 4.0f;
float data6 = 5.1f;
void jj_param_eeprom_init()
{
soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化
PARAM_REG(DATA0, &data0, EFLOAT, 1, "m0_p"); // 注冊
PARAM_REG(DATA1, &data1, EFLOAT, 1, "m1_p"); // 注冊
PARAM_REG(DATA2, &data2, EFLOAT, 1, "m1_i"); // 注冊
PARAM_REG(DATA3, &data3, EFLOAT, 1, "m1_d"); // 注冊
PARAM_REG(DATA4, &data4, EFLOAT, 1, "m2_p"); // 注冊
PARAM_REG(DATA5, &data5, EFLOAT, 1, "m2_i"); // 注冊
PARAM_REG(DATA6, &data6, EFLOAT, 1, "m2_d"); // 注冊
PARAM_REG(angle_Kp, &an_Kp, EFLOAT, 1, "an_P"); // 注冊
PARAM_REG(angle_Ki, &an_Ki, EFLOAT, 1, "an_I"); // 注冊
PARAM_REG(angle_Kd, &an_Kd, EFLOAT, 1, "an_D"); // 注冊
PARAM_REG(imgax_Kp, &im_Kp, EFLOAT, 1, "im_P"); // 注冊
PARAM_REG(imgax_Ki, &im_Ki, EFLOAT, 1, "im_I"); // 注冊
PARAM_REG(imgax_Kd, &im_Kd, EFLOAT, 1, "im_D"); // 注冊
for (uint8 i = 0; i < DATA_IN_FLASH_NUM ; i++) {
for (uint8 i = 0; i < DATA_IN_FLASH_NUM; i++) {
soft_iic_read_8bit_registers(&eeprom_param, 4*i, (uint8 *)&iic_buffer[i], 4);
soft_iic_read_8bit_registers(&eeprom_param, 4 * i, (uint8 *)&iic_buffer[i], 4);
switch (Param_Data[i].type) {
case EFLOAT:
*((float *)(Param_Data[i].p_data)) =
@@ -43,7 +36,7 @@ void jj_param_eeprom_init()
default:
break;
}
system_delay_ms(10);
system_delay_ms(10);
}
}
/**
@@ -66,7 +59,7 @@ void jj_param_update()
default:
break;
}
soft_iic_write_8bit_registers(&eeprom_param, 4*i , (uint8 *)&iic_buffer[i], 4);
system_delay_ms(10);
soft_iic_write_8bit_registers(&eeprom_param, 4 * i, (uint8 *)&iic_buffer[i], 4);
system_delay_ms(10);
}
}

28
app/jj_param.h
View File

@@ -1,7 +1,9 @@
#ifndef _JJ_PARAM_H_
#define _JJ_PARAM_H_
#include "zf_common_headfile.h"
#include "stdio.h"
#include "ch32v30x.h"
#include "zf_driver_soft_iic.h"
/**
* @brief 注册需调参数
*
@@ -13,15 +15,12 @@
Param_Data[_data_tag_].text = _text_;
typedef enum {
DATA_HEAD = -1,
DATA0,
DATA1,
DATA2,
DATA3,
DATA4,
//
DATA5,
DATA6,
angle_Kp,
angle_Ki,
angle_Kd,
imgax_Kp,
imgax_Ki,
imgax_Kd,
DATA_IN_FLASH_NUM,
DATA_NUM,
} data_tag_t;
@@ -36,23 +35,22 @@ typedef enum {
}ENUM_TYPE;
typedef union{
uint32 u32;
int32 s32;
uint32_t u32;
int32_t s32;
float f32;
uint8 u8;
uint8_t u8;
}TYPE_UNION;
typedef struct {
void *p_data;
ENUM_TYPE type;
uint8 cmd;
uint8_t cmd;
char *text;
}PARAM_INFO;
extern soft_iic_info_struct eeprom_param;
extern PARAM_INFO Param_Data[DATA_NUM];
extern TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM];
extern float data1;
void jj_param_eeprom_init();
void jj_param_update();
void jj_param_show();

21
app/main.c
View File

@@ -29,31 +29,34 @@
#include "jj_param.h"
#include "./page/page_ui_widget.h"
#include "by_buzzer.h"
#include "jj_motion.h"
#include "by_imu.h"
int main(void)
{
clock_init(SYSTEM_CLOCK_120M);
system_delay_init();
debug_init();
// mt9v03x_init();
mt9v03x_init();
ips200_init(IPS200_TYPE_SPI);
by_gpio_init();
by_exit_init();
by_pwm_init();
jj_bt_init();
by_buzzer_init();
// while (imu660ra_init())
// ;
while (imu660ra_init())
;
jj_param_eeprom_init();
Page_Init();
sport_pid_init();
pit_ms_init(TIM6_PIT, 2);
pit_ms_init(TIM1_PIT, 2);
// gyroOffset_init();
while (1) {
Page_Run();
jj_bt_run();
queue_pop_read();
bt_printf("hello:%f,%f\n",out_M,out_yaw);
if (mt9v03x_finish_flag) {
// 该操作消耗大概 1970 个 tick，折合约 110us
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
@@ -69,4 +72,6 @@ int main(void)
MidLineTrack();
}
}
}
}

17
app/page/page_param.c
View File

@@ -1,10 +1,12 @@
#include "jj_param.h"
#include "page_ui_widget.h"
#include "jj_motion.h"
#include "page.h"
#define LINE_HEAD 0
#define LINE_END DATA_NUM - 2
static char Text[] = "RealTime Param";
int event_flag = 0;
int32_t index_power = 0;
static int8_t Curser = LINE_HEAD; // 定义光标位置
static int8_t Curser_Last = LINE_HEAD; // 定义光标位置
void jj_param_show();
@@ -29,6 +31,8 @@ static void Setup()
else if (Param_Data[i].type == EFLOAT)
ips200_show_float(50, i * 18 + 2, *((float *)(Param_Data[i].p_data)), 4, 5);
}
ips200_show_int(50, (DATA_NUM-1)* 18 + 2, index_power, 5);
}
/**
@@ -70,6 +74,7 @@ static void Event(page_event event)
return;
} else if (page_event_press_long == event) {
jj_param_update();
sport_pid_init();
Page_Shift(page_menu);
return;
}
@@ -78,11 +83,11 @@ static void Event(page_event event)
} else if (Curser > LINE_END) {
Curser = LINE_HEAD;
}
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
} else if (1 == event_flag) {
if (page_event_forward == event) {
if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) += 0.01f;
*((float *)(Param_Data[Curser].p_data)) += powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) += 1;
} else if (Param_Data[Curser].type == EUINT32) {
@@ -90,14 +95,18 @@ static void Event(page_event event)
}
} else if (page_event_backward == event) {
if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) -= 0.01f;
*((float *)(Param_Data[Curser].p_data)) -=powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) -= 1;
} else if (Param_Data[Curser].type == EUINT32) {
*((uint32 *)(Param_Data[Curser].p_data)) -= 1;
}
} else if (page_event_press_short == event) {
index_power++;
if (index_power > 2) {
index_power = -2;
}
ips200_show_int(50, (DATA_NUM-1)* 18 + 2, index_power, 5);
} else if (page_event_press_long == event) {
event_flag = 0;
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);