/*
本程序完成2020年电赛试题主要内容
如果用户认为已经掌握该文件使用方法,请删除此文件,然后添加FollowLine.c文件
1.拿到了绿色的数据;
2.
*/
#include "FollowLine.h"
#include "HARDWARE_uart.h"
#include "stdbool.h"
#include "pid.h"
#include "timer_drv.h"
#include "myMath.h"
#include "gcs.h"
#include "sdk.h"
#include "stdbool.h"
//#include "program_ctrl.h"
extern Usart_t UsartGroup[Num_USART];
extern PIDInfo_t PIDGroup[emNum_Of_PID_List];
extern UAV_info_t g_UAVinfo;
bool FollowLine = false;
bool FollowApriTag = false;
bool is_set_round(float first_angle, float current_angle);
void proControl(int16_t Distance, int16_t Speed);
void TimeoutCheck(void);
void UpdateStatus(void);
void UpdateAction(float dt);
void UpdateButton(void);
void ActionHoldPoint(int8_t Err, int16_t HoldTime, FSMList_t NextAction);
bool ActionFormChange(int8_t HoldTime, FormType_t TargetFormType, FSMList_t NextAction);
void UpdateDebugInfo(void);
void HoldCurrentPostion(float dt);
void HoldYawAngle(float dt);
float update_yaw_info_in_360(void);
FollowManager_t FollowManager;
SonarManager_t SonarManager;
/*
|+X
|
|
+Y------------- -Y
|
|
|-X
*/
void Follow_Init()
{
FollowManager.ptrPIDInfoV = &PIDGroup[emPID_FolloLinePosVertically];
FollowManager.ptrPIDInfoH = &PIDGroup[emPID_FolloLinePosHorizontally];
FollowManager.ptrPIDInfoY = &PIDGroup[emPID_FollowSpdYaw];
FollowManager.ptrPIDInfoV->kp = 1.5f;
FollowManager.ptrPIDInfoH->kp = 1.5f;
FollowManager.ptrPIDInfoH->DeathArea = 3;
FollowManager.ptrPIDInfoV->DeathArea = 3;
PIDGroup[emPID_FolloLineSpdVertically].kp = 0.45f;
PIDGroup[emPID_FolloLineSpdVertically].ki = 0.13f;
PIDGroup[emPID_FolloLineSpdVertically].kd = 0.014f;
PIDGroup[emPID_FolloLineSpdHorizontally].kp = 0.45f;
PIDGroup[emPID_FolloLineSpdHorizontally].ki = 0.13f;
PIDGroup[emPID_FolloLineSpdHorizontally].kd = 0.014f;
PIDGroup[emPID_FolloLinePosVertically].desired = 60 / 2;
PIDGroup[emPID_FolloLinePosHorizontally].desired = 80 / 2;
FollowManager.ptrPIDInfoH->OutLimitHigh = 20;
FollowManager.ptrPIDInfoH->OutLimitLow = -20;
FollowManager.ptrPIDInfoV->OutLimitHigh = 20;
FollowManager.ptrPIDInfoV->OutLimitLow = -20;
FollowManager.CountDownNumMs = MAX_COUNTDOWN;
FollowManager.TargetAltitudeCM = TARGETALTITUDECM;
FollowManager.FrontOpenmvFramePtr = (OpenMVFrame_t *)UsartGroup[UART_A1].RxBuff;
FollowManager.GroundOpenmvFramePtr = (OpenMVFrame_t *)UsartGroup[UART_A3].RxBuff;
FollowManager.ptrUAVInfo = &g_UAVinfo;
P1DIR &= ~(1 << BIT1);
MAP_GPIO_setAsInputPinWithPullUpResistor(GPIO_PORT_P1, GPIO_PIN1);
P1DIR &= ~(1 << BIT4);
MAP_GPIO_setAsInputPinWithPullUpResistor(GPIO_PORT_P1, GPIO_PIN4);
FollowManager.ptrPIDInfoY->kp = 0.02f;
FollowManager.ptrPIDInfoY->ki = 0.001f;
FollowManager.ptrPIDInfoY->OutLimitLow = -0.5;
FollowManager.ptrPIDInfoY->OutLimitHigh = 0.5;
FollowManager.ptrPIDInfoY->desired = 160 / 2;
}
//以100hz的速度轮询 10ms
void UpdateCentControl(float dt)
{
//判断OpenMV返回的数据是否可用,有的时候OpenMV会返回无效数据
if (FollowManager.GroundOpenmvFramePtr->CentPoint.x1 > 200 || FollowManager.GroundOpenmvFramePtr->CentPoint.y1 > 200)
return;
//更新距离
FollowManager.distance = FollowManager.FrontOpenmvFramePtr->cnt1 | FollowManager.FrontOpenmvFramePtr->Target >> 8;
//更新按钮控制实践
UpdateButton();
//更新程控状态线
UpdateStatus();
//更新程控动作线
UpdateAction(dt);
}
float angle_temp_dt = 0;
//此函数只做状态判断和状态更新
void UpdateStatus()
{
uint16_t distance = 0;
static float pre_yaw_temp = 0;
static int cnt = 0;
//根据ActionList的内容,进入不同的状态
switch (FollowManager.ActionList)
{
//判断
case ActionWaitting:
//Do nothing;
break;
//倒计时状态
case ActionCountdown:
{
//倒计时,数据初始填充位于Follow_Init中
FollowManager.CountDownNumMs--;
//当倒计时结束时候,状态变更为ActionTakeOff
if (FollowManager.CountDownNumMs <= 0)
{
FollowManager.ActionList = ActionTakeOff;
}
}
break;
//自动起飞状态
case ActionTakeOff:
{
//自动起飞动作持续时间为5s(500 * 10ms = 5000ms = 5s),然后跳到ActionHoverStartPoint动作;
ActionHoldPoint(MAX_HOVER_ERR, 500, ActionHoverStartPoint);
}
break;
//悬停三秒开始前进并巡线
case ActionHoverStartPoint:
ActionHoldPoint(MAX_HOVER_ERR, 200, ActionFindLine);
// ActionHoldPoint(MAX_HOVER_ERR, 200, ActionFindLandSpace);
break;
case ActionFindLine:
// 开始前进
// 是否发现第一个圆形?
if (FollowManager.GroundOpenmvFramePtr->FormType == Cirle)
{
// 是
// 停止自旋
// 跳到下一个动作
FollowManager.ActionList = ActionCloseToCirle;
// FollowManager.ActionList = ActionResetAngle;
// FollowManager.ActionList = ActionTest;
}
break;
case ActionCloseToCirle:
//悬停在第一个圆形上
//动作:hold在第一个圆形上2秒
ActionHoldPoint(MAX_HOVER_ERR, 500, ActionGoForward);
break;
case ActionGoForward:
// 目标:保持黑色竖线端点在画面中心
// 向前走,并且控制PID进行左右调整
// 是否找到圆形降落点
// 是
// 跳到下一个动作
if (FollowManager.GroundOpenmvFramePtr->FormType == Cirle)
{
cnt++;
FollowManager.ActionList = ActionResetFind2Cirle;
// pre_yaw_temp = g_Attitude.yaw;
}
break;
case ActionResetFind2Cirle:
//向前移动一小段距离,寻找第二个圆形降落点
//悬停
//
if (FollowManager.GroundOpenmvFramePtr->FormType == Cirle)
{
cnt++;
FollowManager.ActionList = ActionResetFind2Cirle;
// pre_yaw_temp = g_Attitude.yaw;
}
ActionHoldPoint(MAX_HOVER_ERR, 200, ActionFindLandSpace);
break;
case ActionFindLandSpace:
// 是否发现降落点,这里识别下视的OpenMV的数据是否找到
// 是
// 切换到降落模式
if (FollowManager.GroundOpenmvFramePtr->FormType == Cirle)
{
FollowManager.ActionList = ActionPreLand;
}
break;
case ActionPreLand:
ActionHoldPoint(MAX_HOVER_ERR, 200, ActionLand);
break;
//自动降落状态倒计时结束以后,进入上锁动作
case ActionLand:
{
//倒计时逻辑
static int Cnt = MAX_TIMEOUT1;
if (Cnt-- < 0)
{
FollowManager.ActionList = ActionLock;
}
}
break;
//上锁动作
case ActionLock:
FollowManager.ActionList = ActionWaitting;
break;
default:
break;
}
}
extern float PIDGroup_desired_yaw_pos_tmp;
//只执行动作
void UpdateAction(float dt)
{
static float last_yaw = 0;
switch (FollowManager.ActionList)
{
//倒计时命令
case ActionWaitting:
//Do nothing7
break;
//自动起飞命令
case ActionTakeOff:
sdk_takeoff(80);
break;
//悬停命令
case ActionHoverStartPoint:
//起飞
{
}
break;
case ActionFindLine:
sdk_velocity_set(15.0,5);
break;
case ActionGoForward:
sdk_yaw_stop();
sdk_velociyt_x_set(15);
if (FollowManager.FrontOpenmvFramePtr->FormType == Pole)
{
HoldYawAngle(dt);
}
else
{
}
break;
case ActionTurnRound:
sdk_velocity_reset();
if (FollowManager.FrontOpenmvFramePtr->FormType == Pole)
{
HoldYawAngle(dt);
}
if (FollowManager.distance != 0xFF && FollowManager.distance != 20 && FollowManager.distance != 0)
{
if (FollowManager.distance > 65)
{
sdk_velociyt_x_set(15);
}
else if (FollowManager.distance < 35)
{
sdk_velociyt_x_set(-15);
}
else
{
sdk_velociyt_x_set(0);
}
}
sdk_velociyt_y_set(10);
break;
case ActionResetV:
sdk_velocity_reset();
break;
case ActionFindLandSpace:
sdk_velocity_reset();
sdk_velocity_set(10, 0);
break;
case ActionPreLand:
HoldCurrentPostion(dt);
break;
case ActionTest:
sdk_velociyt_x_set(15);
// sdk_yaw_stop();
// sdk_yaw_set(-180);
break;
case ActionGoLeft:
break;
case ActionGoBack:
break;
//自动降落
case ActionLand:
//降落命令
sdk_land();
HoldCurrentPostion(dt);
break;
//上锁动作
case ActionLock:
// g_UAVinfo.FMUflg->unlock = 0;
break;
default:
break;
}
}
void HoldCurrentPostion(float dt)
{
static float OldPos[2];
//对输入做LPF
//更新测量点
PIDGroup[emPID_FolloLinePosVertically].measured = FollowManager.GroundOpenmvFramePtr->CentPoint.y1;
PIDGroup[emPID_FolloLinePosHorizontally].measured = FollowManager.GroundOpenmvFramePtr->CentPoint.x1;
PIDGroup[emPID_FolloLineSpdVertically].measured = (FollowManager.GroundOpenmvFramePtr->CentPoint.y1 - OldPos[0]);
PIDGroup[emPID_FolloLineSpdHorizontally].measured = (FollowManager.GroundOpenmvFramePtr->CentPoint.x1 - OldPos[1]);
OldPos[0] = FollowManager.GroundOpenmvFramePtr->CentPoint.y1;
OldPos[1] = FollowManager.GroundOpenmvFramePtr->CentPoint.x1;
UpdatePID(FollowManager.ptrPIDInfoH, dt); //PID
UpdatePID(FollowManager.ptrPIDInfoV, dt); //PID
PIDGroup[emPID_FolloLineSpdVertically].desired = FollowManager.ptrPIDInfoV->out;
PIDGroup[emPID_FolloLineSpdHorizontally].desired = FollowManager.ptrPIDInfoH->out;
UpdatePID(&PIDGroup[emPID_FolloLineSpdHorizontally], dt); //PID
UpdatePID(&PIDGroup[emPID_FolloLineSpdVertically], dt); //PID
sdk_velocity_set(PIDGroup[emPID_FolloLineSpdVertically].out, PIDGroup[emPID_FolloLineSpdHorizontally].out);
}
bool ActionFormChange(int8_t HoldTime, FormType_t TargetFormType, FSMList_t NextAction)
{
static int cnt = 0;
bool ChangeFinished = false;
if (FollowManager.GroundOpenmvFramePtr->FormType == TargetFormType)
{
cnt++;
if (cnt > HoldTime)
{
cnt = 0;
FollowManager.ActionList = NextAction;
ChangeFinished = true;
}
}
else
{
cnt = 0;
}
return ChangeFinished;
}
void ActionHoldPoint(int8_t Err, int16_t HoldTime, FSMList_t NextAction)
{
static bool Enter = true;
static uint16_t CountDown = 0;
if (Enter)
{
CountDown = HoldTime;
Enter = false;
}
else
{
CountDown--;
}
if (CountDown == 0)
{
Enter = true;
FollowManager.ActionList = NextAction;
}
}
void UpdateButton()
{
//判定两个输入是否有效,其实是判断左右两个按键
volatile static uint8_t input = 0;
volatile static uint8_t input2 = 0;
input = P1IN & BIT1;
input2 = P1IN & BIT4;
//判断巡线按钮是否按下
if (input)
{
}
else
{
FollowLine = true;
}
//判断寻找ApriTag按钮是否按下
if (input2)
{
}
else
{
FollowApriTag = true;
}
//判断当前是否被多按
if (FollowApriTag == false && FollowLine == false)
{
return;
}
else
{
static bool CloseGate = true;
//动作线进入倒计时状态
if (CloseGate)
{
CloseGate = false;
FollowManager.ActionList = ActionCountdown;
}
}
}
extern float PIDGroup_desired_yaw_pos_tmp;
void HoldYawAngle(float dt)
{
//更新测量点
PIDGroup[emPID_FollowSpdYaw].measured = FollowManager.FrontOpenmvFramePtr->CentPoint.x1;
UpdatePID(FollowManager.ptrPIDInfoY, dt);
// PIDGroup_desired_yaw_pos_tmp = -FollowManager.ptrPIDInfoY->out;
sdk_yaw_little(FollowManager.ptrPIDInfoY->out);
// sdk_yaw_little(FollowManager.ptrPIDInfoY->out);
}
float update_yaw_info_in_360()
{
float angle = 0;
angle = g_Attitude.yaw;
if (angle < 0)
{
angle += 360;
}
return angle;
}
bool is_set_round(float first_angle, float current_angle)
{
static uint8_t step = 0;
bool result = false;
switch (step)
{
case 0:
//waiting
step = 1;
break;
case 1:
//mark
step = 2;
break;
case 2:
if (absFloat(current_angle - first_angle) / 2 > 90)
{
step = 3;
}
break;
case 3:
if (absFloat(current_angle - first_angle) / 2 < 5)
{
step = 0;
result = true;
}
break;
default:
break;
}
return result;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)