217 lines
6.4 KiB
C
217 lines
6.4 KiB
C
#include "navi.h"
|
||
|
||
|
||
//导航初始化一系列参数
|
||
int8_t Action_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;
|
||
// while(!Action_ready)
|
||
// {
|
||
// TIM4->CCR3 = 9999;//等待全场定位初始化
|
||
// }
|
||
|
||
// TIM4->CCR3 = 0;
|
||
// HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
|
||
//
|
||
|
||
// osDelay(5);
|
||
|
||
o->param = param;
|
||
/*全场定位pid初始化*/
|
||
//全场定位跑路径的xy方向速度环pid
|
||
PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
|
||
PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_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));//内环
|
||
//底盘xy方向位置环pid初始化
|
||
//内环
|
||
PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
|
||
PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
|
||
//外环
|
||
PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
|
||
PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));
|
||
|
||
|
||
|
||
//底盘路径速度pid初始化
|
||
PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_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_map =param->path ;
|
||
o->chassis_pos = pos;
|
||
|
||
|
||
//设置全场定位允许的误差范围
|
||
o->state.mistake = POS_ALLOW_MISTAKE;
|
||
o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
|
||
o->state.moveState = START;
|
||
o->state.points_num = param->path_num;
|
||
//标志位初始化
|
||
o->POS_IS_CPT = NO;
|
||
|
||
|
||
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
//利用C板imu纠正角度
|
||
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
|
||
{
|
||
fp32 delta_angle,delta_w;
|
||
//外环角度纠正
|
||
delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
|
||
//内环速度纠正
|
||
delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
|
||
return delta_w;
|
||
}
|
||
|
||
//底盘xy速度计算(全场定位)
|
||
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
|
||
o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
|
||
o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
|
||
o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
|
||
}
|
||
|
||
|
||
//定点控制
|
||
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
|
||
{
|
||
fp32 delta_x,delta_y;
|
||
fp32 delta_vx,delta_vy;
|
||
fp32 vx_set,vy_set;//x,y,w目标速度
|
||
|
||
//x
|
||
delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
|
||
delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
|
||
|
||
//y
|
||
delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
|
||
delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
|
||
|
||
|
||
|
||
|
||
vx_set = delta_vx;
|
||
vy_set = delta_vy;
|
||
|
||
POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
|
||
}
|
||
|
||
|
||
|
||
|
||
//判断是否到达函数(全场定位)，最最后判断
|
||
bool isArrive(point_t p,ops_t *o)
|
||
{
|
||
uint16_t xArrive = 0, yArrive = 0, zArrive=0;
|
||
|
||
xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
|
||
yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
|
||
|
||
if(xArrive && yArrive) return true;
|
||
else return false;
|
||
}
|
||
|
||
|
||
|
||
|
||
//寻迹，跑点函数
|
||
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
|
||
{
|
||
static fp32 distance;//两点之间的距离差
|
||
static fp32 chassis_speed_set;//底盘速度设置
|
||
static fp32 SIN,COS;//方向解算
|
||
static fp32 world_vx,world_vy;//x,y方向分解速度
|
||
static fp32 chassis_vx,chassis_vy;//底盘xy分速度
|
||
static int cnt =0 ;//计数标志位
|
||
|
||
static int flag_update_num;//记录flag更新，防止cnt一直变化
|
||
|
||
/*数据同步*/
|
||
o->current_x =o->chassis_pos ->pos_x ;
|
||
o->current_y =o->chassis_pos ->pos_y ;
|
||
o->next_mapx = o->chassis_map[cnt].x;
|
||
o->next_mapy =o->chassis_map[cnt].y;
|
||
o->Navi_Mode .Trig_flag = ops_out ->flag ;
|
||
|
||
if(o == NULL) return DEVICE_ERR;
|
||
if (ops_out == NULL) return DEVICE_ERR;
|
||
|
||
// static fp32 yaw_angle_set = 0;
|
||
if(o->POS_IS_CPT == NO)//路径未跑完
|
||
{
|
||
if(o->state.moveState ==START)
|
||
{
|
||
//更新路径状态
|
||
o->state.moveState =MOVING;
|
||
}
|
||
|
||
|
||
distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
|
||
+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
|
||
|
||
chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
|
||
//速度分解
|
||
SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
|
||
COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
|
||
|
||
world_vx = chassis_speed_set * COS;
|
||
world_vy = chassis_speed_set * SIN;
|
||
|
||
// chassis_vx = world_vx;
|
||
// chassis_vy = world_vy;
|
||
//世界坐标转机器坐标系转换
|
||
|
||
chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
|
||
chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
|
||
|
||
}
|
||
|
||
/*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/
|
||
|
||
|
||
if(isArrive(o->chassis_map[cnt],o)) //判断是否到达(全场定位)，只涉及xy的判断
|
||
{
|
||
/*到达点后，这里应该增加cnt增加或减小的触发方式*/
|
||
|
||
if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt
|
||
|
||
if (o->Navi_Mode .Trig_flag==1){
|
||
cnt++;
|
||
}
|
||
}
|
||
else if (o->Navi_Mode .Trig_flag == -1) {
|
||
cnt--;
|
||
}
|
||
flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
|
||
}
|
||
else
|
||
{
|
||
o->POS_IS_CPT = NO;
|
||
hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle); //调用该函数使临近误差值快速响应
|
||
}
|
||
if(cnt>3)//根据点数更改
|
||
{
|
||
o->POS_IS_CPT =YES;
|
||
//到达目的地
|
||
o->final_out .vx =0;
|
||
o->final_out .vy =0;
|
||
o->final_out.yaw_angle=0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
ops_out->out.Vx= o->final_out .vx ;
|
||
ops_out->out.Vy = o->final_out .vy ;
|
||
ops_out->out.Vw = o->final_out.yaw_angle;
|
||
|
||
return DEVICE_OK;
|
||
}
|