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-03-23 18:26:03 +08:00
parent 2d221df9d8
commit 7d5bee30d1
5 changed files with 22 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app/gl_state.c
View File

@@ -5,3 +5,5 @@ enum state_type_e state_type =0;
float cricle_aim=0.2f; float cricle_aim=0.2f;
float cross_aim=0.49f; float cross_aim=0.49f;
float common_aim=0.35f; float common_aim=0.35f;
float barrier_aim=0.1f;
float barrier_offset=4.0f;

2
app/gl_state.h
View File

@@ -24,5 +24,7 @@ extern enum state_type_e state_type;
extern float cricle_aim; extern float cricle_aim;
extern float cross_aim; extern float cross_aim;
extern float common_aim; extern float common_aim;
extern float barrier_aim;
extern float barrier_offset;
#endif /* STATE_H_ */ #endif /* STATE_H_ */

6
app/gl_tracking.c
View File

@@ -29,7 +29,7 @@ void aim_distance_select(void)
} else if (circle_type != CIRCLE_NONE) { } else if (circle_type != CIRCLE_NONE) {
aim_distance = cross_aim; aim_distance = cross_aim;
} else if (barrier_type != BARRIER_NONE) { } else if (barrier_type != BARRIER_NONE) {
aim_distance = 0.3f; aim_distance = barrier_aim;
} else { } else {
aim_distance = common_aim; aim_distance = common_aim;
} }
@@ -138,10 +138,10 @@ void MidLineTrack()
// float error = -atan2f(dx, dy) * 180 / PI32; // float error = -atan2f(dx, dy) * 180 / PI32;
if (barrier_type == BARRIER_LEFT_BEGIN) { if (barrier_type == BARRIER_LEFT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx; dx_near = rptsn[aim_idx_near][1] - cx +barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} else if (barrier_type == BARRIER_RIGHT_BEGIN) { } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx; dx_near = rptsn[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} else { } else {
dx_near = rptsn[aim_idx_near][1] - cx; dx_near = rptsn[aim_idx_near][1] - cx;

10
app/jj_param.c
View File

@@ -16,9 +16,13 @@ uint32_t *addre[2];
void jj_param_eeprom_init(void) void jj_param_eeprom_init(void)
{ {
soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化 soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化
PARAM_REG(aim_cross, &cross_aim, EFLOAT, 1, "cros:"); // 注冊 PARAM_REG(aim_cross, &cross_aim, EFLOAT, 1, "cros:"); // 注冊
PARAM_REG(aim_cricle, &cricle_aim, EFLOAT, 1, "cric:"); // 注冊 PARAM_REG(aim_cricle, &cricle_aim, EFLOAT, 1, "cric:"); // 注冊
PARAM_REG(aim_common, &common_aim, EFLOAT, 1, "comm:"); // 注冊 PARAM_REG(aim_common, &common_aim, EFLOAT, 1, "comm:"); // 注冊
PARAM_REG(aim_barrier, &barrier_aim, EFLOAT, 1, "barr:");
PARAM_REG(offset_barrier, &barrier_offset, EFLOAT, 1, "offs:");
PARAM_REG(delta_x, &pure_angle, EFLOAT, 2, "far:");
PARAM_REG(delta_y, &dx_near, EFLOAT, 2, "near:");
jj_param_read(); // 注冊 jj_param_read(); // 注冊
} }

11
app/jj_param.h
View File

@@ -16,15 +16,16 @@
typedef enum { typedef enum {
Page1_head=0, Page1_head = 0,
aim_common=Page1_head, aim_common = Page1_head,
aim_cross, aim_cross,
aim_cricle, aim_cricle,
aim_barrier,
offset_barrier,
Page2_head, Page2_head,
// 第二页参数 // 第二页参数
imgax_Kp=Page2_head, imgax_Kp = Page2_head,
imgax_Ki, imgax_Ki,
imgax_Kd, imgax_Kd,
@@ -52,7 +53,7 @@ typedef union {
typedef struct { typedef struct {
void *p_data; void *p_data;
ENUM_TYPE type; ENUM_TYPE type;
uint8_t cmd;//01:仅存储 00：仅显示 02：传输并显示 uint8_t cmd; // 01:仅存储 00：仅显示 02：传输并显示
char *text; char *text;
} PARAM_INFO; } PARAM_INFO;
extern soft_iic_info_struct eeprom_param; extern soft_iic_info_struct eeprom_param;