350 lines
12 KiB
C
350 lines
12 KiB
C
#include "navi.h"
|
||
|
||
//导航初始化一系列参数
|
||
int8_t Navi_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos){
|
||
|
||
if(o ==NULL) return DEVICE_ERR;
|
||
if(param == NULL) return DEVICE_ERR;
|
||
|
||
o->param = param;
|
||
|
||
//底盘路径速度pid初始化
|
||
PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
|
||
/*全场定位pid初始化*/
|
||
//全场定位跑路径的xy方向速度环pid
|
||
PID_init(&o->pid_PosSpeed_xy, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_xy_param));
|
||
/* 初始化角度纠正PID */
|
||
PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
|
||
PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
|
||
|
||
PID_init(&o->pid_OutAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_hold_param));//外环
|
||
PID_init(&o->pid_InnerAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_hold_param));//内环
|
||
//底盘xy方向位置环pid初始化
|
||
PID_init(&o->pid_pos_xy_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_inner_param));
|
||
PID_init(&o->pid_pos_xy_outer,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_outer_param));
|
||
//sick
|
||
PID_init(&o->pid_sick_inner,PID_POSITION,(&o->param->ops_pid.pid_sick_inner_param));
|
||
PID_init(&o->pid_sick_outer,PID_POSITION,(&o->param->ops_pid.pid_sick_out_param));
|
||
|
||
//全场定位底盘坐标初始化
|
||
o->chassis_pos->pos_x=0;
|
||
o->chassis_pos->pos_y=0;
|
||
o->chassis_pos->pos_lastX=0;
|
||
o->chassis_pos->pos_lastY=0;
|
||
|
||
//码盘反馈的坐标
|
||
o->chassis_pos = pos;
|
||
//经过sick调整后码盘反馈的坐标
|
||
o->chassis_pos_offest->pos_x = pos->pos_x + o->sick_offest.x;
|
||
o->chassis_pos_offest->pos_y = pos->pos_y + o->sick_offest.y;
|
||
|
||
//底盘获取地图
|
||
o->chassis_map = get_map_pointer();
|
||
//设置全场定位允许的误差范围
|
||
o->chassis_map->pos_mistake = 30;
|
||
//小量程sick1分米大概变30，大量程sick1分米大概变60
|
||
o->chassis_map->sick_mistake = 15;
|
||
//o->state.points_num = param->path_num;
|
||
//标志位初始化
|
||
o->chassis_map->points_flag = 0;
|
||
o->navi_flag = 0;
|
||
o->sick_flag = 0;
|
||
|
||
//初始化sick微分数据
|
||
for(int i=0 ;i<4 ; i++){
|
||
|
||
o->sick_d.sick_distance[i] = 0;
|
||
o->sick_d.sick_distance_last[i] = 0;
|
||
o->sick_d.sick_derivative[i] = 0;
|
||
o->sick_d.status[i] = normal;
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/*判断是否寻迹完成函数(全场定位)*/
|
||
bool isArrive_path(point_t p,ops_t *o)
|
||
{
|
||
uint16_t xArrive = 0, yArrive = 0;
|
||
xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->chassis_map->pos_mistake ? 1:0;
|
||
yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->chassis_map->pos_mistake ? 1:0;
|
||
|
||
if(xArrive && yArrive ) return true;
|
||
else return false;
|
||
}
|
||
|
||
//利用C板imu纠正角度
|
||
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle,pid_type_def *outer_yaw_pid_set,pid_type_def *inner_yaw_pid_set){
|
||
|
||
fp32 delta_angle,delta_wz;
|
||
if(hope_angle - o->chassis_pos->pos_yaw >180){
|
||
hope_angle -= 360;
|
||
}
|
||
else if(hope_angle - o->chassis_pos->pos_yaw <-180){
|
||
hope_angle += 360;
|
||
}
|
||
//外环角度纠正
|
||
delta_angle = PID_calc(outer_yaw_pid_set,o->chassis_pos->pos_yaw,hope_angle);
|
||
//内环速度纠正
|
||
delta_wz = PID_calc(inner_yaw_pid_set,o->ops_gyro.z,delta_angle);
|
||
|
||
return delta_wz;
|
||
}
|
||
|
||
//底盘xy速度计算(全场定位)
|
||
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
|
||
o->final_out.action.vx = PID_calc(&o->pid_PosSpeed_xy, o->chassis_pos->pos_Vx,vx_set);
|
||
o->final_out.action.vy = PID_calc(&o->pid_PosSpeed_xy, o->chassis_pos->pos_Vy,vy_set);
|
||
o->final_out.action.vz = ops9_AngleCorr(o,yaw_angle_set,&o->pid_OutAngle,&o->pid_InnerAngle);
|
||
}
|
||
|
||
//定点控制
|
||
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set){
|
||
|
||
fp32 delta_x,delta_y;
|
||
fp32 delta_vx,delta_vy;
|
||
fp32 hold_vx,hold_vy;
|
||
|
||
//x
|
||
delta_x = PID_calc(&o->pid_pos_xy_outer,o->chassis_pos->pos_x,p.x);
|
||
delta_vx = PID_calc(&o->pid_pos_xy_inner,o->chassis_pos->pos_Vx,delta_x);
|
||
|
||
//y
|
||
delta_y = PID_calc(&o->pid_pos_xy_outer,o->chassis_pos->pos_y,p.y);
|
||
delta_vy = PID_calc(&o->pid_pos_xy_inner,o->chassis_pos->pos_Vy,delta_y);
|
||
|
||
//目标速度
|
||
hold_vx = delta_vy*cos((o->chassis_pos->pos_yaw)*PI/180.0f) - delta_vx*sin((o->chassis_pos->pos_yaw)*PI/180.0f);
|
||
hold_vy = delta_vy*sin((o->chassis_pos->pos_yaw)*PI/180.0f) + delta_vx*cos((o->chassis_pos->pos_yaw)*PI/180.0f);
|
||
|
||
//底盘速度
|
||
o->final_out.action.vx = hold_vx;
|
||
o->final_out.action.vy = hold_vy;
|
||
o->final_out.action.vz = ops9_AngleCorr(o,yaw_angle_set,&o->pid_OutAngle_hold,&o->pid_InnerAngle_hold);
|
||
|
||
}
|
||
|
||
//sick卡点
|
||
void sick_point(ops_t *o){
|
||
|
||
//x是x轴的sick,y是y轴的sick，每个轴各两个
|
||
fp32 delta_x,delta_y;
|
||
fp32 delta_vx,delta_vy;
|
||
//x轴
|
||
if(o->sick_d.status[0] == normal) {
|
||
delta_x = -PID_calc(&o->pid_sick_outer,o->sick_distance[0],o->chassis_map->sick->sick_qian);
|
||
delta_vx = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vx,delta_x);
|
||
}
|
||
else if(o->sick_d.status[2] == normal){
|
||
delta_x = -PID_calc(&o->pid_sick_outer,o->sick_distance[2],o->chassis_map->sick->sick_hou);
|
||
delta_vx = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vx,delta_x);
|
||
}
|
||
else{
|
||
delta_vx = 0;
|
||
}
|
||
//y轴
|
||
if(o->sick_d.status[1] == normal){
|
||
delta_y = -PID_calc(&o->pid_sick_outer,o->sick_distance[1],o->chassis_map->sick->sick_you);
|
||
delta_vy = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vy,delta_y);
|
||
}
|
||
else if(o->sick_d.status[3] == normal){
|
||
delta_y = -PID_calc(&o->pid_sick_outer,o->sick_distance[3],o->chassis_map->sick->sick_zuo);
|
||
delta_vy = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vy,delta_y);
|
||
}
|
||
else{
|
||
delta_vy = 0;
|
||
}
|
||
|
||
//底盘速度
|
||
o->final_out.sick.vx = delta_vx;
|
||
o->final_out.sick.vy = delta_vy;
|
||
o->final_out.sick.vz = 0;//启用sick时不需要底盘旋转
|
||
}
|
||
|
||
// 计算sick微分函数
|
||
void sick_calculate_d(ops_t *o){
|
||
|
||
static uint32_t last_calc_time =0;
|
||
uint32_t current_time = HAL_GetTick();
|
||
|
||
// 计算实际时间差(ms)
|
||
uint32_t time_diff = current_time - last_calc_time;
|
||
|
||
// 至少400ms才计算一次
|
||
//采样频率不能太高！！
|
||
if (time_diff < 400) {
|
||
return;
|
||
}
|
||
|
||
// 转换为秒
|
||
float dt = time_diff / 1000.0f;
|
||
last_calc_time = current_time;
|
||
|
||
for(int i=0; i<4; i++){
|
||
|
||
o->sick_d.sick_distance[i] = o->sick_distance[i];
|
||
o->sick_d.sick_distance_diff[i] = o->sick_d.sick_distance[i] - o->sick_d.sick_distance_last[i];
|
||
o->sick_d.sick_derivative[i] = (float)o->sick_d.sick_distance_diff[i] / dt;
|
||
o->sick_d.sick_distance_last[i] = o->sick_distance[i];
|
||
|
||
switch(o->sick_d.status[i]){
|
||
|
||
/*目前这套switch能用，但是没有考虑到一种情况，即有车挡住了sick使标志位变成了block，
|
||
但它缓慢后退然后平移离开导致标志位不能变回normal，后期可以加一个如果底盘没有动微分值改变
|
||
说明被阻挡*/
|
||
case normal:
|
||
// 检查是否达到最大测量值 小量程sick测量最大值：3238，大量程sick测量最大值：6487
|
||
if(o->sick_d.sick_derivative[i] == 0 &&
|
||
((o->sick_d.sick_distance[i] > 3200 && o->sick_d.sick_distance[i] < 3300) ||
|
||
(o->sick_d.sick_distance[i] > 6400 && o->sick_d.sick_distance[i] < 6500))){
|
||
|
||
o->sick_d.status[i] = max_reached;
|
||
}
|
||
// 检查是否被阻挡
|
||
else if(o->sick_d.sick_derivative[i] < -1500){
|
||
|
||
o->sick_d.status[i] = blocked;
|
||
}
|
||
break;
|
||
|
||
case blocked:
|
||
// 阻挡解除
|
||
if(o->sick_d.sick_derivative[i] > 1500){
|
||
|
||
o->sick_d.status[i] = normal;
|
||
}
|
||
break;
|
||
|
||
case max_reached:
|
||
// 从最大测量值恢复的条件
|
||
if(o->sick_d.sick_derivative[i] != 0 ){
|
||
|
||
if(o->sick_d.sick_derivative[i] < -1500){
|
||
|
||
o->sick_d.status[i] = blocked;
|
||
}
|
||
else{
|
||
|
||
o->sick_d.status[i] = normal;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
//寻迹函数
|
||
void go_path(ops_t *o,CMD_ACTION_t *ops_out){
|
||
|
||
//检查码盘是否正常
|
||
if(o->chassis_pos->Action_ready == 1){
|
||
//路径未完成
|
||
if(o->chassis_map->points_flag == 0){
|
||
|
||
//计算距离
|
||
float delta_x = o->chassis_map->target->x - o->chassis_pos->pos_x;
|
||
float delta_y = o->chassis_map->target->y - o->chassis_pos->pos_y;
|
||
float distance = sqrtf(delta_x * delta_x + delta_y * delta_y);
|
||
|
||
//计算速度
|
||
//o->chassis_speed_set = o->chassis_map->target->v;//此处设定为固定的期望速度
|
||
o->chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);
|
||
|
||
//速度分解
|
||
float chassis_vx,chassis_vy;//底盘xy分速度
|
||
if (distance > 1e-6f) { // 避免除零
|
||
chassis_vx = o->chassis_speed_set * (delta_x / distance);
|
||
chassis_vy = o->chassis_speed_set * (delta_y / distance);
|
||
}
|
||
else{
|
||
chassis_vx = 0;
|
||
chassis_vy = 0;
|
||
}
|
||
o->navi_flag = 0;
|
||
|
||
//未到达导航点附近
|
||
if(!isArrive_path(*(o->chassis_map->target),o)){
|
||
|
||
POS_chassis_set(o,chassis_vx,chassis_vy,o->chassis_map->target->angle);
|
||
o->navi_flag = 1;
|
||
|
||
}
|
||
else{
|
||
//已到达目标点附近，进行微调
|
||
hold_point(o,*(o->chassis_map->target),o->chassis_map->target->angle);
|
||
o->navi_flag = 2;
|
||
}
|
||
|
||
ops_out->out.Vx = o->final_out.action.vx;
|
||
ops_out->out.Vy = o->final_out.action.vy;
|
||
ops_out->out.Vw = o->final_out.action.vz;
|
||
|
||
//路径完成,具体的判断值根据之后导航速度提上来之后再定
|
||
if (fabsf(o->final_out.action.vx) < 1000.0f &&
|
||
fabsf(o->final_out.action.vy) < 1000.0f &&
|
||
fabsf(o->final_out.action.vz) < 1000.0f){
|
||
|
||
// o->chassis_map->points_flag = 1;
|
||
o->sick_flag = 0;
|
||
}
|
||
}
|
||
// else if(o->chassis_map->points_flag == 1 &&o->chassis_map->sick->sick_flag == 1){
|
||
// if(o->sick_flag != 1){
|
||
//
|
||
// o->navi_flag = 3;
|
||
//
|
||
// bool x_has_valid_sick = false; // X 轴是否有可用的 Sick
|
||
// bool y_has_valid_sick = false; // Y 轴是否有可用的 Sick
|
||
// bool x_adjusted = false; // X 轴是否达标
|
||
// bool y_adjusted = false; // Y 轴是否达标
|
||
|
||
// // X 轴检查
|
||
// if (o->sick_d.status[0] == normal) { // 前 Sick 可用
|
||
// x_adjusted = (abs_float_double(o->sick_distance[0], o->chassis_map->sick->sick_qian) < o->chassis_map->sick_mistake);
|
||
// x_has_valid_sick = true;
|
||
// }
|
||
// else if (o->sick_d.status[2] == normal) { // 后 Sick 可用
|
||
// x_adjusted = (abs_float_double(o->sick_distance[2], o->chassis_map->sick->sick_hou) < o->chassis_map->sick_mistake);
|
||
// x_has_valid_sick = true;
|
||
// }
|
||
// // 如果 X 轴两个 Sick 都不可用，x_has_valid_sick 保持 false
|
||
|
||
// // Y 轴检查
|
||
// if (o->sick_d.status[1] == normal) { // 右 Sick 可用
|
||
// y_adjusted = (abs_float_double(o->sick_distance[1], o->chassis_map->sick->sick_you) < o->chassis_map->sick_mistake);
|
||
// y_has_valid_sick = true;
|
||
// }
|
||
// else if (o->sick_d.status[3] == normal) { // 左 Sick 可用
|
||
// y_adjusted = (abs_float_double(o->sick_distance[3], o->chassis_map->sick->sick_zuo) < o->chassis_map->sick_mistake);
|
||
// y_has_valid_sick = true;
|
||
// }
|
||
// // 如果 Y 轴两个 Sick 都不可用，y_has_valid_sick 保持 false
|
||
|
||
// // 只有 X 轴和 Y 轴各至少有一个 Sick 可用，且所有可用 Sick 都达标时，才标记完成
|
||
// if (x_has_valid_sick && y_has_valid_sick && x_adjusted && y_adjusted) {
|
||
//
|
||
// o->sick_flag = 1;
|
||
// o->sick_offest.x = o->chassis_map->target->x - o->chassis_pos->pos_x;
|
||
// o->sick_offest.y = o->chassis_map->target->y - o->chassis_pos->pos_y;
|
||
// o->navi_flag = 4;
|
||
// }
|
||
//
|
||
// sick_point(o);
|
||
//
|
||
//// ops_out->out.Vx = o->final_out.sick.vx;
|
||
//// ops_out->out.Vy = o->final_out.sick.vy;
|
||
//// ops_out->out.Vw = o->final_out.sick.vz;
|
||
// }
|
||
// else{
|
||
// ops_out->out.Vx = 0;
|
||
// ops_out->out.Vy = 0;
|
||
// ops_out->out.Vw = 0;
|
||
// }
|
||
// }
|
||
}else{
|
||
ops_out->out.Vx = 0;
|
||
ops_out->out.Vy = 0;
|
||
ops_out->out.Vw = 0;
|
||
}
|
||
}
|