130 lines
2.8 KiB
C
130 lines
2.8 KiB
C
#ifndef NAVI_H
|
|
#define NAVI_H
|
|
|
|
|
|
#include "Action.h"
|
|
#include "pid.h"
|
|
#include "ahrs.h"
|
|
#include "bmi088.h"
|
|
#include "map.h"
|
|
#include "bsp_delay.h"
|
|
|
|
#define PI 3.1415926535f
|
|
|
|
typedef struct{
|
|
float pos_x;
|
|
float pos_y;
|
|
float last_pos_x;
|
|
float last_pos_y;
|
|
}chassis_position_t;
|
|
|
|
typedef struct {
|
|
fp32 vx;
|
|
fp32 vy;
|
|
fp32 vz;
|
|
}action_out_t;
|
|
typedef struct {
|
|
fp32 vx;
|
|
fp32 vy;
|
|
fp32 vz;
|
|
}sick_out_t;
|
|
|
|
typedef struct{
|
|
|
|
pid_param_t path_speed_pid_param;
|
|
pid_param_t pid_PosSpeed_xy_param;
|
|
pid_param_t pid_OutAngle_param;
|
|
pid_param_t pid_InnerAngle_param;
|
|
pid_param_t pid_OutAngle_hold_param;
|
|
pid_param_t pid_InnerAngle_hold_param;
|
|
pid_param_t pid_pos_xy_inner_param;
|
|
pid_param_t pid_pos_xy_outer_param;
|
|
pid_param_t pid_sick_out_param;
|
|
pid_param_t pid_sick_inner_param;
|
|
|
|
}ops_pid_param_t;
|
|
|
|
typedef struct {
|
|
ops_pid_param_t ops_pid;
|
|
point_t *path;
|
|
int8_t path_num;
|
|
}ops_param_t;
|
|
|
|
typedef enum {
|
|
normal = 0, // sick正常
|
|
blocked, // sick被阻挡
|
|
max_reached // sick测距已达最大值
|
|
} sick_status_t;
|
|
|
|
typedef struct {
|
|
|
|
int32_t sick_distance[4]; // 当前距离值
|
|
int32_t sick_distance_last[4]; // 上一次的距离值
|
|
int32_t sick_distance_diff[4]; //距离差值
|
|
fp32 sick_derivative[4]; // 存储微分结果
|
|
sick_status_t status[4]; // 每个距离传感器的状态标志位
|
|
} sick_d_t;
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
typedef struct {
|
|
|
|
float chassis_speed_set;
|
|
Action_POS_t *chassis_pos;
|
|
|
|
struct
|
|
{
|
|
fp32 x;
|
|
fp32 y;
|
|
}sick_offest;
|
|
|
|
Action_POS_t *chassis_pos_offest;
|
|
|
|
Path_t *chassis_map;
|
|
|
|
point_t transformed_target;
|
|
|
|
const ops_param_t *param;
|
|
|
|
AHRS_Eulr_t ops_imu_pos;
|
|
AHRS_Gyro_t ops_gyro;
|
|
|
|
struct
|
|
{
|
|
action_out_t action;
|
|
sick_out_t sick;
|
|
}final_out;
|
|
|
|
int16_t sick_distance[4];
|
|
|
|
sick_d_t sick_d;
|
|
|
|
int navi_flag;
|
|
int sick_flag;
|
|
|
|
pid_type_def path_speed_pid;
|
|
pid_type_def pid_PosSpeed_xy;
|
|
pid_type_def pid_OutAngle;
|
|
pid_type_def pid_InnerAngle;
|
|
pid_type_def pid_OutAngle_hold;
|
|
pid_type_def pid_InnerAngle_hold;
|
|
pid_type_def pid_pos_xy_inner;
|
|
pid_type_def pid_pos_xy_outer;
|
|
pid_type_def pid_sick_outer;
|
|
pid_type_def pid_sick_inner;
|
|
|
|
}ops_t;
|
|
|
|
int8_t Navi_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
|
|
bool isArrive(point_t p,ops_t *o);
|
|
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set);
|
|
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle,pid_type_def *outer_yaw_pid_set,pid_type_def *inner_yaw_pid_set);
|
|
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set);
|
|
void sick_point(ops_t *o);
|
|
void sick_calculate_d(ops_t *o);
|
|
void go_path(ops_t *o,CMD_ACTION_t *ops_out);
|
|
#endif
|
|
|