2020电赛D题绕组飞行器
在准备电赛的过程中，做了一下去年的题，本文将介绍我的方案及部分代码，希望可以帮助到大家。
一、我的装备
由于初学飞控所以主控用的是匿名的拓空者，还有匿名的光流传感器，北醒的激光雷达，星瞳科技的OPENMV和正点原子的激光测距模块ATK-VL53L0X。
二、实现思路
1、一键起飞，然后自旋找到第一个杆。
2、找到第一个杆后，由于激光测距模块最远测距是2m，实际更短，所以还测不到距离，因此要先缓慢靠近杆，直到激光测距模块有效，再进行PID控制与杆的位置，期间始终有OPENMV的角度调节。
3、等到角度和距离都合适开始绕杆，通过航向角控制旋转角度
4、绕完第一个杆后边后退，自旋寻找第二个杆。
5、找到后重复绕第一个杆的过程
6、寻找降落点然后降落（我没有实现这个任务）
三、代码介绍
1、下面这一段代码是ATK-VL53L0X的控制程序，由正点原子的测试例程修改，具体的模块介绍可以看正点原子的使用手册。
#include "vl53l0x.h"
#include "vl53l0x_i2c.h"
VL53L0X_Dev_t vl53l0x_dev;//设备I2C数据参数
VL53L0X_DeviceInfo_t vl53l0x_dev_info;//设备ID版本信息
VL53L0X_RangingMeasurementData_t vl53l0x_data;//测距测量结构体
vu16 vl53l0x_pdata[10];
mode_data Mode_data[]=
{
{
(FixPoint1616_t)(0.25*65536),
(FixPoint1616_t)(18*65536),
33000,
14,
10},//默认
{
(FixPoint1616_t)(0.25*65536) ,
(FixPoint1616_t)(18*65536),
200000,
14,
10},//高精度
{
(FixPoint1616_t)(0.1*65536) ,
(FixPoint1616_t)(60*65536),
33000,
18,
14},//长距离
{
(FixPoint1616_t)(0.25*65536) ,
(FixPoint1616_t)(32*65536),
20000,
14,
10},//高速
}; //三种模式的默认校准参数
VL53L0X_Error vl53l0x_start_single_test(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata,char *buf)
{
static u8 i=0;
static vu32 sum=0;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_GetRangingMeasurementData(dev,pdata);//获取测量距离,并且显示距离
if(pdata->RangeMilliMeter<2000) //这里人工做一个滤波，因为测距模块超出最大距离会显示八米
{
sum+=pdata->RangeMilliMeter;
i++;
if(i==5)
{
opmv_gan_ctrl.Juli=sum/5; //取五次的平均值
sum=0;
i=0;
}
}
return status;
}
VL53L0X_Error vl53l0x_set_mode(VL53L0X_Dev_t *dev,u8 mode) //设置测距模式，这里用的是连续高速测距
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
uint32_t refSpadCount;
uint8_t isApertureSpads;
status = VL53L0X_StaticInit(dev);
status = VL53L0X_PerformRefCalibration(dev, &VhvSettings, &PhaseCal);//Ref参考校准
Delay_ms(2);
status = VL53L0X_PerformRefSpadManagement(dev, &refSpadCount, &isApertureSpads);//执行参考SPAD管理
Delay_ms(2);
status = VL53L0X_SetDeviceMode(dev,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);//使能连续测量模式
Delay_ms(2);
status = VL53L0X_SetInterMeasurementPeriodMilliSeconds(dev,Mode_data[mode].timingBudget);//设置内部周期测量时间
Delay_ms(2);
status = VL53L0X_SetLimitCheckEnable(dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,1);//使能SIGMA范围检查
Delay_ms(2);
status = VL53L0X_SetLimitCheckEnable(dev,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1);//使能信号速率范围检查
Delay_ms(2);
status = VL53L0X_SetLimitCheckValue(dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,Mode_data[mode].sigmaLimit);//设定SIGMA范围
Delay_ms(2);
status = VL53L0X_SetLimitCheckValue(dev,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,Mode_data[mode].signalLimit);//设定信号速率范围范围
Delay_ms(2);
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev,Mode_data[mode].timingBudget);//设定完整测距最长时间
Delay_ms(2);
status = VL53L0X_SetVcselPulsePeriod(dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, Mode_data[mode].preRangeVcselPeriod);//设定VCSEL脉冲周期
Delay_ms(2);
status = VL53L0X_SetVcselPulsePeriod(dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, Mode_data[mode].finalRangeVcselPeriod);//设定VCSEL脉冲周期范围
VL53L0X_StartMeasurement(dev);
return status;
}
VL53L0X_Error vl53l0x_init(VL53L0X_Dev_t *dev) //主要是初始化IIC
{
GPIO_InitTypeDef GPIO_InitStructure;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_Dev_t *pMyDevice = dev;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//先使能外设IO PORTC时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;//端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//普通输出模式
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
GPIO_Init(GPIOB, &GPIO_InitStructure);//初始化
pMyDevice->I2cDevAddr = VL53L0X_Addr;//I2C地址(上电默认0x52)
pMyDevice->comms_type = 1; //I2C通信模式
pMyDevice->comms_speed_khz = 400; //I2C通信速率
VL53L0X_i2c_init();//初始化IIC总线
GPIO_ResetBits(GPIOB,GPIO_Pin_0);//失能VL53L0X
Delay_ms(30);
GPIO_SetBits(GPIOB,GPIO_Pin_0);//使能VL53L0X,让传感器处于工作
Delay_ms(30);
Status = VL53L0X_DataInit(pMyDevice);//设备初始化
Delay_ms(2);
Status = VL53L0X_GetDeviceInfo(pMyDevice,&vl53l0x_dev_info);//获取设备ID信息
return Status;
}
void vl53l0x_Init(void) //这个函数放到主函数的初始化那里
{
while(vl53l0x_init(&vl53l0x_dev)) ; //vl53l0x初始化
while(vl53l0x_set_mode(&vl53l0x_dev,3)); //配置测量模式
}
void vl53l0x_Task(u8 dT_ms) //这个函数放到50ms的线程中
{
static char buf[VL53L0X_MAX_STRING_LENGTH];//测试模式字符串字符缓冲区
vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
}
2、下面是主要的控制函数，结构是模仿匿名的巡线程序写的
#include "Gan_Ctrl.h"
#include "Drv_OpenMV.h"
#include "Ano_OPMV_Ctrl.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "ANO_IMU.h"
#include "Ano_FlightCtrl.h"
#include "vl53l0x.h"
#include "Drv_usart.h"
#include "Ano_FlyCtrl.h"
//需要操作赋值的外部变量：
//全局变量：
_opmv_gan_ctrl_st opmv_gan_ctrl; //控制结构体
Ldc_gan_PID Gan_PID; //PID结构体
//参数设定：
#define FORWARD_VEL 12 //前进速度
#define Ganyspeed 6 //绕杆切向速度
#define Ganyawspeed 90 //绕杆角速度
static u8 step; //在分布控制中用作标志位
float Color=1; //标志位，可以实现正逆时针绕杆
unsigned char Uartsendbuff[7]={
0xaa,0xaf,0x05,0x01,0x06,0x03,0x68}; //给OPENMV发送要寻找第二个杆
void Ldc_pid_Init(void) //PID参数及其他参数的初始化
{
Gan_PID.Kp=20;
Gan_PID.Kd=5;
Gan_PID.Expect=500;
opmv_gan_ctrl.Ganyaw=0;
opmv_gan_ctrl.Gannumber=0;
opmv.gan.sta=0;
opmv_gan_ctrl.target_loss=1;
}
void Ldc_gan_Ctrl(void) //PID控制函数
{
Gan_PID.Out=0;
Gan_PID.Now=(float)opmv_gan_ctrl.Juli;
Gan_PID.P=Gan_PID.Now-Gan_PID.Expect;
Gan_PID.D=Gan_PID.Old-Gan_PID.Now;
Gan_PID.Out=Gan_PID.Kp*Gan_PID.P+Gan_PID.Kd*Gan_PID.D;
Gan_PID.Out =0.001f*Gan_PID.Out;
Gan_PID.Old =Gan_PID.Now;
}
void Ldcqiehuan(void) //这个函数用于控制旋转角度
{
static u8 x=0; //杆数目标志位
if(opmv_gan_ctrl.Gannumber==0)
{
opmv_gan_ctrl.Gannumber=1;
opmv_gan_ctrl.Ganyaw=imu_data.yaw+200*Color; //再旋转200度进入下个阶段
if(opmv_gan_ctrl.Ganyaw<-180) opmv_gan_ctrl.Ganyaw+= 360; //控制角度在+-180度
else if(opmv_gan_ctrl.Ganyaw>180) opmv_gan_ctrl.Ganyaw -= 360;
}
else if((opmv_gan_ctrl.Gannumber==1)&&( (opmv_gan_ctrl.Ganyaw-imu_data.yaw<35)&&(opmv_gan_ctrl.Ganyaw-imu_data.yaw>-35))) //判断条件比较宽泛
{
opmv_gan_ctrl.Gannumber=2 ;
opmv_gan_ctrl.Ganyaw=imu_data.yaw+220*Color; //再旋转220度
if(opmv_gan_ctrl.Ganyaw<-180) opmv_gan_ctrl.Ganyaw+= 360;
else if(opmv_gan_ctrl.Ganyaw>180) opmv_gan_ctrl.Ganyaw -= 360;
}
else if((opmv_gan_ctrl.Gannumber==2)&&( (opmv_gan_ctrl.Ganyaw-imu_data.yaw<35)&&(opmv_gan_ctrl.Ganyaw-imu_data.yaw>-35)))
{
opmv_gan_ctrl.Gannumber=3;
Usart3_Send(Uartsendbuff,7); //寻找第二个杆
}
else if(opmv_gan_ctrl.Gannumber==3)
{
if(x==0)
{
step=2;
x=1;
Color=-1;
opmv_gan_ctrl.Gannumber=0;
}
else
{
MyFlyCtrl(0x02,0,0); //降落
}
}
}
void Ldc_gan_StepProcedure(u8 *dT_ms) //分步控制函数
{
static u8 time=0;
switch (step)
{
case 0:{
//什么都不做
opmv_gan_ctrl.exp_yaw_pal_dps = 0;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
opmv_gan_ctrl.exp_velocity_h_cmps[1]=0;
time=0;
step=1;
}break;
case 1:{
if(opmv.gan.sta==1) //如果找到杆调节角度
{
opmv_gan_ctrl.exp_yaw_pal_dps=opmv.gan.ganx;
step=3; //执行第三步
}
else
{
if(opmv_gan_ctrl.Gannumber==0) //自旋
{
opmv_gan_ctrl.exp_yaw_pal_dps=(-15);
}
else
{
if(time<=100) //等待然后自旋，这里是为了保证绕杆过程中丢失可以重新找到杆
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps =(-15)*Color;
time=0;
}
}
}
}break;
case 2:{
//只有在两个杆中间转换的时候才会执行这里，内容是边自旋边后退，防止飘
opmv_gan_ctrl.exp_velocity_h_cmps[0]=-16;
opmv_gan_ctrl.exp_yaw_pal_dps=(-4);
if(time<200)
{
time+=*dT_ms;
}
else
{
time=0;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
step=0;
opmv_gan_ctrl.Gannumber=0;
}
}break;
case 3:{
//判断距离是否合适
if(opmv_gan_ctrl.Juli<=550&&opmv_gan_ctrl.Juli>=450)
{
step=4;
Ldcqiehuan(); //合适就开始绕杆
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
}
else
{
if(opmv_gan_ctrl.Gannumber==0)
{
opmv_gan_ctrl.exp_velocity_h_cmps[0] =FORWARD_VEL;
if(time<80)
{
time+=*dT_ms;
}
else
{
time=0;
}
}
else
{
step=4;
}
}
}break;
case 4:{
//如果角度合适就y方向平移
if((opmv.gan.ganx<=20)&&(opmv.gan.ganx>=-20))
{
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
opmv_gan_ctrl.exp_velocity_h_cmps[1] =Ganyspeed*Color;
if(time<300) //延时
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_velocity_h_cmps[1]=0;
step=5;
time=0;
}
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps =opmv.gan.ganx;
}
}break;
case 5:{
//改变航向角
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
opmv_gan_ctrl.exp_yaw_pal_dps=Ganyawspeed*Color;
if(time<250)
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps=0;
step=1; //回到第一步
time=0;
}
}break;
default:{
}break;
}
}
void Gan_Ctrl(u8 *dT_ms,u8 en) //放到线程中50ms执行一次
{
//开启控制
if(en)
{
Ldc_gan_Ctrl(); //首先设置PID
if(opmv.gan.sta == 1) //如果找到了杆
{
opmv_gan_ctrl.exp_yaw_pal_dps =opmv.gan.ganx; //进行角度调节
opmv_gan_ctrl.target_loss=0; //这个标志位可以控制灯
}
else
{
opmv_gan_ctrl.target_loss=1;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
opmv_gan_ctrl.exp_velocity_h_cmps[1] = 0;
opmv_gan_ctrl.exp_yaw_pal_dps =0;
}
if(opmv_gan_ctrl.Gannumber!=0) //等待首次位置调节好再执行
{
Ldcqiehuan();
}
Ldc_gan_StepProcedure(dT_ms); //分布控制
opmv_gan_ctrl.exp_yaw_pal_dps = LIMIT(opmv_gan_ctrl.exp_yaw_pal_dps,-90,90);//限幅
}
else //不动
{
opmv_gan_ctrl.exp_velocity_h_cmps[0] = 0;
opmv_gan_ctrl.exp_velocity_h_cmps[1] = 0;
opmv_gan_ctrl.exp_yaw_pal_dps = 0;
step=0;
}
}
本人水平有限，此代码仅供参考，如果要做找降落点的任务可以修Ldcqiehuan（）函数，大佬们有好的方法或者认为我的代码有可以优化的地方的欢迎留言，我变量的命名实在是比较随便。
最终效果可以看我在b站的视频:https://www.bilibili.com/video/BV1o64y1y7Em