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 cross_aim=0.49f;
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 cross_aim;
extern float common_aim;
extern float barrier_aim;
extern float barrier_offset;
#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) {
aim_distance = cross_aim;
} else if (barrier_type != BARRIER_NONE) {
aim_distance = 0.3f;
aim_distance = barrier_aim;
} else {
aim_distance = common_aim;
}
@@ -138,10 +138,10 @@ void MidLineTrack()
// float error = -atan2f(dx, dy) * 180 / PI32;
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;
} 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;
} else {
dx_near = rptsn[aim_idx_near][1] - cx;

4
app/jj_param.c
View File

@@ -19,6 +19,10 @@ void jj_param_eeprom_init(void)
PARAM_REG(aim_cross, &cross_aim, EFLOAT, 1, "cros:"); // 注冊
PARAM_REG(aim_cricle, &cricle_aim, EFLOAT, 1, "cric:"); // 注冊
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(); // 注冊
}

3
app/jj_param.h
View File

@@ -21,7 +21,8 @@ typedef enum {
aim_common = Page1_head,
aim_cross,
aim_cricle,
aim_barrier,
offset_barrier,
Page2_head,
// 第二页参数
imgax_Kp = Page2_head,