110 lines
2.2 KiB
C
110 lines
2.2 KiB
C
/*
|
||
*地图，存储全场定位点位
|
||
*
|
||
*/
|
||
#include "map.h"
|
||
/* Private variables -------------------------------------------------------- */
|
||
|
||
|
||
Path_t path_state;
|
||
point_t path_1;
|
||
sick_t sick_1;
|
||
point_t path_2;
|
||
sick_t sick_2;
|
||
point_t path_3;
|
||
sick_t sick_3;
|
||
|
||
//点位坐标x轴朝前，y轴朝右
|
||
void block_select(Action_POS_t*pos,CMD_t*cmd){
|
||
|
||
//1
|
||
path_1.x = 0;
|
||
path_1.y = 0;
|
||
path_1.angle=0;
|
||
sick_1.sick_qian = 0;
|
||
sick_1.sick_you = 0;
|
||
sick_1.sick_hou = 0;
|
||
sick_1.sick_zuo = 0;
|
||
sick_1.sick_flag = 0;
|
||
//2
|
||
path_2.x = 2000;
|
||
path_2.y = -2000;
|
||
path_2.angle=0;
|
||
sick_2.sick_qian = 1873;
|
||
sick_2.sick_you = 2180;
|
||
sick_2.sick_hou = 0;
|
||
sick_2.sick_zuo = 3350;
|
||
sick_2.sick_flag = 1;
|
||
//3
|
||
path_3.x = pos->pos_x;
|
||
path_3.y = pos->pos_y;
|
||
//path_3.angle= 10;
|
||
sick_3.sick_qian = 0;
|
||
sick_3.sick_you = 0;
|
||
sick_3.sick_hou = 0;
|
||
sick_3.sick_zuo = 0;
|
||
sick_3.sick_flag = 0;
|
||
|
||
// 状态控制变量
|
||
static float fixed_yaw = 0;
|
||
static int is_calibrating = 0;
|
||
static float prev_camera_yaw = 0;
|
||
|
||
// 非NAVI模式直接重置状态
|
||
if (cmd->C_cmd.mode != NAVI) {
|
||
is_calibrating = 0;
|
||
}
|
||
|
||
if (cmd->camera_yaw != 0) {
|
||
// 只有未校准时才更新固定值
|
||
if (is_calibrating == 0) {
|
||
|
||
fixed_yaw = pos->pos_yaw + cmd->camera_yaw;
|
||
is_calibrating = 1;
|
||
}
|
||
|
||
// 校准期间始终使用fixed_yaw
|
||
path_3.angle = fixed_yaw;
|
||
prev_camera_yaw = cmd->camera_yaw;
|
||
}
|
||
}
|
||
|
||
|
||
/**
|
||
* 路径选择
|
||
*
|
||
* path_state.target
|
||
*/
|
||
void path_select(CMD_t*cmd){
|
||
|
||
static int8_t last_pos = -1;
|
||
|
||
if (cmd->C_cmd.pos != last_pos) {
|
||
|
||
switch (cmd->C_cmd.pos){
|
||
case POS_1:
|
||
path_state.target = &path_1;//选择路径
|
||
path_state.sick = &sick_1;
|
||
break;
|
||
case POS_2:
|
||
path_state.target = &path_2;//选择路径
|
||
path_state.sick = &sick_2;
|
||
break;
|
||
case POS_3:
|
||
path_state.target = &path_3;//选择路径
|
||
path_state.sick = &sick_3;
|
||
break;
|
||
}
|
||
path_state.points_flag = 0;
|
||
}
|
||
last_pos = cmd->C_cmd.pos; // 更新上一次的位置
|
||
|
||
if(cmd->C_cmd.mode != NAVI){
|
||
path_state.points_flag = 0;
|
||
}
|
||
}
|
||
|
||
Path_t *get_map_pointer(void){
|
||
return &path_state;
|
||
}
|