162 lines
3.0 KiB
C
162 lines
3.0 KiB
C
/*
|
||
*地图，存储全场定位点位
|
||
*
|
||
*/
|
||
#include "map.h"
|
||
#include "define.h"
|
||
/* Private variables -------------------------------------------------------- */
|
||
|
||
/*
|
||
0---- 1 ______ 7
|
||
/ \
|
||
2 6
|
||
| |
|
||
3 ---- 4 ---- 5
|
||
|
||
计算值: 0-1：3.1m 1-2: 根2x 2-3：2.5-x 3-4:1.25+x 4-5:1.25+x 5-6:2.5-x 6-7:根2x 7-1:2.5m
|
||
测量值：x≈0.8（量的） 1-2: 2-3: 3-4: 4-5: 5-6: 6-7: 7-1:
|
||
|
||
*/
|
||
|
||
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;
|
||
point_t path_4;
|
||
sick_t sick_4;
|
||
point_t path_5;
|
||
sick_t sick_5;
|
||
point_t path_6;
|
||
sick_t sick_6;
|
||
point_t path_7;
|
||
sick_t sick_7;
|
||
point_t path_8;
|
||
sick_t sick_8;
|
||
point_t path_9;
|
||
sick_t sick_9;
|
||
|
||
//点位坐标x轴朝前，y轴朝右
|
||
void block_select(Action_POS_t*pos,CMD_t*cmd){
|
||
|
||
// path_1.x = 0;
|
||
// path_1.y = 0;
|
||
// path_1.angle=0;
|
||
// sick_1.sick_flag = 0;
|
||
|
||
// path_2.x = pos->pos_x;
|
||
// path_2.y = pos->pos_y;
|
||
// path_2.angle= 0;
|
||
// sick_2.sick_flag = 0;
|
||
|
||
//1
|
||
path_1.x = 0;
|
||
path_1.y = 0;
|
||
path_1.angle=0;
|
||
sick_1.sick_flag = 0;
|
||
//2
|
||
path_2.x = 0;
|
||
path_2.y = 3100;
|
||
path_2.angle=0;
|
||
sick_2.sick_flag = 0;
|
||
//3
|
||
path_3.x = -800;
|
||
path_3.y = 2300;
|
||
path_3.angle=0;
|
||
sick_3.sick_flag = 0;
|
||
//4
|
||
path_4.x = -2500;
|
||
path_4.y = 2300;
|
||
path_4.angle= 0;
|
||
sick_4.sick_flag = 0;
|
||
//5
|
||
path_5.x = -2500;
|
||
path_5.y = 4350;
|
||
path_5.angle= 0;
|
||
sick_5.sick_flag = 0;
|
||
//6
|
||
path_6.x = -2500;
|
||
path_6.y = 6400;
|
||
path_6.angle= 0;
|
||
sick_6.sick_flag = 0;
|
||
//7
|
||
path_7.x = -800;
|
||
path_7.y = 6400;
|
||
path_7.angle= 0;
|
||
sick_7.sick_flag = 0;
|
||
//8
|
||
path_8.x = 0;
|
||
path_8.y = 5600;
|
||
path_8.angle= 0;
|
||
sick_8.sick_flag = 0;
|
||
//9
|
||
path_9.x = 0;
|
||
path_9.y = 3100;
|
||
path_9.angle= 0;
|
||
sick_9.sick_flag = 0;
|
||
}
|
||
|
||
|
||
/**
|
||
* 路径选择
|
||
*
|
||
* 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;
|
||
break;
|
||
case POS_3:
|
||
path_state.target = &path_3;
|
||
break;
|
||
case POS_4:
|
||
path_state.target = &path_4;
|
||
path_state.sick = &sick_4;
|
||
break;
|
||
case POS_5:
|
||
path_state.target = &path_5;
|
||
path_state.sick = &sick_5;
|
||
break;
|
||
case POS_6:
|
||
path_state.target = &path_6;
|
||
path_state.sick = &sick_6;
|
||
break;
|
||
case POS_7:
|
||
path_state.target = &path_7;
|
||
path_state.sick = &sick_7;
|
||
break;
|
||
case POS_8:
|
||
path_state.target = &path_8;
|
||
path_state.sick = &sick_8;
|
||
break;
|
||
case POS_9:
|
||
path_state.target = &path_9;
|
||
path_state.sick = &sick_9;
|
||
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;
|
||
}
|