Search and rescue strategy of underwater robot (FISH)

background

Graduate training courses 《 Underwater robot training 》 The final assessment is the robot fish competition , The rules of the game are as follows ：
The pool is designed as follows ：

It is necessary to control the fish to swim from the initial position to avoid obstacles to the ball position , Then push the ball back to the original position of the fish , Time to finish , Short time, high score , Duration not exceeding 5 minute .

This group's rough strategy

//Purpose:  Fish avoid obstacles from the starting point to the ball , And dribble to the starting point （ One obstacle ）
//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;
}

// following id And fish information 、 Fish action information array correlation 

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)// Performing original turn mode 
	{
    
		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[])
{
    
	// Initialize action parameters 
	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;// The coordinates of the center point of the fish 
	f_pt=m_FishInfo[0].GetCenterPoint();//GetCenterPt() Returns the coordinates of the center point 

	double f_dir;// The direction of the fish ,[-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;// Coordinates of the center point of the ball 
	b_pt=m_goalinfo.GetBallPoint();

	CPoint g_pt;// The coordinates of the center point of the goal //distance
	g_pt=m_Channel[0].center;

    CPoint start;// The starting point 
    start.x = 562;
    start.y = 100;


	bool m_right;
	if (start.x>312)
		m_right = true;
	else
		m_right = false;

    if(m_right)
	{
    
        if(f_pt.x>g_pt.x&&b_pt.x<g_pt.x)// Fish in B Zone and ball in A District 
        {
    
            temp0.x = g_pt.x-50;// The target point of the fish in the first and third stages is the goal , The temporary target point is a little to the left of the goal 
            temp0.y = g_pt.y;

            //********** Swim through the goal at full speed **********
			m_action[0].mode=0;
			m_action[0].speed=14;
			Roundp2p1(temp0,m_action[0],0,m_FishInfo[0]);// Full speed to the goal 
        }

        else if(f_pt.x<g_pt.x&&b_pt.x<g_pt.x)// Fish in A Zone and ball in A District 
        {
    
            g_pt.x = g_pt.x + 50;

            double dirfishtoball;
            dirfishtoball = this->Angle(f_pt, b_pt);
            double disfishtoball;
            disfishtoball = this->Distance(f_pt, b_pt);
            double dirballtogoal;
            dirballtogoal = this->Angle(b_pt, g_pt);
            double dirgoaltoball;
            dirgoaltoball = this->Distance(g_pt, b_pt);

            dirfishtoball -= f_dir;
            dirfishtoball = this->Checkangle(dirfishtoball);
            double dir = dirfishtoball * 180 / PI;

			double Cf_dir=this->Checkangle(f_dir);

			double r = 6;

            temp11.x = b_pt.x - r*cos(dirballtogoal);// The goal of the fish in the second stage is to head the ball over the goal , The temporary target point is the heading position 
            temp11.y = b_pt.y - r*sin(dirballtogoal);

			temp12.x = b_pt.x - r - 30;// The temporary target point when the fish is on the right side of the ball 
            temp12.y = b_pt.y - r - 40;

            if(f_headpt.x<b_pt.x+r)// The fish is on the top left of the ball , Start heading 
            {
    	
                if(disfishtoball<80)// Low speed approach 
                {
    	
                    m_action[0].mode=0;
                    m_action[0].speed=4;
                }
                else// High speed propulsion 
                {
    
                    m_action[0].mode=0;
                    m_action[0].speed=8;
                }
                Roundp2p1(temp11,m_action[0],0,m_FishInfo[0]);
            }         
			else
			{
    
				m_action[0].mode=0;
                m_action[0].speed=14;
				Roundp2p1(temp12,m_action[0],0,m_FishInfo[0]);
			}
        }

        else if(f_pt.x<g_pt.x&&b_pt.x>g_pt.x)// Fish in A Zone and ball in B District 
        {
    
            temp0.x = g_pt.x+50;// The target point of the fish in the first and third stages is the goal , The temporary target point is a little to the right of the goal 
            temp0.y = g_pt.y;

            //********** Full speed to the goal **********
			m_action[0].mode=0;
			m_action[0].speed=12;
			Roundp2p1(temp0,m_action[0],0,m_FishInfo[0]);// Full speed to the goal 
        }

        else if(f_pt.x>g_pt.x&&b_pt.x>g_pt.x)// Fish in B Zone and ball in B District 
        {
    
            //********** The original heading in strategy speed down version **********
            double dirfishtoball;
            dirfishtoball = this->Angle(f_pt, b_pt);
            double disfishtoball;
            disfishtoball = this->Distance(f_pt, b_pt);
            double dirballtostart;
            dirballtostart = this->Angle(b_pt, start);
            double dirstarttoball;
            dirstarttoball = this->Distance(start, b_pt);

            dirfishtoball -= f_dir;
            dirfishtoball = this->Checkangle(dirfishtoball);
            double dir = dirfishtoball * 180 / PI;

            double r=6;
            temp211.x = b_pt.x - r*sin(dirballtostart);// The goal of the fish in the fourth stage is to head the ball into the starting point , The temporary target point is the heading position 
            temp211.y = b_pt.y + r*cos(dirballtostart);
			temp212.x = b_pt.x + r*sin(dirballtostart);// The goal of the fish in the fourth stage is to head the ball into the starting point , The temporary target point is the heading position 
            temp212.y = b_pt.y + r*cos(dirballtostart);
			temp22.x = b_pt.x - r - 30;// Temporary target point when the fish is on the upper side 
			temp22.y = b_pt.y + r + 40;

			
            if (f_headpt.y>b_pt.y + r)// The fish is under the ball , Direct header 
            {
    
                if (disfishtoball<80)//the velosity change easy to approach
                {
    
                    m_action[0].mode = 0;
                    m_action[0].speed = 4;
                }
                else
                {
    
                    m_action[0].mode = 0;
                    m_action[0].speed = 8;
                }
				if (start.x>b_pt.x)// The ball is to the left of the starting point 
				{
    
					Roundp2p1(temp211, m_action[0], 0, m_FishInfo[0]);
				}
				else// The ball is to the right of the starting point 
				{
    
					Roundp2p1(temp212, m_action[0], 0, m_FishInfo[0]);
				}
            }
            else// The fish is on the ball , Swim to the lower temporary point first 
			{
    
				m_action[0].mode = 0;
				m_action[0].speed = 14;
				Roundp2p1(temp22, m_action[0], 0, m_FishInfo[0]);
			}
        }
    }
    return TRUE;
}


double CStrategy::Distance(CPoint point,CPoint aimer)
{
    
	double a;
	a=sqrt((double)(point.x-aimer.x)*(point.x-aimer.x)+(double)(point.y-aimer.y)*(point.y-aimer.y));
	return a;
}
double CStrategy::Checkangle(double dir)// hold dir The conversion [-pi~pi] Number between 
{
    
	if(dir>PI)
		dir-=2*PI;
	else if(dir<-PI)
		dir+=2*PI;
	return dir;
}
void CStrategy::Roundp2p(CPoint aimpt,CFishAction &action,int fishID,CFishInfo m_FishInfo)
{
    
	CPoint FishPt;//  The position of the center of the fish 
	double FishDir;//  The direction of the fish head 
	CPoint FishHeadPt;// The position of the fish head 
	CPoint LastFishPt;
	CPoint FishRotatePt;// The rotation position of the fish 
	CPoint FishTailPt;// The position of the fish tail  
	CPoint GoalPt1, GoalPt2;// Temporary target point ,GoalPt2 Not used 
	CPoint Pt1, Pt2, Pt3, Pt4;
	//CPoint f_headpt;//point of fish's head
	//f_headpt=m_FishInfo[0].GetHeadPoint();

	double dir1=0;
	double dir0;
	double dist0;
	double dist1=0;
	double dir2=0;
	double dist2=0;// Directions and distances for temporary use 
	CPoint centerpt1,centerpt2;// It is used as the center of the fish  
	CPoint centerpt3,centerpt4,centerpt5,centerpt6;
	CPoint centerpt0,centerpt00;
	double dis1,dis2;
	dis1=0;
	dis2=0;		 
	double radius;/// Need to swim radius , It can be set at will .
	FishPt=m_FishInfo.GetCenterPoint();
	FishDir=m_FishInfo.GetDirection();
	dist1=Distance(FishPt,aimpt);
	dir1=Angle(FishPt,aimpt);
	dir1-=FishDir;
	dir1=Checkangle(dir1)*180/PI;
	action.speed=15;
	m_FishInfo.SetAction(action);
	if(dist1>100)
	{
    
		if(dir1>-5&&dir1<5)
			action.direction=7;
		else if(dir1<-5&&dir1>-10)
			action.direction=5;
		else if(dir1<-10&&dir1>-30)
			action.direction=4;
		else if(dir1<-30&&dir1>-50)
			action.direction=2;
		else if(dir1<-50&&dir1>-70)
			action.direction=1;
		else if(dir1<-70&&dir1>-90)
			action.direction=0;
		else if(dir1<-90)
			action.direction=0;
		else if(dir1>5&&dir1<10)
			action.direction=9;
		else if(dir1>10&&dir1<20)
			action.direction=10;
		else if(dir1>20&&dir1<40)
			action.direction=12;
		else if(dir1>40&&dir1<50)
			action.direction=12;
		else if(dir1>50&&dir1<80)
			action.direction=14;
		else if(dir1>80&&dir1<90)
			action.direction=14;
		else 
			action.direction=14;
	}

	else 
	{
    
		if(dir1>-5&&dir1<5)
			action.direction=7;
		else if(dir1<-5&&dir1>-20)
			action.direction=4;
		else if(dir1<-20&&dir1>-40)
			action.direction=1;
		else if(dir1<-40&&dir1>-60)
			action.direction=0;
		else if(dir1<-60&&dir1>-70)
			action.direction=0;
		else if(dir1<-70&&dir1>-90)
			action.direction=0;
		else if(dir1<-120)
			action.direction=0;
		else if(dir1>5&&dir1<20)
			action.direction=10;
		else if(dir1>20&&dir1<30)
			action.direction=12;
		else if(dir1>30&&dir1<50)
			action.direction=13;
		else if(dir1>50&&dir1<70)
			action.direction=14;
		else if(dir1>70&&dir1<90)
			action.direction=14;
		else if(dir1<120)
			action.direction=14;
		else 
			action.direction=14;

	}
	m_FishInfo.SetAction(action);
}
void CStrategy::con2tempt(CPoint aimpt,CFishAction m_action[],CFishInfo m_FishInfo[])// Swim to a temporary point and exit 
{
    
	// Get information about fish 
	CPoint f_pt;// The coordinates of the center point of the fish 
	f_pt=m_FishInfo[0].GetCenterPoint();//GetCenterPt() Returns the coordinates of the center point 
	double f_dir;// The direction of the fish ,[-PI,PI]
	f_dir=m_FishInfo[0].GetDirection();

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


	double disheadtoaimpt;
	disheadtoaimpt=this->Distance(f_headpt,aimpt);

	bool tem=true;

	if(tem)
	{
    
		Roundp2p(aimpt,m_action[0],0,m_FishInfo[0]);
		if(disheadtoaimpt<10)
			tem=false;
	}
}


double CStrategy::Angle(CPoint point,CPoint aimer)
{
    	
	double a;
	a=atan2((double)(aimer.y-point.y),(double)(aimer.x-point.x));
	return a;
	//return point.x;
}
void CStrategy::Point2point(CPoint aimpt,CFishAction &action, int fishID, CFishInfo m_FishInfo)
{
    
	CPoint FishPt;//  The position of the center of the fish 
	double FishDir;//  The direction of the fish head 
	CPoint FishHeadPt;// The position of the fish head 
	CPoint LastFishPt;
	CPoint FishRotatePt;// The rotation position of the fish 
	CPoint FishTailPt;// The position of the fish tail  
	CPoint GoalPt1, GoalPt2;// Temporary target point ,GoalPt2 Not used 
	CPoint Pt1, Pt2, Pt3, Pt4;
	double dir1=0;
	double dir0;
	double dist0;
	double dist1=0;
	double dir2=0;
	double dist2=0;// Directions and distances for temporary use 
	CPoint centerpt1,centerpt2;// It is used as the center of the fish  
	CPoint centerpt3,centerpt4,centerpt5,centerpt6;
	CPoint centerpt0,centerpt00;
	double dis1,dis2;
	dis1=0;
	dis2=0;		 
	double radius;/// Need to swim radius , It can be set at will .
	FishPt=m_FishInfo.GetCenterPoint();
	FishDir=m_FishInfo.GetDirection();
	dist1=Distance(FishPt,aimpt);
	dir1=Angle(FishPt,aimpt);
	dir1-=FishDir;
	dir1=Checkangle(dir1)*180/PI;
	action.speed=15;	
	if(dir1>-5&&dir1<5)
		action.direction=7;
	else if(dir1<-5&&dir1>-10)
		action.direction=5;
	else if(dir1<-10&&dir1>-30)
		action.direction=4;
	else if(dir1<-30&&dir1>-50)
		action.direction=2;
	else if(dir1<-50&&dir1>-70)
		action.direction=1;
	else if(dir1<-70&&dir1>-90)
		action.direction=0;
	else if(dir1<-90)
		action.direction=0;
	else if(dir1>5&&dir1<10)
		action.direction=9;
	else if(dir1>10&&dir1<20)
		action.direction=10;
	else if(dir1>20&&dir1<40)
		action.direction=12;
	else if(dir1>40&&dir1<50)
		action.direction=12;
	else if(dir1>50&&dir1<80)
		action.direction=14;
	else if(dir1>80&&dir1<90)
		action.direction=14;
	else 
		action.direction=14;
}

effect

The control is rough , And there are BUG, The actual effect depends on luck , As soon as possible 47 Seconds to complete the task , The slowest will be more than 5 minute .