起伏地形速度控制

StartSimulationEngine  
Ruilei Zhang CopyRight

AddSkyDomeEntity sky3

AddLightSourceEntity sun1

AddTerrainEntity ground1

//AddPioneer3DX robot01  	/Position:0  0  0
//	/Orientation:0 0 0

AddFourByFourDriveEntity robot01
	/Position:0  0  0
	/Orientation:0 270 0
//	/AngularDamping:50
//	/LinearDamping:50
AddNewEntity zrlc1  	/Position:0  0  0
	/ParentEntity:robot01
	/Mesh:"4x4Body.bos"


SPLPoseSensors rbv1
	/Procedure_PoseNotify:proc1
//AddPioneer3DX robot02  	/Position:1.5  0  1.5
//	/Orientation:0 0 0

//AddPioneer3DX robot03  	/Position:0  0  2
//	/Orientation:0 0 0

//AddPioneer3DX robot04  	/Position:-1.5  0  1.5
//	/Orientation:0 0 0

//AddPioneer3DX robot05  	/Position:0  0  4
//	/Orientation:0 0 0
//GameController controller1
//	/Procedure_UpdateAxes:proc_ctrl

//SimpleDashboard simpledashboard1


AddFollowingChaseCamera cam1
	/Position:0  0.5  2.0
	/ChaseTarget:robot01

UpdateCameraView /EyePosition:0  5  5  	/LookAtPoint:0  0  0


FlushScript  
rbv1.PoseSubscribe("robot01", 200)
///
WaitForEntityCreation robot01 //WaitForEntityCreation robot02   
//WaitForEntityCreation robot03   
//WaitForEntityCreation robot04   
//WaitForEntityCreation robot05   
///
double  vtall=0.2 // >0.4
double  wtall=0.0 // >0.6
double dprl=0.2
double  vtsign=0
double  wtsign=0
double kpvtf=1.1
double kpwtf=3.3
//double kivtf=0.08
//double kiwtf=0.24
double kivtf=0.06
double kiwtf=0.18
double ctrl_x=0.0
double ctrl_y=0.0
double ctrl_z=0.0
double pxsum=0.0
double pzsum=0.0
double kpld1=8.0
double kpld2=8.0
double SimTime=202.0
double p_xn=0
double p_yn=0
double p_zn=0
double vel=0
float PadNum=0.0
float ctrl_x=0.0
float ctrl_y=0.0	
float ctrl_z=0.0
matlabtheta1 = Util.CreateArray(double, 200)  
matlabderr1 = Util.CreateArray(double, 200)  
matlabtheta2 = Util.CreateArray(double, 200)  
matlabderr2 = Util.CreateArray(double, 200) 
matlabtheta3 = Util.CreateArray(double, 200)  
matlabderr3 = Util.CreateArray(double, 200) 
matlabtheta4 = Util.CreateArray(double, 200)  
matlabderr4 = Util.CreateArray(double, 200) matlablx = Util.CreateArray(double, 200)  
matlably = Util.CreateArray(double, 200)  
matlabfx1= Util.CreateArray(double, 200)  
matlabfy1= Util.CreateArray(double, 200)
matlabfx2 = Util.CreateArray(double, 200)  
matlabfy2 = Util.CreateArray(double, 200)
matlabfx3= Util.CreateArray(double, 200)  
matlabfy3= Util.CreateArray(double, 200)
matlabfx4 = Util.CreateArray(double, 200)  
matlabfy4 = Util.CreateArray(double, 200)
/
matlablvt = Util.CreateArray(double, 200)  
matlablwt = Util.CreateArray(double, 200)  
matlabfvt1 = Util.CreateArray(double, 200)  
matlabfwt1 = Util.CreateArray(double, 200) 
matlabfvt2 = Util.CreateArray(double, 200)  
matlabfwt2 = Util.CreateArray(double, 200) 
matlabfvt3 = Util.CreateArray(double, 200)  
matlabfwt3 = Util.CreateArray(double, 200) 
matlabfvt4 = Util.CreateArray(double, 200)  
matlabfwt4 = Util.CreateArray(double, 200) 
matlabt = Util.CreateArray(double, 200) 
matlabtime = Util.CreateArray(double, 200)


wait 20000

call proc_main  with concur

Procedure  proc_main

	call proc_robot_01

End

Procedure  proc1

	vel = value.Velocity

End

Procedure Matlab_Data

	MATLAB matlab1
///
	matlab1.PutFullMatrix("theta1", "base", matlabtheta1, matlabt)
	matlab1.PutFullMatrix("derr1", "base", matlabderr1, matlabt)
	matlab1.PutFullMatrix("theta2", "base", matlabtheta2, matlabt)
	matlab1.PutFullMatrix("derr2", "base", matlabderr2, matlabt)
	matlab1.PutFullMatrix("theta3", "base", matlabtheta3, matlabt)
	matlab1.PutFullMatrix("derr3", "base", matlabderr3, matlabt)
	matlab1.PutFullMatrix("theta4", "base", matlabtheta4, matlabt)
	matlab1.PutFullMatrix("derr4", "base", matlabderr4, matlabt)
///
	matlab1.PutFullMatrix("lx", "base", matlablx, matlabt)
	matlab1.PutFullMatrix("ly", "base", matlably, matlabt)
	matlab1.PutFullMatrix("fx1", "base", matlabfx1, matlabt)
	matlab1.PutFullMatrix("fy1", "base", matlabfy1, matlabt)
	matlab1.PutFullMatrix("fx2", "base", matlabfx2, matlabt)
	matlab1.PutFullMatrix("fy2", "base", matlabfy2, matlabt)
	matlab1.PutFullMatrix("fx3", "base", matlabfx3, matlabt)
	matlab1.PutFullMatrix("fy3", "base", matlabfy3, matlabt)
	matlab1.PutFullMatrix("fx4", "base", matlabfx4, matlabt)
	matlab1.PutFullMatrix("fy4", "base", matlabfy4, matlabt)
/
	matlab1.PutFullMatrix("lvt", "base", matlablvt, matlabt)
	matlab1.PutFullMatrix("lwt", "base", matlablwt, matlabt)
	matlab1.PutFullMatrix("fvt1", "base", matlabfvt1, matlabt)
	matlab1.PutFullMatrix("fwt1", "base", matlabfwt1, matlabt)
	matlab1.PutFullMatrix("fvt2", "base", matlabfvt2, matlabt)
	matlab1.PutFullMatrix("fwt2", "base", matlabfwt2, matlabt)
	matlab1.PutFullMatrix("fvt3", "base", matlabfvt3, matlabt)
	matlab1.PutFullMatrix("fwt3", "base", matlabfwt3, matlabt)
	matlab1.PutFullMatrix("fvt4", "base", matlabfvt4, matlabt)
	matlab1.PutFullMatrix("fwt4", "base", matlabfwt4, matlabt)
	matlab1.PutFullMatrix("mtime", "base", matlabtime, matlabt)
//	matlab1.PutFullMatrix("linef", "base", matlableaderx, matlableadery)
//	matlab1.PutFullMatrix("derr", "base", matlabdiserror, matlabt)
End

/

Procedure  proc_robot_01
//leader
	for (i = 0; i < SimTime; i++)
	{
// ily=100.0
//		vtall = (0.5*ily)
//		if(i>600)
//		{
// vtall=-(0.5*ily)
//		}
//		if(i>400)
//		{
// vtall=(0.5*ily)
//		}
//		if(i>600)
//		{
// vtall=-(0.5*ily)
//		}
//		if(i>800)
//		{
// vtall=(0.5*ily)
//		} vsss = Math.Sign( vtall )
//		if(i<500)
//		{
//		vtall=vtall+0.001
//		}
//		if(i>500)
//		{
//		if(i<1000)
//		{
//		vtall=vtall-0.001
//		}
//		if(i>1000)
//		{
//		if(i<1500)
//		{
//		vtall=vtall+0.001
//		}
//		if(i>1500)
//		{
//		vtall=vtall-0.001
//		}
//		}
//		}
		theta_l=robot01.Orientation.Y
		p_xp = p_xn p_zp = p_zn
		p_yp = p_yn
		p_xn = robot01.Position.X p_zn = robot01.Position.Z
		p_yn = robot01.Position.Y
		p_xx = (p_xn-p_xp) p_zz = (p_zn-p_zp)
		p_yy = (p_yn-p_yp)
		vtot= Math.Sqrt( p_xx*p_xx+ p_zz*p_zz+p_yy*p_yy ) vt=vtall wt=wtall matlablvt[i]=vt
		matlablwt[i]=wt matlabfvt1[i]=vsss*vtot
//		matlabfwt1[i]=vel matlabfvt2[i]=((matlabfvt1[i]+matlabfvt1[i-1]+matlabfvt1[i-2]+matlabfvt1[i-3])/4.0)
//		matlabfwt1[i]=wt_f 
		matlabtime[i]=i
		left= ( vt + dprl * wt ) right= ( vt - dprl * wt ) 
///
		robot01.Go(left, right)
		wait 200
	}
//	FlushScript
	robot01.Go(0, 0)
	call Matlab_Data

End