📄 模糊控制仿真view.cpp
字号:
// 模糊控制仿真View.cpp : implementation of the CMyView class
//
#include "stdafx.h"
#include "模糊控制仿真.h"
#include "模糊控制仿真Doc.h"
#include "模糊控制仿真View.h"
#include "math.h"
#include "SomeOption.h"
#define Rad (Pi/180)
#define Pi 3.1415926
#define MoveVmax 120
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CMyView
IMPLEMENT_DYNCREATE(CMyView, CView)
BEGIN_MESSAGE_MAP(CMyView, CView)
//{{AFX_MSG_MAP(CMyView)
ON_WM_TIMER()
ON_COMMAND(ID_CONTROL_START, OnControlStart)
ON_COMMAND(ID_CONTROL_STOP, OnControlStop)
//}}AFX_MSG_MAP
// Standard printing commands
ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CMyView construction/destruction
extern RobotInformation Robot;
CMyView::CMyView()
{
// TODO: add construction code here
m_nTimer=80;
newPoint.x=10;
newPoint.y=130;
//fRobot.fTargetX=(float)Robot.nCenter.x; //保存浮点初值,象素值
//fRobot.fTargetY=(float)Robot.nCenter.y;
fRobot.fTargetX=(float)10;
fRobot.fTargetY=(float)110;
MoveVmax1=70;
}
CMyView::~CMyView()
{
}
BOOL CMyView::PreCreateWindow(CREATESTRUCT& cs)
{
// TODO: Modify the Window class or styles here by modifying
// the CREATESTRUCT cs
return CView::PreCreateWindow(cs);
}
/////////////////////////////////////////////////////////////////////////////
// CMyView drawing
void CMyView::OnDraw(CDC* pDC)
{
CMyDoc* pDoc = GetDocument();
ASSERT_VALID(pDoc);
// TODO: add draw code for native data here
DrawField(pDC);
DrawRobot(pDC);
DrawHopePath(pDC);
}
/////////////////////////////////////////////////////////////////////////////
// CMyView printing
BOOL CMyView::OnPreparePrinting(CPrintInfo* pInfo)
{
// default preparation
return DoPreparePrinting(pInfo);
}
void CMyView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
// TODO: add extra initialization before printing
}
void CMyView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
// TODO: add cleanup after printing
}
/////////////////////////////////////////////////////////////////////////////
// CMyView diagnostics
#ifdef _DEBUG
void CMyView::AssertValid() const
{
CView::AssertValid();
}
void CMyView::Dump(CDumpContext& dc) const
{
CView::Dump(dc);
}
CMyDoc* CMyView::GetDocument() // non-debug version is inline
{
ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CMyDoc)));
return (CMyDoc*)m_pDocument;
}
#endif //_DEBUG
/////////////////////////////////////////////////////////////////////////////
// CMyView message handlers
void CMyView::DrawField(CDC *pDC)
{
CRect rect;
GetClientRect(rect);
CBrush brush(RGB(0,126,0));
pDC->FillRect(rect,&brush);
}
void CMyView::DrawRobot(CDC *pDC)
{
CPoint rbpt[4];
CPoint rbcenter;
CPoint rb1,rb2,rb3,rb4;
float rbtheta;
rbcenter=Robot.nCenter;
rbtheta=Robot.Theta;
GetRobotPeak(rbcenter,rbtheta,rbpt);
rb1.x=(rbpt[0].x+rbpt[1].x)/2;
rb1.y=(rbpt[0].y+rbpt[1].y)/2;
rb2.x=(rbpt[2].x+rbpt[3].x)/2;
rb2.y=(rbpt[2].y+rbpt[3].y)/2;
rb3.x=((rbpt[0].x+rbpt[3].x)/2+rbpt[0].x)/2;
rb3.y=((rbpt[0].y+rbpt[1].y)/2+rbpt[0].y)/2;
rb4.x=((rbpt[1].x+rbpt[2].x)/2+rbpt[1].x)/2;
rb4.y=((rbpt[0].y+rbpt[1].y)/2+rbpt[1].y)/2;
pDC->Arc(rbcenter.x-3,rbcenter.y-3,rbcenter.x+3,rbcenter.y+3,rbcenter.x-3,rbcenter.y-3,rbcenter.x-3,rbcenter.y-3);
//************Draw Robot********************
pDC->MoveTo(rbpt[0]);
pDC->LineTo(rbpt[1]);
pDC->LineTo(rbpt[2]);
pDC->LineTo(rbpt[3]);
pDC->LineTo(rbpt[0]);
pDC->MoveTo(rb2);
pDC->LineTo(rb1);
pDC->MoveTo(rb3);
pDC->LineTo(rb1);
pDC->LineTo(rb4);
}
void CMyView::GetRobotPeak(CPoint center, float theta, CPoint pt4[], float side)
{
pt4[0]=center; pt4[1]=center;
pt4[2]=center; pt4[3]=center;
pt4[0].Offset(int(side*0.707*cos((45-theta)*Rad)),int(-side*0.707*sin((45-theta)*Rad)));
pt4[1].Offset(int(side*0.707*cos((45+theta)*Rad)),int(side*0.707*sin((45+theta)*Rad)));
pt4[2].Offset(int(-side*0.707*cos((45-theta)*Rad)),int(side*0.707*sin((45-theta)*Rad)));
pt4[3].Offset(int(-side*0.707*cos((45+theta)*Rad)),int(-side*0.707*sin((45+theta)*Rad)));
}
void CMyView::DrawHopePath(CDC *pDC)
{
CPen pen,*oldpen;
pen.CreatePen(PS_SOLID,2,RGB(255,255,255));
oldpen=pDC->SelectObject(&pen);
int hopespeed=70; // 70cm/s
pDC->MoveTo(newPoint);
newPoint.x=newPoint.x+hopespeed*m_nTimer*2/1000;
newPoint.y=130;
pDC->LineTo(newPoint.x,newPoint.y);
pDC->SelectObject(&oldpen);
}
void CMyView::OnTimer(UINT nIDEvent)
{
// TODO: Add your message handler code here and/or call default
CClientDC pDC(this);
switch(nIDEvent)
{
case 0:
//DrawRobotPath(&pDC);
//*************************************************
DataBase(Robot.nCenter,Robot.Theta);
int deltaA;
deltaA=RuleBase(0,FEy,FEh);
int DesireJiao;
DesireJiao=GetDesireJiao(Robot.Theta,deltaA);
Move(DesireJiao);
GetNextPosure(Robot.RobotVL,Robot.RobotVR);
//*************************************************
DrawRobot(&pDC);
DrawHopePath(&pDC);
break;
}
CView::OnTimer(nIDEvent);
}
void CMyView::OnControlStart()
{
// TODO: Add your command handler code here
SetTimer(0,100,NULL);
}
void CMyView::OnControlStop()
{
// TODO: Add your command handler code here
KillTimer(0);
}
void CMyView::OnUpdate(CView* pSender, LPARAM lHint, CObject* pHint)
{
// TODO: Add your specialized code here and/or call the base class
switch(lHint)
{
case 1:
SendMessage(WM_COMMAND,ID_CONTROL_START);
break;
case 2:
SendMessage(WM_COMMAND,ID_CONTROL_STOP);
break;
default:
Invalidate();
}
}
void CMyView::DrawRobotPath(CDC *pDC)
{
float RobotSpeed=70.0; // 70cm/s
float Distance;
Distance=RobotSpeed*m_nTimer*2/1000;
Robot.nCenter.x+=Distance*cos(Robot.Theta);
Robot.nCenter.y+=Distance*sin(Robot.Theta);
}
void CMyView::DataBase(CPoint point, float theta)
{
//********Y Direction*********
if(point.y-130<=Ey_NB) FEy=Ey_NB;
else if(point.y-130>Ey_NB && point.y-130<=Ey_NM) FEy=Ey_NM;
else if(point.y-130>Ey_NM && point.y-130<=Ey_NS) FEy=Ey_NS;
else if(point.y-130>Ey_NS && point.y-130<=Ey_NZ) FEy=Ey_NZ;
else if(point.y-130>Ey_NZ && point.y-130<=Ey_PZ) FEy=Ey_PZ;
else if(point.y-130>Ey_PZ && point.y-130<Ey_PS) FEy=Ey_PZ;
else if(point.y-130>=Ey_PS && point.y-130<Ey_PM) FEy=Ey_PS;
else if(point.y-130>=Ey_PM && point.y-130<Ey_PB) FEy=Ey_PM;
else FEy=Ey_PB;
//*******Turn Direction********
theta=AlterAngle(theta);
if((theta-0)<=(360-Eh_NB) && theta-0>320) FEh=Eh_NB;
else if((theta-0)>(360-Eh_NB) && theta-0<=360-Eh_NM) FEh=Eh_NM;
else if((theta-0)>(360-Eh_NM) && theta-0<=360-Eh_NS) FEh=Eh_NS;
else if((theta-0)>(360-Eh_NS) && theta-0<360) FEh=Eh_ZO;
else if((theta-0)>=0 && theta-0<Eh_PS) FEh=Eh_ZO;
else if((theta-0)>=Eh_PS && theta-0<Eh_PM) FEh=Eh_PS;
else if((theta-0)>=Eh_PM && theta-0<Eh_PB) FEh=Eh_PM;
else FEh=Eh_PB;
//********X Direction**********
if(point.x-newPoint.x<=Ex_NB) FEx=Ex_NB;
else if((point.x-newPoint.x)>Ex_NB && (point.x-newPoint.x)<=Ex_NS) FEx=Ex_NS;
else if((point.x-newPoint.x)>Ex_NS && (point.x-newPoint.x)<Ex_PS) FEx=Ex_ZO;
else if((point.x-newPoint.x)>=Ex_PS && (point.x-newPoint.x)<Ex_PB) FEx=Ex_PS;
else FEx=Ex_PB;
}
float CMyView::AlterAngle(float degree)
{
if(degree>=360)
{
degree-=360;
}
if(degree<0)
{
degree+=360;
}
return degree;
}
int CMyView::RuleBase(int deltaX, int deltaY, int deltaH)
{
int H;
if((deltaH==Eh_NB || deltaH==Eh_NM) && (deltaY==Ey_NB || deltaY==Ey_NM)) H=RB;
else if((deltaH==Eh_NB || deltaH==Eh_NM) && deltaY==Ey_NS) H=RB;
else if(deltaH==Eh_NB && deltaY==Ey_NZ) H=RB;
else if(deltaH==Eh_NB && deltaY==Ey_PZ) H=RB;
else if(deltaH==Eh_NS && (deltaY==Ey_NB || deltaY==Ey_NM)) H=RB;
else if(deltaH==Eh_ZO && deltaY==Ey_NB) H=RB;
else if(deltaH==Eh_PS && deltaY==Ey_NB) H=RM;
else if(deltaH==Eh_ZO && deltaY==Ey_NM) H=RM;
else if(deltaH==Eh_NS && deltaY==Ey_NS) H=RM;
else if(deltaH==Eh_NM && (deltaY==Ey_NZ || deltaY==Ey_PZ)) H=RM;
else if(deltaH==Eh_NB && deltaY==Ey_PS) H=RM;
else if(deltaH==Eh_PM && deltaY==Ey_NB) H=RS;
else if(deltaH==Eh_PS && deltaY==Ey_NM) H=RS;
else if(deltaH==Eh_ZO && (deltaY==Ey_NS || deltaY==Ey_NZ)) H=RS;
else if(deltaH==Eh_NS && (deltaY==Ey_NZ || deltaY==Ey_PZ)) H=RS;
else if(deltaH==Eh_NM && deltaY==Ey_PS) H=RS;
else if(deltaH==Eh_NB && deltaY==Ey_PM) H=RS;
else if(deltaH==Eh_PB && deltaY==Ey_NB) H=ZO;
else if(deltaH==Eh_PM && deltaY==Ey_NM) H=ZO;
else if(deltaH==Eh_PS && deltaY==Ey_NS) H=ZO;
else if(deltaH==Eh_NS && deltaY==Ey_PS) H=ZO;
else if(deltaH==Eh_NM && deltaY==Ey_PM) H=ZO;
else if(deltaH==Eh_NB && deltaY==Ey_PB) H=ZO;
else if(deltaH==Eh_PB && deltaY==Ey_NM) H=LS;
else if(deltaH==Eh_PM && deltaY==Ey_NS) H=LS;
else if(deltaH==Eh_PS && (deltaY==Ey_NZ || deltaY==Ey_PZ)) H=LS;
else if(deltaH==Eh_ZO && (deltaY==Ey_PZ || deltaY==Ey_PS)) H=LS;
else if(deltaH==Eh_NS && deltaY==Ey_PM) H=LS;
else if(deltaH==Eh_NM && deltaY==Ey_PB) H=LS;
else if(deltaH==Eh_PB && deltaY==Ey_NS) H=LM;
else if(deltaH==Eh_PM && (deltaY==Ey_NZ || deltaY==Ey_PZ)) H=LM;
else if(deltaH==Eh_PS && deltaY==Ey_PS) H=LM;
else if(deltaH==Eh_ZO && deltaY==Ey_PM) H=LM;
else if(deltaH==Eh_NS && deltaY==Ey_PB) H=LM;
else if(deltaH==Eh_PB && (deltaY==Ey_NZ || deltaY==Ey_PZ)) H=LB;
else if((deltaH==Eh_PM || deltaH==Eh_PB) && deltaY==Ey_PS) H=LB;
else if((deltaH==Eh_PB || deltaH==Eh_PM) && (deltaY==Ey_PB || deltaY==Ey_PM)) H=LB;
else if(deltaH==Eh_PS && (deltaY==Ey_PM || deltaY==Ey_PB)) H=LB;
else H=LB;
return H;
}
float CMyView::GetDanQianJiao(float jiao)
{
double R; //转弧半径
double L=7.5; //车宽
double Arc_jiao; //转动圆心角
if(Robot.RobotVL==Robot.RobotVR)
{
return jiao;
}
else
{
R=L*(1+2*Robot.RobotVR/(Robot.RobotVL-Robot.RobotVR))/2; //可正可负
Arc_jiao=((m_nTimer/1000.0)*(Robot.RobotVL-Robot.RobotVR)/L)*180.0/Pi; //转动圆心角
jiao+=Arc_jiao;
}
return jiao;
}
int CMyView::GetDesireJiao(float a,int delta_a)
{
a+=delta_a;
return (int)a;
}
void CMyView::Move(int Desired_angle)
{
struct speedinformation robotV;
int Theta_e,acute;
//******************************************************************
//*****************************横方向模糊控制器的加入***************
int delta_v;
if(FEx==Ex_NB) delta_v=V_PB;
else if(FEx==Ex_NS) delta_v=V_PS;
else if(FEx==Ex_ZO) delta_v=V_ZO;
else if(FEx==Ex_PS) delta_v=V_NS;
else delta_v=V_NB;
MoveVmax1+=delta_v;
//******************************************************************
//******************************************************************
Theta_e = Desired_angle - Robot.Theta;
Theta_e = AlterAngle(Theta_e);
acute = Theta_e % 90 ; //get the angular value less than 90 degree;
switch(Theta_e/90) //Theta_e is integer
{
case 0://in first quadint
robotV.lv = MoveVmax1;
robotV.rv = (int)(cos(1.0 * acute/180*Pi)*MoveVmax1);
break;
case 1:
acute = 90 - acute;
robotV.lv = -MoveVmax1;
robotV.rv = (int)(-cos(1.0 * acute/180*Pi)*MoveVmax1);
break;
case 2:
robotV.rv = -MoveVmax1;
robotV.lv = (int)(-cos(1.0 * acute/180*Pi)*MoveVmax1);
break;
case 3:
acute =90 - acute;
robotV.rv = MoveVmax1;
robotV.lv = (int)(cos(1.0 * acute/180*Pi)*MoveVmax1);
break;
}
if(MoveVmax1>MoveVmax) MoveVmax1=MoveVmax;
Robot.RobotVL=robotV.lv;
Robot.RobotVR=robotV.rv;
}
void CMyView::GetNextPosure(double VL, double VR)
{
double l=7.5; //轮半径
double r; //圆心角半径
double Xr,Yr; //圆心坐标
robot_jiao=Robot.Theta;
if(VL==VR)
{
double s=m_nTimer*VL*3/1000.0;
fRobot.fTargetX+=s*cos(robot_jiao/180.0*Pi);
fRobot.fTargetY+=s*sin(robot_jiao/180.0*Pi);
Robot.nCenter.x=(int)fRobot.fTargetX;
Robot.nCenter.y=(int)fRobot.fTargetY;
}
else
{
r=l/2.0*(1+2*VR/(VL-VR));
Xr=fRobot.fTargetX-r*sin(robot_jiao/180.0*Pi);
Yr=fRobot.fTargetY+r*cos(robot_jiao/180.0*Pi);
double a=GetDanQianJiao(robot_jiao);
robot_jiao=a;
fRobot.fTargetX=Xr+r*sin(a/180.0*Pi);
fRobot.fTargetY=Yr-r*cos(a/180.0*Pi);
Robot.nCenter.x=(int)fRobot.fTargetX;
Robot.nCenter.y=(int)fRobot.fTargetY;
//Robot[who-1].nCenter.x=(int)(Xr+r*sin(a/180.0*pi));
//Robot[who-1].nCenter.y=(int)(Yr-r*cos(a/180.0*pi));
Robot.Theta=robot_jiao;
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -