learnview.cpp

机器人路径规划程序 机器人路径规划程序 机器人路径规划程序

//			AfxMessageBox("111");
//			R_k=-52.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;

		}
		else if (((int)trd06==Bit_color)||((int)trd06==RedColor)||((int)trd06==GreenColor))//&&((int)trd08==Bit_color))
		{
			trd_numB=11;
			dis1=dis;
//			KillTimer(10);
//			AfxMessageBox("111");
//			R_k=-37.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;

		}
		else if (((int)trd08==Bit_color)||((int)trd08==RedColor)||((int)trd08==GreenColor))//&&((int)trd10==Bit_color))
		{
			trd_numB=12;
			dis1=dis;
//			KillTimer(10);
//			AfxMessageBox("111");
//			R_k=-22.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;

		}
		else if (((int)trd10==Bit_color)||((int)trd10==RedColor)||((int)trd10==GreenColor))//&&((int)trd12==Bit_color))
		{
			trd_numB=13;
			dis1=dis;
//			KillTimer(10);
//			AfxMessageBox("111");
//			R_k=-7.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;

		}
		else if (((int)trd12==Bit_color)||((int)trd12==RedColor)||((int)trd12==GreenColor))//&&((int)trd14==Bit_color))
		{
			trd_numB=14;
			dis1=dis;
//			KillTimer(10);
//			AfxMessageBox("111");
//			R_k=+7.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;

		}
		else if (((int)trd14==Bit_color)||((int)trd14==RedColor)||((int)trd14==GreenColor))//&&((int)trd16==Bit_color))
		{
			trd_numB=15;
			dis1=dis;
//			KillTimer(10);
// 			AfxMessageBox("111");
//			R_k=+7.5*Pi/180.0;
			flag0=1;
//			Sta2Action(trd_num);
			break;
		}


	}




//		flag0=0;

	}


//
//
//
//
//
//
/////////////////////////////////////////////////////////////////
////显示机器人运行步数
//	bushu0++;
//    bushu.Format("%d",bushu0);
//	dc.TextOut(30,20,"步数");	
//	dc.TextOut(60,20,bushu);
//
//
//
//	///////////****计算机器人前方180度范围内的探测物////////////////////////////////////////////////////////////////
/////************从左至右分别为trd0,...trd6**********************/////////////////////
//    trd6=dc.GetPixel((int)(x+di*cos(R_k+Pi/2.0)),(int)(y+di*sin(R_k+Pi/2.0)));
//	trd5=dc.GetPixel((int)(x+di*cos(R_k+Pi/3.0)),(int)(y+di*sin(R_k+Pi/3.0)));
//	trd4=dc.GetPixel((int)(x+di*cos(R_k+Pi/6.0)),(int)(y+di*sin(R_k+Pi/6.0)));
//	trd3=dc.GetPixel((int)(x+di*cos(R_k)),(int)(y+di*sin(R_k)));
//	trd2=dc.GetPixel((int)(x+di*cos(R_k-Pi/6.0)),(int)(y+di*sin(R_k-Pi/6.0)));
//	trd1=dc.GetPixel((int)(x+di*cos(R_k-Pi/3.0)),(int)(y+di*sin(R_k-Pi/3.0)));
//	trd0=dc.GetPixel((int)(x+di*cos(R_k-Pi/2.0)),(int)(y+di*sin(R_k-Pi/2.0)));
//
////******************************************************************************************//
//if(test2==1)	
//{
//    if(((int)trd0==Bit_color)&&((int)trd1==Bit_color))
//	{
////	    KillTimer(10);
////		AfxMessageBox("trd0,1");
//	    TurnTo(11);
//		xp1=x+di*cos(R_k-Pi/2.0);
//		yp1=y+di*sin(R_k-Pi/2.0);
//		xp2=x+di*cos(R_k-Pi/3.0);
//   		yp2=y+di*sin(R_k-Pi/3.0);
//		R_z=-atan(1/tan(R_k-5.0*Pi/12.0));
//		WallLeft(1);
//		flag0=1;
//
//	}
//	else if(((int)trd1==Bit_color)&&((int)trd2==Bit_color))
//		{
////		    KillTimer(10);
////		    AfxMessageBox("trd21");
////		    R_k=R_k-Pi/2.0;
//		     TurnTo(9);
//			 xp1=x+di*cos(R_k-Pi/3.0);
//   		     yp1=y+di*sin(R_k-Pi/3.0);
//			 xp2=x+di*cos(R_k-Pi/6.0);
//		     yp2=y+di*sin(R_k-Pi/6.0);
//		     R_z=-atan(1/tan(R_k-3.0*Pi/12.0));
//			 WallLeft(1);
//			 flag0=1;
//		}
//
//	else if(((int)trd2==Bit_color)&&((int)trd3==Bit_color))
//		{
////		    KillTimer(10);
////		    AfxMessageBox("trd23");
// 			TurnTo(7);
//			xp1=x+di*cos(R_k-Pi/6.0);
//		    yp1=y+di*sin(R_k-Pi/6.0);
//			xp2=x+di*cos(R_k);
//		    yp2=y+di*sin(R_k);
//            R_z=-atan(1/tan(R_k-Pi/12.0));
//			WallLeft(1);
//			flag0=1;
//			
////			R_Angle=R_Angle-90;
////			R_k=R_k-Pi/2.0;				
////			str_A.Format("%f",R_Angle);    //显示R_Angle
////        	dc.TextOut(430,40,str_A);
//			
//		}
//	else if(((int)trd3==Bit_color)&&((int)trd4==Bit_color))
//		{
////		KillTimer(10);
////		AfxMessageBox("trd34");
//		TurnTo(5);		
//		xp1=x+di*cos(R_k);
//		yp1=y+di*sin(R_k);
//		xp2=x+di*cos(R_k+Pi/6.0);
//		yp2=y+di*sin(R_k+Pi/6.0);
//		R_z=-atan(1/tan(R_k+Pi/12.0));
//		WallLeft(1);
//		flag1=1;
//		
//		}
//	else if(((int)trd4==Bit_color)&&((int)trd5==Bit_color))
//		{
////		    KillTimer(10);
////		    AfxMessageBox("trd45");
//		    TurnTo(3);
//			xp1=x+di*cos(R_k+Pi/6.0);
//		    yp1=y+di*sin(R_k+Pi/6.0);
//			xp2=x+di*cos(R_k+Pi/3.0);
//		    yp2=y+di*sin(R_k+Pi/3.0);
//			R_z=-atan(1/tan(R_k+3.0*Pi/12.0));
//
//			WallLeft(1);
//			flag1=1;
//		}
//	else if(((int)trd5==Bit_color)&&((int)trd6==Bit_color))
//		{
////		    KillTimer(10);
////		    AfxMessageBox("trd56");
//		     TurnTo(1);
//			 xp1=x+di*cos(R_k+Pi/3.0);
//		     yp1=y+di*sin(R_k+Pi/3.0);
//			 xp2=x+di*cos(R_k+Pi/2.0);
//		     yp2=y+di*sin(R_k+Pi/2.0);
//			 R_z=-atan(1/tan(R_k+5.0*Pi/12.0));
//			 WallLeft(1);
//			 flag1=1;
//		}
//
//	else 
//	{
//		test1=1;
//		
//	}
//}
////******************************************************************************************//
////        ToAim();      //前方无障碍物，则走直线向前着目标方向移动
//
//////////************墙角环境判断**********************************
//
//		if((((R_k>=0)&&(MidValue<0))||((R_k<=0)&&(MidValue>0)))&&(first==0))
//		{
//			num0++;
//			CString str_num0;
//			str_num0.Format("R_k变号次数:%d",num0);
//			dc.TextOut(750,80,str_num0);
//			test1=0;     //进入墙角环境后路径规划方法不再与原来一样
//			test2=0;
//			first=1;     
//// 			KillTimer(10);
//// 			AfxMessageBox("r==0");
////			WallRight(1);
//
////			R_k=Pi/2.0+R_z;
//			
//
//			if ((flag0==1)&&(flag1==1))
//			{
////				KillTimer(10);
////				AfxMessageBox("flag");
////				WallRight(0);
//			}
//
//		}
//		else if (first==1)
//		{
////             WallRight (1);
////			 KillTimer(10);
////			 AfxMessageBox("first==1");
//
//		} 
//
//	MidValue=R_k;       /////保存上一次的R_k值
//
//	
////////////////////////////////////////////////////////////////////////////////////////////////
//  CPen RedPen(PS_SOLID,1,RGB(255,0,255));
//  dc.SelectObject(RedPen);
//  dc.Ellipse((int)x-2,(int)y-2,(int)x+2,(int)y+2);    ///机器人运行轨迹
//
//
//
//
//  str_px.Format("(%d,",(int)x);      //机器人运行当前坐标
//  str_py.Format("%d)",(int)y);
//  dc.TextOut(550,20,str_px);
//  dc.TextOut(580,20,str_py);
//
//
//	str3.Format("R_z:%.4f",R_z);
//	dc.TextOut(250,20,str3);
//
//
//// 	str3=(char)color2;
//// 	str3.Format("%d",color2);
//// 	dc.TextOut(250,20,str3);
//	
//
//16711215白色,06777215黑色16777215
	
//    if (x>=x2)
//    {
//		x1=x0;
//		y1=y0;
//		KillTimer(10);
//    }

//}






void CLearnView::Sta2Action(int n)
{
//	flag0=1;
	CClientDC cDC(this);
	CString strs;
	double rwd,Qmax=0;

	reward(1);
		
	if (test1==0)
	{
		R_z=R_k;
		test1=1;
		Action(0);
	}
	
	else if (test1==1)
	{
		
		strs.Format("R_k-R_z:%.2f",R_k-R_z);
		cDC.TextOut(550,30,strs);
	
		if ((R_k-R_z>0.5))//&&(R_k-R_z<9))//||(R_k-R_z<-0.1))
		{
			flag0=1;    //沿墙壁走
	    	Action(1);
		}
		else 
		{
//			AfxMessageBox("else");
			flag0=0;    //向着目标走
			test1=0;
			if (ColorSignG==1||ColorSignR==1)    //如果遇到运动障碍物不要沿着目标走
			{
				flag0=1;
			}
		    Action(0);
		}
 	}



//	strs.Format("%d",trd_numB);
//	cDC.TextOut(50,30,strs);


	double dis12=0.0;
	COLORREF  trd01,trd02,trd11,trd12;
	CClientDC dc(this);
/*
	if (flag0==1) 
	{
		Action(1);
	}
    else Action(0);
*/


/*
	    if ((trd_num==0))
		{
			for(dis12=0.0;dis12<15.0;dis12=dis12+0.50)
			{
/////////取得传感器的值/////////
		    trd11=dc.GetPixel((int)(x+dis12*cos(R_k-6.0*Pi/12.0)),(int)(y+dis12*sin(R_k-6.0*Pi/12.0)));
		    trd12=dc.GetPixel((int)(x+dis12*cos(R_k+6.0*Pi/12.0)),(int)(y+dis12*sin(R_k+6.0*Pi/12.0)));
		  		if (((int)trd11==Bit_color))//&&((int)trd01==Bit_color)) //(0,1)
				{
//				StopTest();
				Action(3);
				break;
				}

				else if (((int)trd12==Bit_color))//&&((int)trd03==Bit_color))//(1,3)
				{
				Action(2);	
			
//				StopTest();
				break;

				}
			}

		    if (dis12>=15)
			{
				Action(3);
			
			}

		  
		}


	
		
		 else if ((trd_num>=1)&&(trd_num<=5))
		{
//			srand(GetTickCount());
//			Action(3+rand()%3);

			Action(trd_num);
			if ((qq>=3) &&(qq<=5))
			{
				rwd=2.0;
			}
			else rwd=-4.0;
			
		}
		
		else if ((trd_num>=6)&&(trd_num<=7)&&(dis2>=14))
		{
//			srand(GetTickCount());
//			Action(rand()%6);
//			Action(rand()%3);
//			Action(1);
			Action(trd_num);
			if ((qq>=0) &&(qq<=2))
			{
				rwd=2.0;
			}
			else rwd=-4.0;
		
 		}

    	else if ((trd_num>=9)&&(trd_num<=13))
		{
//			srand(GetTickCount());
//			Action(rand()%6);
//			Action(rand()%3);
//			Action(1);//
			Action(trd_num);
			if ((qq>=0) &&(qq<=2))
			{
				rwd=2.0;
			}
			else rwd=-4.0;
			

		}
		
		else if ((trd_num>=14)&&(trd_num<=15)&&(dis2>=14)) 
		{
//			
//			srand(GetTickCount());
//			Action(rand()%6);
//			Action(3+rand()%3);
//			Action(4);
			Action(trd_num);
			if ((qq>=3) &&(qq<=5))
			{
				rwd=2.0;
			}
			else rwd=-4.0;
		}

		str.Format("%.2f",R_z-R_k);
		cDC.TextOut(450,30,str);
		if (trd_num>=16)
		{	
			flag0=0;

		}

/////////////////////////////////////

	Qsa[trd_num][qq]=Qsa[trd_num][qq]+rwd;*/



/*	if (Qsa[trd_num][qq]<=0) 
	{
		Qsa[trd_num][qq]=0;
	}

	

			    Qmax=Qsa[trd_num][0];
				for(int i0=0;i0<6;i0++)
				{
					if (Qmax<Qsa[trd_num][i0]) 
					{
						Qmax=Qsa[trd_num][i0];
					}
				}
				Qsa[trd_num][qq]=rwd+0.25*Qmax;
				
				for (int i2=0;i2<4;i2++)
				{
					for(int i3=0;i3<6;i3++)
					{
						Qvamax=Qvamax+Qsa[i2][i3];
					}
				}
*/

////////////////////////////////////
	

	
//	KillTimer(10);

//	AfxMessageBox("wall");
//	int size=sizeof(QValue);
//	double rwd=0.0;//回报值
//	double rapd=0.9;//学习率
//	double max=0;
//	double QQ[17][5],PP[17][5];
//	int qq,pp;
//	int A_num=0,i=1,q=10;
//    	q=(n)*5+i-1;
//		
//	CFile QQfile,PPfile;
//	struct QValue *pQQ=new struct QValue;
//
//	for (qq=0;qq<17;qq++)
//	{
//		for (pp=0;pp<5;pp++)
//		{
//			pQQ->Q[qq][pp]=10;
//		}
//	}
//		
//
//
//
////	if (QQfile.Open("QQfile",CFile::modeCreate|CFile::modeWrite))
//		if (QQfile.Open("QQfile",CFile::modeWrite))
//	{
//		QQfile.Write(pQQ,size);
//		QQfile.Close();
//	}
//
//
//	struct QValue *pPP=new struct QValue;
//	if (PPfile.Open("QQfile",CFile::modeRead))
//	{
//		PPfile.Read(pPP,size);
//		PPfile.Close();
//	}
//
//	for (qq=0;qq<17;qq++)
//	{
//	    for (pp=0;pp<5;pp++)
//		{
//			PP[qq][pp]=pQQ->Q[qq][pp];
//		}
//	}
//	str.Format("PP:%.3f",PP[1][2]);
//	dc.TextOut(50,50,str);
//
//	
//       max=Q[q];
//
//			  for(i=1;i<5;i++)
//			  {
//				  if(max<Q[q+i])
//				  {
//					  max=Q[q+i];
//					  A_num=i;						  
//  				  }
//			  }
//			  Action(A_num);             //执行动作
//		      rwd=reward(A_num);
////			  Q[(n-1)*5+A_num+1]=rwd+rapd*Q[(n-1)*5+A_num+1];     //更新Q值
////			  q=q+A_num;
//
//			  Q[q]=rwd+0.9*Q[q];
//			  str.Format("%f",Q[q]);
//			  dc.TextOut(50,30,str);
//			 
// 			  CFile myfile;
//// 			  CString filename;
////  	          filename="test.txt";
// 			  if(myfile.Open("QQ.txt",CFile::modeCreate|CFile::modeNoTruncate|CFile::modeReadWrite))
//// 			  {
//// 				
// 					  str.Format("%f",Q[q]);
//					  myfile.Write(str,sizeof(str));
//					  myfile.Write(" \n",sizeof(str));
//// 			  }
//				myfile.Close();
//// 				  
// 	

}

void CLearnView::Action(int A_num)
{
	//机器人转过的角度为：
	//0—8号传感器检测到墙时，向右90度加0号传感器和检测到墙的传感器的夹角
	//9—15号传感器检测到墙时，向左90度减去0号传感器和检测到墙的传感器的夹角
    if(ColorSignB==1&&ColorSignG!=1&&ColorSignR!=1)
	{
    	if (trd_numB>=0&&trd_numB<=8)
		{
		    R_k=R_k+(Pi/2+R_s);
		}
	    else if (trd_numB>=9&&trd_numB<=15) 
		{
		    R_k=R_k-(Pi/2-R_s);
		}	
	}
   //如果机器人同时遇到地图中障碍物和运动障碍物时，机器人停止运动进行等待
   if (ColorSignB==1&&ColorSignG==1||ColorSignB==1&&ColorSignR==1)
	{
        buchang=0;
	}
	else buchang=1.5;
	
	if (A_num==1)
	{
	//在机器人沿墙走时，限定机器人的dis2距离在15-19之间来变化实现沿墙走，并判断是哪侧传感器
	//此处由于步长的选择及传感器的检测范围，要注意dis限定范围的选取
	    if (dis2<=14&&trd_numB>=0&&trd_numB<=8||dis2>=17&&trd_numB>=9&&trd_numB<=15)
		{
	    	R_k=R_k+Pi/12;
		}
	    else if (dis2<=14&&trd_numB>=9&&trd_numB<=15||dis2>=17&&trd_numB>=0&&trd_numB<=8)
		{
		    R_k=R_k-Pi/12;
		}
	}

	else if (A_num==0)
	{
	//机器人不沿墙走时，限定机器人的dis2距离的内边界，外边界不必限制
	if (ColorSignG==1)  //遇到向左的绿色障碍物时，机器人向右转
	{
		R_k=0;
	}
	else if (ColorSignR==1)  //遇到向右的绿色障碍物时，机器人向左转
	{