基于MSRDS机器人仿真平台的多机器人PID编队控制算法
基于MSRDS机器人仿真平台的多机器人PID编队控制算法
zhangrelay
发布于 2019-01-23 10:15:18
发布于 2019-01-23 10:15:18
自己调试的编队PID算法，效果也还可以，具体使用教程参考视频链接：
http://v.youku.com/v_show/id_XMTUwNjc3NjMyNA
仿真中三个机器人保持编队，做直线运动，队形和参数都可以调整。
仿真结束后，编队数据直接推送的MATLAB中，用于曲线绘制，并分析。
源代码下载地址：http://download.csdn.net/detail/zhangrelay/9514411
--------
StartSimulationEngine
//zhangrelay formation_control PID
AddSkyEntity sky
/VisualCubeTextureFile:"sky.dds"
/LightingCubeTextureFile:"sky_diff.dds"
AddLightSourceEntity sun
/LightType:Directional
/Color:0.8 0.8 0.8 1
/Direction:-1.0 -1.0 0.5
AddHeightFieldEntity ground1
////Ruilei Zhang CopyRight
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AddiRobotCreate robot01 /Position:0 0 0
/Orientation:0 -90 0
AddiRobotCreate robot02 /Position:-3 0 -3
/Orientation:0 -90 0
// /Procedure_LRF_SensorNotify:proc_robot_02_LRF
AddiRobotCreate robot03 /Position:-3 0 3
/Orientation:0 -90 0
// /Procedure_LRF_SensorNotify:proc_robot_03_LRF
//GameController controller1
// /Procedure_UpdateAxes:proc_ctrl
//SimpleDashboard simpledashboard1
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//AddiRobotCreate icreate1 /Position:0 0 0
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
UpdateCameraView
/EyePosition:0 20 5 /LookAtPoint:0 0 0
FlushScript
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
WaitForEntityCreation robot01
WaitForEntityCreation robot02
WaitForEntityCreation robot03
//////WaitForEntityCreation robot04
//////WaitForEntityCreation robot05
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double vtall=0.2 // >0.4
double wtall=0.00 // >0.6
double dprl=0.2
double leadfy=0.0
double vtsign=0
double wtsign=0
double kpvtf=1.0
double kpwtf=4.0
//double kivtf=0.08
//double kiwtf=0.24
double kivtf=0.1
double kiwtf=0.04
double jdqw=0.04
double jlqw=0.04
double ctrl_x=0.0
double ctrl_y=0.0
double ctrl_z=0.0
double pxsum1=0.0
double pzsum1=0.0
double pxsum2=0.0
double pzsum2=0.0
double kpld1=8.0
double kpld2=8.0
double SimTime=1002.0
float rob2ff=0.0
float rob3ff=0.0
float rob2lrf1=0.0
float rob2lrf2=0.0
float rob2lrf3=0.0
float rob2lrf4=0.0
float rob2lrf5=0.0
float rob3lrf1=0.0
float rob3lrf2=0.0
float rob3lrf3=0.0
float rob3lrf4=0.0
float rob3lrf5=0.0
float j=0
////float PadNum=0.0
////float ctrl_x=0.0
////float ctrl_y=0.0
////float ctrl_z=0.0
matlabtheta1 = Util.CreateArray(double, 1000)
matlabderr1 = Util.CreateArray(double, 1000)
matlabtheta2 = Util.CreateArray(double, 1000)
matlabderr2 = Util.CreateArray(double, 1000)
matlabtheta3 = Util.CreateArray(double, 1000)
matlabderr3 = Util.CreateArray(double, 1000)
matlabtheta4 = Util.CreateArray(double, 1000)
matlabderr4 = Util.CreateArray(double, 1000)
////////////////////////////////////////////////////////////////////////////////////////
matlablx = Util.CreateArray(double, 1000)
matlably = Util.CreateArray(double, 1000)
matlablz = Util.CreateArray(double, 1000)
matlabla = Util.CreateArray(double, 1000)
matlablb = Util.CreateArray(double, 1000)
matlablc = Util.CreateArray(double, 1000)
matlabfx1= Util.CreateArray(double, 1000)
matlabfy1= Util.CreateArray(double, 1000)
matlabfz1= Util.CreateArray(double, 1000)
matlabfx2 = Util.CreateArray(double, 1000)
matlabfy2 = Util.CreateArray(double, 1000)
matlabfz2= Util.CreateArray(double, 1000)
matlabfx3= Util.CreateArray(double, 1000)
matlabfy3= Util.CreateArray(double, 1000)
matlabfx4 = Util.CreateArray(double, 1000)
matlabfy4 = Util.CreateArray(double, 1000)
/////////////////////////////////////////////////////////////////////////////////////////
matlablvt = Util.CreateArray(double, 1000)
matlablwt = Util.CreateArray(double, 1000)
matlabfvt1 = Util.CreateArray(double, 1000)
matlabfwt1 = Util.CreateArray(double, 1000)
matlabfvt2 = Util.CreateArray(double, 1000)
matlabfwt2 = Util.CreateArray(double, 1000)
matlabfvt3 = Util.CreateArray(double, 1000)
matlabfwt3 = Util.CreateArray(double, 1000)
matlabfvt4 = Util.CreateArray(double, 1000)
matlabfwt4 = Util.CreateArray(double, 1000)
matlabt = Util.CreateArray(double, 1000)
matlabtime = Util.CreateArray(double, 1000)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
wait 10000
call proc_main with concur
Procedure proc_main
call proc_robot_01
call proc_robot_02
call proc_robot_03
//// call proc_robot_04
//// call proc_robot_05
//// call proc_line
// call proc_ctrl
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("lz", "base", matlablz, matlabt)
matlab1.PutFullMatrix("la", "base", matlabla, matlabt)
matlab1.PutFullMatrix("lb", "base", matlablb, matlabt)
matlab1.PutFullMatrix("lc", "base", matlablc, matlabt)
matlab1.PutFullMatrix("fx1", "base", matlabfx1, matlabt)
matlab1.PutFullMatrix("fy1", "base", matlabfy1, matlabt)
matlab1.PutFullMatrix("fz1", "base", matlabfz1, matlabt)
matlab1.PutFullMatrix("fx2", "base", matlabfx2, matlabt)
matlab1.PutFullMatrix("fy2", "base", matlabfy2, matlabt)
matlab1.PutFullMatrix("fz2", "base", matlabfz2, 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)
robot01.Go(0, 0)
robot02.Go(0, 0)
robot03.Go(0, 0)
//// robot04.Go(0, 0)
//// robot05.Go(0, 0)
// matlab1.PutFullMatrix("linef", "base", matlableaderx, matlableadery)
// matlab1.PutFullMatrix("derr", "base", matlabdiserror, matlabt)
End
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Procedure proc_robot_01
//leader
for (i = 0; i < SimTime; i++)
{
theta_l=robot01.Orientation.Y
p_xl = robot01.Position.X
p_zl = robot01.Position.Z
theta_y=robot01.Orientation.X //FY
theta_x=robot01.Orientation.Z //QX
p_yl = robot01.Position.Y //Z
vt=vtall
wt=wtall
if(i>600)
{
vt=(0.2+(i-600.0)/6000.0)
wt=0.04
}
matlablvt[i]=vt
matlablwt[i]=wt
matlabla[i]=theta_y
matlablb[i]=theta_x
matlablc[i]=theta_l
leadfy=( Math.Cos( theta_y* Math.PI / 180.0))
left= ( vt + dprl * wt )
right= ( vt - dprl * wt )
///////////////////////////////////////////////////
robot01.Go(left, right)
wait 50
}
// FlushScript
// robot01.Go(0, 0)
End
////Procedure proc_robot_02_LRF
//// rob2lrf2=value.DistanceMeasurements[270]
//// rob2lrf1=value.DistanceMeasurements[360]
////// print rob2lrf2. ToString()
////End
////Procedure proc_robot_03_LRF
//// rob3lrf4=value.DistanceMeasurements[90]
//// rob3lrf5=value.DistanceMeasurements[0]
//// print rob3lrf5. ToString()
////End
Procedure proc_robot_02
//follower01
for (i = 0; i < SimTime; i++)
{
vt_l=1.0*vtall
wt_l=1.0*wtall
vtsign= Math.Sign( vt_l )
wtsign= Math.Sign( wt_l )
// lpp = 0.8
// wpp= (- Math.PI /2.0) //( Math.PI /6.0) // Math.PI / 6.0 //Math.PI //left- right +
////// if(rob2lrf2<1500.0)
////// {
////// if(rob2ff<1.0)
////// {
////// if(rob2lrf2>600.0)
////// {
////// lpp = 1.0
////// wpp=( - Math.PI /2.0 + ((Math.PI /2.5)*((-rob2lrf2+1500.0)/900.0)))
////// }
////// else
////// {
////// lpp = 1.0
//////// wpp= - Math.PI /6.0
//////// rob2ff=2.0
////// }
////// }
////// }
////// else
////// {
////// if(rob2lrf1>2000.0)
////// {
////// lpp = 1.0
////// wpp= (- Math.PI /2.0)
////// rob2ff=0.0
////// }
////// }
//// if(i<300)
//// {
//// lpp = 1.0
//// wpp= 0
//// }
//// else
//// {
//// if(i<600)
//// {
//// lpp = 1.0
//// wpp= (- Math.PI /6.0)
//// }
//// else
//// {
lpp = 1.0
wpp= (- Math.PI /2.0)
//// }
//// }
// rob2ff=0.0
// print thetadt. ToString()
//////////////////////////////////////////////////////////////////////////////////basic2D
theta_l=robot01.Orientation.Y
theta_f=robot02.Orientation.Y
p_xl = robot01.Position.X
p_zl = robot01.Position.Z
p_xf = robot02.Position.X
p_zf = robot02.Position.Z
/////////////////////////////////////////////////////////////////////////////////////3D
theta_yl=robot01.Orientation.X //FY
theta_xl=robot01.Orientation.Z //QX
p_yl = robot01.Position.Y //Z
theta_yf=robot02.Orientation.X //FY
theta_xf=robot02.Orientation.Z //QX
p_yf = robot02.Position.Y //Z
///////////////////////////////////////////////////////////////////////////////////////// 3D-2D
// lpp= Math.Sqrt( 1.0/(1.0+(((p_yf-p_yl)*(p_yf-p_yl))/(((p_xf-p_xl)*(p_xf-p_xl))+((p_yf-p_yl)*(p_yf-p_yl))))))
// print lpp. ToString()
lsin= ( lpp* Math.Sin( wpp)) //L'
lcos= ( lpp* Math.Cos( wpp)) //L
thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
theta_yl=robot01.Orientation.X * Math.PI /180.0
theta_yf=robot02.Orientation.X * Math.PI /180.0
//////////////////////////////////////////////////////////////////////////////////////////
theta_=-(theta_l-theta_f)
if(theta_ > 180.0)
theta_=-(360.0-theta_)
if( -theta_ > 180.0)
theta_=(360.0+theta_)
theta_l=robot01.Orientation.Y * Math.PI /180.0
theta_f=robot02.Orientation.Y * Math.PI /180.0
theta_=(theta_/180.0)*Math.PI
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp)) //up - //down + this is not right but ...OK
zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
p_x_= (xff-p_xf)
p_z_= (zff-p_zf)
p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f))
ptxz= Math.Atan( p_t_x/p_t_z)
p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
p_jl= ( p_t_z)
p_jd= ( p_t_x)
// print p_jl. ToString()
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
matlablx[i]=p_xl
matlably[i]=p_zl
matlablz[i]=p_yl
matlabfx1[i]=p_xf
matlabfy1[i]=p_zf
matlabfz1[i]=p_yf
matlabtheta1[i]=theta_
matlabderr1[i]=p_ld
matlabbt[i]=0 //use matlab
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if((-ptxz)>(Math.PI))
ptxz= ( ptxz+ 2.0* Math.PI)
if(ptxz>(Math.PI))
ptxz= ( ptxz- 2.0* Math.PI)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// coordinate conversion
if( Math.Abs( wt_l) < 0.01 )
vt_l=vt_l
else
vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l //this change some wrong
p_ld=(kpld1*p_ld / Math.Abs( vt_l))
if(p_ld> (Math.PI /2) )
p_ld= Math.PI /2
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
kpld1=2.0
pxsum1=pxsum1+p_jd
pzsum1=pzsum1+p_jl
absjd = Math.Abs( p_jd)
absjl = Math.Abs( p_jl)
wt_f= ((kpwtf*p_jd+kiwtf*pxsum1)*vtsign+(kpwtf*(theta_-thetadt)*(jdqw/absjd))) //+ (kpwtf*theta_* Math.Cos( p_ld)))
vt_f= ((kpvtf*p_jl/leadfy+kivtf*pzsum1)*(jdqw/absjd))
if(absjd<(jdqw/4.0))
{
pxsum1=0
// absjd=jdqw
wt_f=matlabfwt1[i-1]
}
if(absjl<(jlqw/4.0))
{
pzsum1=0
// absjl=jlqw
vt_f=matlabfvt1[i-1]
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// print p_ld. ToString()
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
matlabfvt1[i]=vt_f
matlabfwt1[i]=wt_f
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
left= vt_f + dprl * wt_f
right=vt_f - dprl * wt_f
if(left>1.0)
left=1.0
if(right>1.0)
right=1.0
if(-left>1.0)
left=-1.0
if(-right>1.0)
right=-1.0
robot02.Go(left, right)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
wait 50
// print p_ld. ToString()
}
// robot02.Go(0, 0)
call Matlab_Data
End
Procedure proc_robot_03
//follower02
for (i = 0; i < SimTime; i++)
{
vt_l=1.0*vtall
wt_l=1.0*wtall
vtsign= Math.Sign( vt_l )
wtsign= Math.Sign( wt_l )
// lpp = 0.8
// wpp= Math.PI /2.0 //(- Math.PI /6.0) // Math.PI / 6.0 //Math.PI //left- right +
////// if(rob3lrf4<1500.0)
////// {
////// if(rob3ff<1.0)
////// {
////// if(rob3lrf4>600.0)
////// {
////// lpp = 1.0 //+1.0*((-rob2lrf2+1500.0)/900.0)
////// wpp=( Math.PI /2.0 - ((Math.PI /2.5)*((-rob3lrf4+1500.0)/900.0)))
////// }
////// else
////// {
////// lpp = 1.0
//////// wpp= Math.PI /6.0
//////// rob3ff=2.0
////// }
////// }
////// }
////// else
////// {
////// if(rob3lrf5>2000.0)
////// {
//////// tempi=i
////// lpp = 1.0
////// wpp= ( Math.PI /2.0)
////// rob3ff=0.0
////// }
////// }
//// if(i<300)
//// {
//// lpp = 2.0
//// wpp= 0
//// }
//// else
//// {
//// if(i<600)
//// {
//// lpp = 1.0
//// wpp= ( Math.PI /6.0)
//// }
//// else
//// {
lpp = 1.0
wpp= ( Math.PI /2.0)
//// }
//// }
// rob3ff=0.0
lsin= ( lpp* Math.Sin( wpp)) //L'
lcos= ( lpp* Math.Cos( wpp)) //L
thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
//////////////////////////////////////////////////////////////////////////////////basic
theta_l=robot01.Orientation.Y
theta_f=robot03.Orientation.Y
p_xl = robot01.Position.X
p_zl = robot01.Position.Z
p_xf = robot03.Position.X
p_zf = robot03.Position.Z
///////////////////////////////////////////////////////////////////////////////////////////////////
theta_yf=robot03.Orientation.X //FY
theta_xf=robot03.Orientation.Z //QX
p_yf = robot03.Position.Y //Z
theta_=-(theta_l-theta_f)
if(theta_ > 180.0)
theta_=-(360.0-theta_)
if( -theta_ > 180.0)
theta_=(360.0+theta_)
theta_l=robot01.Orientation.Y * Math.PI /180.0
theta_f=robot03.Orientation.Y * Math.PI /180.0
theta_=(theta_/180.0)*Math.PI
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp)) //up - //down + this is not right but ...OK
zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
p_x_= (xff-p_xf)
p_z_= (zff-p_zf)
p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f))
ptxz= Math.Atan( p_t_x/p_t_z)
p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
p_jl= ( p_t_z)
p_jd= ( p_t_x)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
matlabfx2[i]=p_xf
matlabfy2[i]=p_zf
matlabfz2[i]=p_yf
matlabtheta2[i]=theta_
matlabderr2[i]=p_ld
matlabtime[i]=i
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if((-ptxz)>(Math.PI))
ptxz= ( ptxz+ 2.0* Math.PI)
if(ptxz>(Math.PI))
ptxz= ( ptxz- 2.0* Math.PI)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// coordinate conversion
if( Math.Abs( wt_l) < 0.01 )
vt_l=vt_l
else
vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l //this change some wrong
p_ld=(kpld2*p_ld / Math.Abs( vt_l))
if(p_ld> (Math.PI /2) )
p_ld= Math.PI /2
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
kpld2=2.0
pxsum2=pxsum2+p_jd
pzsum2=pzsum2+p_jl
absjd = Math.Abs( p_jd)
absjl = Math.Abs( p_jl)
if(absjd1.0)
left=1.0
if(right>1.0)
right=1.0
if(-left>1.0)
left=-1.0
if(-right>1.0)
right=-1.0
robot03.Go(left, right)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
wait 50
//// print thetadt. ToString()
//// print theta_. ToString()
////// theta_
}
// print p_ld. ToString()
// robot03.Go(0, 0)
End
////////Procedure proc_robot_04
//////////follower03
//////// for (i = 0; i < SimTime; i++)
//////// {
//////// vt_l=vtall
//////// wt_l=wtall
//////// vtsign= Math.Sign( vt_l )
//////// wtsign= Math.Sign( wt_l )
//////// if(i<300)
//////// {
//////// lpp = 3.0
//////// wpp= 0
//////// }
//////// else
//////// {
//////// if(i<600)
//////// {
//////// lpp = 2.0
//////// wpp= (- Math.PI /6.0)
//////// }
//////// else
//////// {
//////// lpp = 2.0
//////// wpp= (- Math.PI /2.0) //(- Math.PI /6.0) // Math.PI / 6.0 //Math.PI //left- right +
//////// }
//////// }
//////// lsin= ( lpp* Math.Sin( wpp)) //L'
//////// lcos= ( lpp* Math.Cos( wpp)) //L
//////// thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
//////////////////////////////////////////////////////////////////////////////////////////basic
//////// theta_l=robot01.Orientation.Y
//////// theta_f=robot04.Orientation.Y
//////// p_xl = robot01.Position.X
//////// p_zl = robot01.Position.Z
//////// p_xf = robot04.Position.X
//////// p_zf = robot04.Position.Z
//////// theta_=-(theta_l-theta_f)
//////// if(theta_ > 180.0)
//////// theta_=-(360.0-theta_)
//////// if( -theta_ > 180.0)
//////// theta_=(360.0+theta_)
//////// theta_l=robot01.Orientation.Y * Math.PI /180.0
//////// theta_f=robot04.Orientation.Y * Math.PI /180.0
//////// theta_=(theta_/180.0)*Math.PI
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp)) //up - //down + this is not right but ...OK
//////// zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
//////// p_x_= (xff-p_xf)
//////// p_z_= (zff-p_zf)
//////// p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
//////// p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f))
//////// ptxz= Math.Atan( p_t_x/p_t_z)
//////// p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
//////// p_jl= ( p_t_z)
//////// p_jd= ( p_t_x)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// matlabfx3[i]=p_xf
//////// matlabfy3[i]=p_zf
//////// matlabtheta3[i]=theta_
//////// matlabderr3[i]=p_ld
////////// matlabtime[i]=i
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// if((-ptxz)>(Math.PI))
//////// ptxz= ( ptxz+ 2.0* Math.PI)
//////// if(ptxz>(Math.PI))
//////// ptxz= ( ptxz- 2.0* Math.PI)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// coordinate conversion
//////// if( Math.Abs( wt_l) < 0.01 )
//////// vt_l=vt_l
//////// else
//////// vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l //this change some wrong
//////// p_ld=(kpld2*p_ld / Math.Abs( vt_l))
//////// if(p_ld> (Math.PI /2) )
//////// p_ld= Math.PI /2
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// if(p_ld>1.56)
//////// {
//////// kpld2=4.0
//////// wt_f= (wt_l+(kpwtf*p_jd)*vtsign+(kpwtf*theta_* Math.Cos( p_ld)))
//////// vt_f= (vt_l+kpvtf*p_jl)
//////// }
//////// else
//////// {
//////// kpld2=2.0
//////// pxsum=pxsum+p_jd
//////// pzsum=pzsum+p_jl
//////// wt_f= (wt_l+(kpwtf*p_jd+kiwtf*pxsum)*vtsign+(kpwtf*(theta_-thetadt)* Math.Cos( p_ld))) //+ (kpwtf*theta_* Math.Cos( p_ld)))
//////// vt_f= (vt_l+kpvtf*p_jl+kivtf*pzsum)
//////// }
//////// matlabfvt3[i]=vt_f
//////// matlabfwt3[i]=wt_f
//////// left= vt_f + dprl * wt_f
//////// right=vt_f - dprl * wt_f
//////// if(left>0.6)
//////// left=0.6
//////// if(right>0.6)
//////// right=0.6
//////// if(-left>0.6)
//////// left=-0.6
//////// if(-right>0.6)
//////// right=-0.6
//////// robot04.Go(left, right)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// wait 50
//////// }
////////// robot04.Go(0, 0)
////////End
////////Procedure proc_robot_05
//////////follower04
//////// for (i = 0; i < SimTime; i++)
//////// {
//////// vt_l=vtall
//////// wt_l=wtall
//////// vtsign= Math.Sign( vt_l )
//////// wtsign= Math.Sign( wt_l )
//////// if(i<300)
//////// {
//////// lpp = 4.0
//////// wpp= 0
//////// }
//////// else
//////// {
//////// if(i<600)
//////// {
//////// lpp = 2.0
//////// wpp= ( Math.PI /6.0)
//////// }
//////// else
//////// {
//////// lpp =2.0
//////// wpp= ( Math.PI /2.0) //(- Math.PI /6.0) // Math.PI / 6.0 //Math.PI //left- right +
//////// }
//////// }
//////// lsin= ( lpp* Math.Sin( wpp)) //L'
//////// lcos= ( lpp* Math.Cos( wpp)) //L
//////// thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
//////////////////////////////////////////////////////////////////////////////////////////basic
//////// theta_l=robot01.Orientation.Y
//////// theta_f=robot05.Orientation.Y
//////// p_xl = robot01.Position.X
//////// p_zl = robot01.Position.Z
//////// p_xf = robot05.Position.X
//////// p_zf = robot05.Position.Z
//////// theta_=-(theta_l-theta_f)
//////// if(theta_ > 180.0)
//////// theta_=-(360.0-theta_)
//////// if( -theta_ > 180.0)
//////// theta_=(360.0+theta_)
//////// theta_l=robot01.Orientation.Y * Math.PI /180.0
//////// theta_f=robot05.Orientation.Y * Math.PI /180.0
//////// theta_=(theta_/180.0)*Math.PI
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp)) //up - //down + this is not right but ...OK
//////// zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
//////// p_x_= (xff-p_xf)
//////// p_z_= (zff-p_zf)
//////// p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
//////// p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f))
//////// ptxz= Math.Atan( p_t_x/p_t_z)
//////// p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
//////// p_jl= ( p_t_z)
//////// p_jd= ( p_t_x)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// matlabfx4[i]=p_xf
//////// matlabfy4[i]=p_zf
//////// matlabtheta4[i]=theta_
//////// matlabderr4[i]=p_ld
////////// matlabtime[i]=i
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// if((-ptxz)>(Math.PI))
//////// ptxz= ( ptxz+ 2.0* Math.PI)
//////// if(ptxz>(Math.PI))
//////// ptxz= ( ptxz- 2.0* Math.PI)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// coordinate conversion
//////// if( Math.Abs( wt_l) < 0.01 )
//////// vt_l=vt_l
//////// else
//////// vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l //this change some wrong
//////// p_ld=(kpld2*p_ld / Math.Abs( vt_l))
//////// if(p_ld> (Math.PI /2) )
//////// p_ld= Math.PI /2
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// if(p_ld>1.56)
//////// {
//////// kpld2=4.0
//////// wt_f= (wt_l+(kpwtf*p_jd)*vtsign+(kpwtf*theta_* Math.Cos( p_ld)))
//////// vt_f= (vt_l+kpvtf*p_jl)
//////// }
//////// else
//////// {
//////// kpld2=2.0
//////// pxsum=pxsum+p_jd
//////// pzsum=pzsum+p_jl
//////// wt_f= (wt_l+(kpwtf*p_jd+kiwtf*pxsum)*vtsign+(kpwtf*(theta_-thetadt)* Math.Cos( p_ld))) //+ (kpwtf*theta_* Math.Cos( p_ld)))
//////// vt_f= (vt_l+kpvtf*p_jl+kivtf*pzsum)
//////// }
////////// print p_ld. ToString()
//////// matlabfvt4[i]=vt_f
//////// matlabfwt4[i]=wt_f
//////// left= vt_f + dprl * wt_f
//////// right=vt_f - dprl * wt_f
//////// if(left>0.6)
//////// left=0.6
//////// if(right>0.6)
//////// right=0.6
//////// if(-left>0.6)
//////// left=-0.6
//////// if(-right>0.6)
//////// right=-0.6
//////// robot05.Go(left, right)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////// wait 50
//////// }
////////// robot05.Go(0, 0)
////////End
Procedure proc_line
//// for (i = 0; i < SimTime; i++)
//// {
////// robot01
//// p_x01 = robot01.Position.X
//// p_z01 = robot01.Position.Z
////// robot02
//// p_x02 = robot02.Position.X
//// p_z02 = robot02.Position.Z
////// robot03
//// p_x03 = robot03.Position.X
//// p_z03 = robot03.Position.Z
////// robot04
//// p_x04 = robot04.Position.X
//// p_z04 = robot04.Position.Z
////// robot05
//// p_x05 = robot05.Position.X
//// p_z05 = robot05.Position.Z
////////// robot06
//////// p_x06 = robot06.Position.X
//////// p_z06 = robot06.Position.Z
//// if(i>299)
//// {
//// i=0
//// }
//// if(i==0)
//// {
//// j++
//// AddTextSpriteEntity sprite1{j}
//// /Position:{p_x01} 0.04 {p_z01}
//// /Orientation:-90 180 0
//// /FontSize:32
//// /Text:●
//// /Width:1
//// /Height:1
//// /TextureWidth:100
//// /TextureHeight:100
//// /FontColor:255 255 255 255
//// AddTextSpriteEntity sprite2{j}
//// /Position:{p_x02} 0.04 {p_z02}
//// /Orientation:-90 180 0
//// /FontSize:32
//// /Text:●
//// /Width:1
//// /Height:1
//// /TextureWidth:100
//// /TextureHeight:100
//// /FontColor:255 255 255 255
//// AddTextSpriteEntity sprite3{j}
//// /Position:{p_x03} 0.04 {p_z03}
//// /Orientation:-90 180 0
//// /FontSize:32
//// /Text:●
//// /Width:1
//// /Height:1
//// /TextureWidth:100
//// /TextureHeight:100
//// /FontColor:255 255 255 255
//// AddTextSpriteEntity sprite4{j}
//// /Position:{p_x04} 0.04 {p_z04}
//// /Orientation:-90 180 0
//// /FontSize:32
//// /Text:●
//// /Width:1
//// /Height:1
//// /TextureWidth:100
//// /TextureHeight:100
//// /FontColor:255 255 255 255
//// AddTextSpriteEntity sprite5{j}
//// /Position:{p_x05} 0.04 {p_z05}
//// /Orientation:-90 180 0
//// /FontSize:32
//// /Text:●
//// /Width:1
//// /Height:1
//// /TextureWidth:100
//// /TextureHeight:100
//// /FontColor:255 255 255 255
//// FlushScript
//// }
//// wait 50
//// }
End本文参与 腾讯云自媒体分享计划，分享自作者个人站点/博客。
原始发表：2016年05月09日，如有侵权请联系 cloudcommunity@tencent.com 删除
评论
登录后参与评论
推荐阅读