水中机器人（鱼）搜救策略

背景

研究生实训课程《水中机器人实训》结课考核为机器鱼比赛，比赛规则如下：
池子设计如下：

需控制鱼从初始位置避让障碍游到球位置，然后将球顶回鱼的初始位置，计时完成，时间短得分高，时长不超过5分钟。

所编写的策略的思想

对机器鱼的控制分为以下四个阶段来进行：

1. 以障碍为界，分为左右两侧。球在左侧，鱼在右侧时，定义为初始阶段，此时鱼的任务是避开障碍游到左侧。因此本阶段鱼的目标位置为障碍开口位置的中心点。本控制算法中使用简便方法，即将障碍定义为一个偏移的球门，球门中心位置即障碍开口的中心点，因此在此阶段可以直接使用现有的鱼游动控制程序，目标位置为球门；
2. 球在左侧，鱼在左侧时，为顶球过门阶段。此阶段又分为两个子阶段：
   a.球在鱼左侧，这时鱼不可直接顶球，需要控制鱼游到球的左侧临时目标位置。而临时目标位置不能直接设为球的正左侧一定距离，因为若鱼与球y坐标相同时，鱼仍然会顶到球。因此本控制程序将这个临时目标点设为球的左上角；
   b.球在鱼右侧，这时可以直接顶球，并可以直接调用鱼顶球过门的程序，球门点设为障碍物开口中心点。
3. 球在右侧，鱼在左侧时，与初始阶段相同，鱼的目标仍为穿过障碍物，目标点为障碍开口的中心点；
4. 球在右侧，鱼在右侧时，为顶球到初始位置阶段。此阶段类似于顶球过门阶段，又分为两个子阶段：
   a.球在鱼下侧，这时鱼不可直接顶球，需要控制鱼游到球的下侧临时目标位置。而临时目标位置不能直接设为球的正下侧一定距离，因为若鱼与球x坐标相同时，鱼仍然会顶到球。因此本控制程序将这个临时目标点设为球的左下角；
   b.球在鱼上侧，这时可以直接顶球，并可以直接调用鱼顶球过门的程序，球门点设为初始位置。

本组粗糙的策略

//Purpose: 鱼从起始点避障至球处，并运球避障至与起始点（一个障碍）
//Author:Jace, cheng
//Data:2022.1.7
#include "../Header/StdAfx.h"
#include "../Header/Strategy.h"

CStrategy::CStrategy(void)
{
   
	m_StatePeriod=15;
	m_TurnYuan=0;
}

CStrategy::~CStrategy(void)
{
   
}

BOOL CStrategy::Strategy(IplImage* imgOrig,IplImage* imgRecog ,CFishAction action[], int n_action, CFishInfo fishinfo[], int n_fishinfo,
			  CBallInfo ballinfo[], int n_ballinfo, OBSTAINFO obst[],int n_obst, CHANNEL channel[], int n_channel)
{
   
	return TRUE;
}

//以下id及鱼信息、鱼动作信息数组相关

void CStrategy::BasicActionStop(CFishInfo fish[],CFishAction action[],int id)
{
   
	action[id].speed=0;
	action[id].direction=7;
	action[id].mode=0;
	action[id].state++;
	if(action[id].state==m_StatePeriod)
	{
   
		action[id].state=0;
		m_TurnYuan=0;
	}
	fish[id].SetAction(action[id]);
}

void CStrategy::BasicActionGo(CFishInfo fish[],CFishAction action[],int id,int speed,int direction,int mode)
{
   
	action[id].speed=speed;
	action[id].direction=direction;
	action[id].mode=mode;

	if(m_TurnYuan)//正在执行原始转弯模式
	{
   
		action[id].state++;
		if(action[id].state>=m_StatePeriod)
		{
   
			action[id].state=0;
			m_TurnYuan=0;
		}
	}
	else
	{
   
		if(mode)
		{
   
			action[id].state=0;
			m_TurnYuan=mode;
		}
		else
		{
   
			action[id].state++;
			if(action[id].state>=m_StatePeriod)
				action[id].state=0;
		}		
	}
	fish[id].SetAction(action[id]);
}

BOOL CStrategy::Strategy0(CFishAction m_action[], CFishInfo m_FishInfo[], CBallInfo  &m_goalinfo, OBSTAINFO m_obst[], CHANNEL m_Channel[])
{
   
	//初始化动作参数
	m_action[0].state = 0;
	m_action[0].mode = 0;

	CPoint temp0,temp2;
	CPoint temp11;
	CPoint temp12;
	CPoint temp211;
	CPoint temp212;
	CPoint temp22;

	CPoint f_pt;//鱼的中心点坐标
	f_pt=m_FishInfo[0].GetCenterPoint();//GetCenterPt()返回中心点坐标

	double f_dir;//鱼的方向，[-PI,PI]
	f_dir=m_FishInfo[0].GetDirection();

	CPoint f_headpt;//point of fish's head
	f_headpt=m_FishInfo[0].GetHeaderPoint();

	CPoint b_pt;//球的中心点坐标
	b_pt=m_goalinfo.GetBallPoint();

	CPoint g_pt;//球门中心点坐标//distance
	g_pt=m_Channel[0].center;

    CPoint start;//起始点
    start.x = 562;
    start.y = 100;


	bool m_right;
	if (start.x>312)
		m_right