当前位置：网站首页>Search and rescue strategy of underwater robot (FISH)
Search and rescue strategy of underwater robot (FISH)
2022-06-12 07:47:00 【Jace Lee】
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 .
边栏推荐
- R language uses neuralnet package to build neural network regression model (feedforward neural network regression model) and calculate MSE value (mean square error) of the model on the test set
- Voice assistant - overall architecture and design
- 鸿蒙os-第一次培训
- R语言使用RStudio将可视化结果保存为pdf文件（export--Save as PDF）
- AcWing——4268. Sexy element
- 謀新局、促發展，桂林綠色數字經濟的頭雁效應
- AI fanaticism ｜ come to this conference and work together on the new tools of AI!
- The first demand in my life - batch uploading of Excel data to the database
- 20220525 RCNN---＞Faster RCNN
- What is a good recommendation system?
猜你喜欢
Process terminated
Exploring shared representations for personalized federated learning paper notes + code interpretation
二、八、十、十六进制相互转换
Gd32f4 (5): gd32f450 clock is configured as 200m process analysis
Support vector machine (SVM)
Leverage contextual information
Some summaries of mathematical modeling competition in 2022
![[tutorial] deployment process of yolov5 based on tensorflow Lite](/img/d0/c38f27ad76b62b27cdeb68728e9c8c.jpg)
[tutorial] deployment process of yolov5 based on tensorflow Lite
L'effet de l'oie sauvage sur l'économie numérique verte de Guilin
Logistic regression
随机推荐
AcWing——4268. Sexy element
Voice assistant - Introduction and interaction process
Summary of machine learning + pattern recognition learning (II) -- perceptron and neural network
‘CMRESHandler‘ object has no attribute ‘_ timer‘，socket. gaierror: [Errno 8] nodename nor servname pro
Formatting the generalization forgetting trade off in continuous learning
Continuous local training for better initialization of Federated models
R language uses rstudio to save visualization results as PDF files (export--save as PDF)
Rnorm function of R language generates positive distribution data, calculates descriptive statistical summary information of vector data using sum function of epidisplay package, and visualizes ordere
[yolo-v5 learning notes]
Connect to the database but cannot modify the data
Summary of machine learning + pattern recognition learning (IV) -- decision tree
R language dplyr package mutate_ At function and one_ The of function converts the data type of a specified data column (specified by a vector) in dataframe data to a factor type
2021.10.29-30 scientific research log
R语言caTools包进行数据划分、scale函数进行数据缩放、class包的knn函数构建K近邻分类器、比较不同K值超参数下模型准确率(accuracy)
Voice assistant -- Architecture and design of Instruction Assistant
Web page performance optimization interview questions
移动端、安卓、IOS兼容性面试题
Leverage contextual information
Topic 1 Single_Cell_analysis（2）
The Poisson regression model (posion) is constructed by GLM function of R language, and the poisgof function of epidisplay package is used to test the goodness of fit of the fitted Poisson regression