微软仿真武术机器人比赛国赛this on stage

usingusingSystem.Collections.Generic;usingSystem.Linq;usingSystem.Text;usingWushuStrategy;usingWushuEnvironment;usingSystem.IO;namespace{publicclass{//privatefloat[]basWheelSpeed=newfloat[2];privatefloat[]turnK=newfloat[2];//publicMotion(floatwsL,floatwsR,floattkL,float this.basWheelSpeed[0]=wsL;this.basWheelSpeed[1]=wsR;this.turnK[0]tkL;//躯干左右，用来判断this.turnK[1]=tkR;}publicfloat{return(this.basWheelSpeed[0]*this.turnK[0]);}publicfloatgetRightWS(){return(this.basWheelSpeed[1]*this.turnK[1]);}publicclass{//privatefloat[]acc=new//publicMotionChange(floataL,float{this.acc[0]=aL;this.acc[1]=}publicfloat{returnthis.acc[0];}publicfloatgetAR(){returnthis.acc[1];}publicclass //Fieldspublicintb;publicintg;publicint//publicPoint(intpr,intpg,int{this.r=pr;this.g=pg;this.b=}}publicclass{//publicintx;publicint//publicRotation(intrx,int{this.x=rx;this.y=ry;}publicclass{//publicintbMax;publicintbMin;publicintgMax;publicintpublicintrMax;publicint//publicTargetColor(intr,intg,intb,intR,intG,int{this.rMin=r;this.rMax=R;this.gMin=g;this.gMax=G;this.bMin=b;this.bMax=}}publicclass WushuEnv_wushuEnv=newWushuEnv();publicint_count=0;publicbool_ifNewPictrue;publicbool_ifNewLaserArray;publicfloat_headYaw=0;publicfloat_headPitch=0;publicfloat_leftShoulderPitch=0;publicfloat_leftShoulderRoll=0;publicfloat_lefbowRoll=0;publicfloat_rightShoulderPitch=0;publicfloat_rightShoulderRoll=0;publicfloat_righbowRoll=0;publicfloat_leftWheelSpeed=0;publicfloat_rightWheelSpeed=0;publicVecPositionRobotPos=newVecPosition(0,0);publicfloat_theta=0;privateintDanger1Cycle;privateintpublicint[]_laserArray=newint[17];publicbyte[]_srcPic=null;publicbyte[]_prdPic=publicint[]oppoentRobotInfo=new//publicGameAssistInfor_infor=newpublicDetectInfoDetect=newDetectInfo();//检测对方位置与预测自己运动轨迹publicRunActionMoveRobotnewRunAction();（）的类publicstaticVecPositionoldRobotPosnewVecPosition(0,0);publicfloatOld_LeftSpeed=0;//用来保存上周期的轮速publicfloatOld_RightSpeed=0;publicint_CycleAfterLaser=0;publicstaticVecPositionRobotPos_LastLasernewVecPosition(124,124);privatedouble[]oldX=newdouble[10];privatedouble[]oldY=newdouble[10];privateintcycle=0;privatestaticfloatoldWS=0f;privatestaticfloatnewWS=0f;privatestaticuintcount=0;publicintstartcount=0;publicintHeadColor=1;publicintCenterColor=2;publicintBodyColor=3;publicintGroundColor=4;publicintstate=0;privateintal=0;privateintpublicstringGetTeam()// return"PDSNO.1";}publicvoidcrash(){count=0;this._wushuEnv.jointsAngle[0]=0f;this._wushuEnv.jointsAngle[1]=//intnum=70;floattkL=1f;floattkR=if(this.justDirection()=={tkL=1.8f;tkR=1.3f;elseif(this.justDirection()=={tkL=1.3f;tkR=1.8f;elseif(this.justDirection()=={tkL=1.55f;tkR=1.55f;MoveRobot.LeftSpeed*=tkL;MoveRobot.RightSpeed*=}publicMotion{returnnewMotion(this._wushuEnv.wheelSpeed[0],this._wushuEnv.wheelSpeed[1],1f,1f);}publicbooljustWhite(){inty=for(inti=1;i<=320; Pointpoint=this.see(i,&&(point.b==0xff)){returntrue;}return}publicfloatMin(floatleft,float{if(left> returnright;}returnleft;}publicint{intnum=0;intnum2=for(inti=0;i<8;{if(this._wushuEnv.laserArray[i]>{num2++;}for(intj=9;j<0x11; if(this._wushuEnv.laserArray[j]>{}if(num>{return1;}if(num2>num){return2;if((num2==num)&&(this._wushuEnv.laserArray[8]>{return3;}return-1;}publicbool intx=0;inty=this._wushuEnv.jointsAngle[1]=Rotationrotation=this.picture(this.ChooseColor(this.GroundColor));x=rotation.x;y=rotation.y;if(x>0) if(x<{this._wushuEnv.jointsAngle[0]++;}elseif(x>180){this._wushuEnv.jointsAngle[0]--;if(this._wushuEnv.jointsAngle[0]<-{this._wushuEnv.jointsAngle[0]+=360f;}returntrue;}this._wushuEnv.jointsAngle[0]-=if(this._wushuEnv.jointsAngle[0]<-{this._wushuEnv.jointsAngle[0]+=360f;}returnfalse;}publicMotion{if(count>{if{if((this._wushuEnv.jointsAngle[0]>-110f)&&(this._wushuEnv.jointsAngle[0]<-{returnnewMotion(40f,0f,1f,1f);{if(this._wushuEnv.jointsAngle[0]>-{returnnewMotion(40f,40f,-1f,1f);}if(this._wushuEnv.jointsAngle[0]<-{returnnewMotion(40f,40f,1f,-1f);}returnnewMotion(0f,50f,1f,1f);}returnnewMotion(-40f,40f,1f,}returnnewMotion(0f,0f,0f,}this._wushuEnv.jointsAngle[1]=43f;this._wushuEnv.jointsAngle[0]=0f;if((this._wushuEnv.wheelSpeed[0]>0f)&(this._wushuEnv.wheelSpeed[1]>{returnnewMotion(0f,0f,1f, if(!((this._wushuEnv.wheelSpeed[1]==0f)&(this._wushuEnv.wheelSpeed[1]=={}returnnewMotion(10f,10f,1f,-}publicMotion count=0;this._wushuEnv.jointsAngle[1]=43f;this._wushuEnv.jointsAngle[0]=0f;floatwsL=30f;floattkL=0f;floattkR=0f;floatnum4=0f;wsL=30f;if(this.justDirection()=={tkL=1f;tkR=1.28f;elseif(this.justDirection()=={tkL=1.28f;tkR=1f;elseif(this.justDirection()=={tkL=1.28f;tkR=1.28f;{tkL=1f;tkR=1f;{num4=this.Min(this._wushuEnv.wheelSpeed[0],this._wushuEnv.wheelSpeed[1]);this._wushuEnv.wheelSpeed[0]=num4;this._wushuEnv.wheelSpeed[1]=num4;tkL=1f;tkR=}{tkL=1f;tkR=1f;returnnewMotion(wsL,wsL,tkL,}publicMotion count=0;this._wushuEnv.jointsAngle[0]=0f;this._wushuEnv.jointsAngle[1]=43f;intnum=70;floattkL=1f;floattkR=if(this.justDirection()=={tkL=1.7f;tkR=1.5f;elseif(this.justDirection()=={tkL=1.5f;tkR=1.7f;elseif(this.justDirection()=={tkL=1.85f;tkR=1.85f;returnnewMotion((float)num,(float)num,tkL,}publicMotion count=returnnewMotion(10f,10f,-1f,-}publicMotiongetMotion(ints)//机器人4{switch{case0:returnthis.getLostMotion();case1:returnthis.getAppMotion();case2:returnthis.getCrashMotion();case3:returnthis.getEdgeMotion();}returnnewMotion(0f,0f,1f,}publicTargetColorChooseColor(int{if(Color==this.CenterColor)//正的颜returnnewTargetColor(190,0x1c,0x24,0xc2,0x20,40);}if(Color==this.HeadColor)//头的黑色部分，检测到；{returnnewTargetColor(20,0x16,0x1c,0x1b,0x1c,40);}if(Color==this.BodyColor){returnnewTargetColor(190,0x1c,0x24,0xc0,0x20,40);}if(Color==this.GroundColor){returnnewTargetColor(50,50,50,100,100,100);returnnewTargetColor(0,0,0,0,0,}publicRotation{intrx=0;intry=0;rx=0;ry=for(inti=1;i<=320;{Pointpthis.see(i,1);//黑色的点。。。用来确定rxryif(((p.r==0x1c)&&(p.g==0x1d))&&(p.b=={rx=i;if((rx!=0)&&this.ColorNoise(newRotation(i,1),p,{ry=i;}returnnewRotation(rx,}publicRotationjustBlackL()//{intrx=intry=0;rx=0;ry=for(inti=240;i>=1;i--{Pointpoint=this.see(1,if((rx==0)&&(((point.r==0x1c)&&(point.g==0x1d))&&(point.b=={rx=i;Pointpoint2=this.see(320,if((ry==0)&&(((point2.r==0x1c)&&(point.g==0x1d))&&(point.b=={ry=i;}returnnewRotation(rx,}publicint{floatnum=0f;floatnum2=0f;if(count<5){Rotationrotation=if((rotation.x<30)||(rotation.y<{return3;}for(inti=0;i<0x11;{if((this._wushuEnv.laserArray[i]<0xc3)&&(this._wushuEnv.laserArray[i]>{num++;if(this._wushuEnv.laserArray[i]>{num2++;}if(num>{num=0f;num2=0f;return2;}if(num2>{num2=num=0f;return}num2=0f;num=0f;return0;}publicvoidchangeMotion(MotionChange{if(acc.getAL()!={this._wushuEnv.wheelSpeed[0]+=acc.getAL();}if(acc.getAR()!=0f){this._wushuEnv.wheelSpeed[1]+=}publicfloataccchose(floatgapL,float floatnum=newWS=if((this.getState()==0)&&(this._wushuEnv.wheelSpeed[0]>{num=8f;elseif((this.getState()==1)&&(this._wushuEnv.wheelSpeed[0]>{num=9f;elseif((this.getState()==2)&&(this._wushuEnv.wheelSpeed[0]<{num=6f;} num=4f;if(this.getState()=={num=9.5f;}returnnum;}publicMotionChangegetMotionChange(MotionnowMotion,MotionexpMotion)//al,ar floataL=0f;floataR=0f;floatnum3=9f;floatgapL=0f;floatgapR=0f;floatnum6=gapL=expMotion.getLeftWS()-nowMotion.getLeftWS();gapR=expMotion.getRightWS()-nowMotion.getRightWS();num6=this.accchose(gapL,gapR);if(Math.Abs(gapL)<={if(Math.Abs(gapR)<= this._wushuEnv.wheelSpeed[0]=expMotion.getLeftWS();this._wushuEnv.wheelSpeed[1]=expMotion.getRightWS();aL=0f;aR=} this._wushuEnv.wheelSpeed[0]=expMotion.getLeftWS();aL=0f;this._wushuEnv.wheelSpeed[1]=expMotion.getRightWS();aR=0f;}}elseif(Math.Abs(gapR)<{this._wushuEnv.wheelSpeed[1]=aR=this._wushuEnv.wheelSpeed[0]=expMotion.getLeftWS();aL=0f;}elseif(Math.Abs(gapR)> aL=(Math.Abs(gapL)/gapL)*num6;aR=(gapR/Math.Abs(gapL))*} aR=(Math.Abs(gapR)/gapR)*num6;aL=(gapL/Math.Abs(gapR))*}returnnewMotionChange(aL,}publicboolColorNoise(RotationPixel,Pointp,TargetColor intnum=4;intnum2=2;intnum3=0;num3=0;for(inti=Pixel.y;i<=(Pixel.y+num);{for(intj=Pixel.x;j<=(Pixel.x+num2);{if((((p.r>=color.rMin)&&(p.r<=color.rMax))&&((p.g>=color.gMin)&&<=color.gMax)))&&((p.b>=color.bMin)&&(p.b<={}}return(num3>}publicPointsee(intx,int{intindex=0x36+(((((y-1)*320)+x)-1)*4);returnnewPoint(this._wushuEnv.sourceImage[index+2],this._wushuEnv.sourceImage[index+1],}publicRotationpicture(TargetColor{intlength=this._wushuEnv.sourceImage.GetLength(0);for(inti=1;i<240;i++){for(intj=1;j<320;{Pointp=this.see(j,if(((((p.r>=color.rMin)&&(p.r<=color.rMax))&&((p.g>=color.gMin)&&(p.g<=color.gMax)))&&((p.b>=color.bMin)&&(p.b<=color.bMax)))&&this.ColorNoise(newRotation(j,i),p,color)){returnnewRotation(j,i);}}returnnewRotation(-1,-}publicvoid{intfor(num=0;num<9;{this.oldX[num]=this.oldX[num+1];this.oldY[num]=this.oldY[num+}this.oldX[9]=RobotPos.GetX();this.oldY[9]=RobotPos.GetY();if(this.cycle<10){}||((this.Detect.MotionAngle-((RobotPos-oldRobotPos)).GetdirectionDeg())<-30.0))||(this.PredictPos(1).GetDistanceTo(RobotPos)>8.0)){for(num=0;num<8;{this.oldX[num]=this.oldY[num]=}this.cycle=}doublenum2=0.0;doublenum3=0.0;doublenum4=0.0;doublenum5=for(num=0;num<this.cycle;{num2+=this.oldX[(10-num)-1];num3+=this.oldY[(10-num)-num4+=this.oldX[(10-num)-1]*this.oldY[(10-num)-1];num5+=this.oldX[(10-num)-1]*this.oldX[(10-num)-}num2/=(double)this.cycle;num3/=(double)this.cycle;num4/=(double)this.cycle;num5/=doublenum6=num5-(num2*num2);if(num6==0.0){this.Detect.MotionVelocity=this.Detect.MotionLine.SetLine(0.0,0.0,}{this.Detect.MotionLine.m_a=-1.0;this.Detect.MotionLine.m_b=(num4-(num2*num3))/num6;this.Detect.MotionLine.m_c=num3-(this.Detect.MotionLine.m_b*num2);VecPositionpos=newVecPosition(this.oldX[9],this.oldY[9]);VecPositionposition2=newVecPosition(this.oldX[10-this.cycle],this.oldY[10VecPositionpointOnLineClosestTo=VecPositionposition4=this.Detect.MotionAngle=(pointOnLineClosestTo-getmotionif(this.cycle<{this.cycle=2;((double)(this.cycle-1));}if(this.Detect.MotionVelocity>{this.Detect.MotionVelocity=0.0;}publicWushuEnvStrategyStep(WushuEnv{this._wushuEnv=this._count=this._wushuEnv.countDown;if(((this._count<10)||(this.RobotPos.GetDistanceTo(newVecPosition(25.0,<1.0))||(this.RobotPos.GetDistanceTo(newVecPosition(223.0,24.0))<{this.AlarmCycle=this.sidestep=this.Danger1Cycle=this.Danger4Cycle=}if(this._count<{} returnthis._wushuEnv;}publicvoid{_ifNewPictrue=_ifNewLaserArray=_theta=_wushuEnv.robotRotation[1]-90; if(_theta<-180)_theta+=360;this.RobotPos.SetX((double)(124f+(this._wushuEnv.robotPosition[0]*100f)));this.RobotPos.SetY((double)(124f-(this._wushuEnv.robotPosition[2]*if(_ifNewPictrue==true) _srcPic=_wushuEnv.sourceImage;if{_CycleAfterLaser=RobotPos_LastLaser=}oldRobotPosRobotPos;//每个周期重新赋值MoveRobot.posRobot=RobotPos;MoveRobot.theta=_theta*Math.PI/ }publicvoid{_leftWheelSpeed=_rightWheelSpeed=MoveRobot.RightSpeed;if(this.Detect.MotionVelocity>0.7){if((this.Old_LeftSpeed>0f)&&((this._leftWheelSpeed-this.Old_LeftSpeed)-{this._leftWheelSpeed=this.Old_LeftSpeed-}if((this.Old_LeftSpeed<0f)&&((this._leftWheelSpeed-this.Old_LeftSpeed){this._leftWheelSpeed=this.Old_LeftSpeed+}if((this.Old_RightSpeed>0f)&&((this._rightWheelSpeed-this.Old_RightSpeed)-{this._rightWheelSpeed=this.Old_RightSpeed-}if((this.Old_RightSpeed<0f)&&((this._rightWheelSpeed-this.Old_RightSpeed){this._rightWheelSpeed=this.Old_RightSpeed+}}_wushuEnv.wheelSpeed=newfloat[2]{_leftWheelSpeed,_rightWheelSpeed_wushuEnv.jointsAngle=newfloat[8]{_headYaw_headPitch_lef_rightShoulderPitch_rightShoulderRoll_righbowRollOld_LeftSpeed=_leftWheelSpeed;Old_RightSpeed=_rightWheelSpeed;oldRobotPos=RobotPos;}publicint{intnum;intnum2;intVecPositionp=newVecPosition(124.0,124.0);if(RobotPos.GetDistanceTo(p)<30.0){num=} num=2;if(this.Detect.Enemy_Pos.GetDistanceTo(p)< num2=1; num2=2;}if(num=={if(num=={num3=0;}{VecPositionposition2=RobotPos-VecPositionposition3=this.Detect.Enemy_Pos-p;doubledirectionDeg=position2.GetdirectionDeg();doublenum5=position3.GetdirectionDeg();if(VecPosition.NormalizeAngleDeg(directionDeg-num5)< num3=1;} num3=2;}}elseif(num== num3=3;} num3=4;if(!this.OnStageOrNot(this.Detect.Enemy_Pos, num3=5;}returnnum3;}publicint{intnum;intnum2;intVecPositionp=newVecPosition(124.0,124.0);if(RobotPos.GetDistanceTo(p)<95.0){num=1;}{num=2;if(this.Detect.Enemy_Pos.GetDistanceTo(p)< num2=1;} num2=2;}if(num=={if(num== num3=0;}{VecPositionposition2=RobotPos-VecPositionposition3=this.Detect.Enemy_Pos-p;doubledirectionDeg=position2.GetdirectionDeg();doublenum5=position3.GetdirectionDeg();if(VecPosition.NormalizeAngleDeg(directionDeg-num5)< num3=1;} num3=2;}}elseif(num== num3=3;} num3= if(!this.OnStageOrNot(this.Detect.Enemy_Pos, num3=5;}returnnum3;}publicvoid{boolboolflag4;doubleVecPositionp=newVecPosition(124.0,if(Math.Sign((float)(this.Old_LeftSpeed+this.Old_RightSpeed))>0)//ifmoveforward,goon{num2=}if(Math.Abs(VecPosition.NormalizeAngleDeg(num2-this.Detect.MotionAngle))>30.0){if(this.Danger1Cycle> flag=true;this.Danger1Cycle=3;} flag=false;}{flag=false;this.Danger1Cycle=0;}this.Detect.Danger_BePushed=flag&&(this.Detect.MotionVelocity>LineperpendicularLine=Line.MakeLineFromPositionAndAngle(RobotPos,(this._theta*3.1415926535897931)/180.0).GetPerpendicularLine(RobotPos);VecPositionpointInLine=perpendicularLine.GetPointInLine(RobotPos,10.0);VecPositionposition3=perpendicularLine.GetPointInLine(RobotPos,-10.0);boolflag2=!this.OnStageOrNot(pointInLine,3.0)||this.Detect.Danger_WheelOut=flag2;VecPositionposition4=this.PredictPos(2);VecPositionposition5=this.PredictPos(0x10);this.Detect.Danger_GoingToOut=flag3;if(position5.GetDistanceTo(p)>{if(this.Danger4Cycle>{flag4=true;this.Danger4Cycle=3;}{flag4=}}{flag4=false;this.Danger4Cycle=0;}this.Detect.Danger_DepartCenter=flag4&&(this.Detect.MotionVelocity>}publicboolOnStageOrNot(VecPositionP,double{boolflag=if((((P.GetX()>MarginSize)&&(P.GetX()<(248.0-MarginSize)))&&(P.GetY()>MarginSize))&&(P.GetY()<(248.0-MarginSize))){flag=}return}publicVecPositionPredictPos(int{if(nCycle<{nCycle=}return(RobotPos+((VecPosition)((newVecPosition(1.0,(this.Detect.MotionAngle*3.1415926535897931)/180.0,true)*nCycle)*this.Detect.MotionVelocity)));}publicintsidestep=0;publicintAlarmCycle=0;publicvoidStrategy2(){VecPositionposition2=newVecPosition(124.0,124.0);VecPositionp=newVecPosition(124.0,124.0);if(this.Detect.Danger_RedAlarm){//Console.Wriine("！！-------------进入处理模 if(RobotPos.GetDistanceTo(p)<{}{doubledistanceTo=doublenum2=if(distanceTo< num2=5.0;elseif(distanceTo< num2=7.0;elseif(distanceTo<{num2=8.0;elseif(distanceTo< num2=9.0;position2=RobotPos+((VecPosition)(num2*(p-RobotPos)));this.MoveRobot.ToPositionSaveLife(position2,100.0);}if{this.Detect.Danger_RedAlarm=false;this.AlarmCycle=0;}}if(this.sidestep>{if(this.sidestep== this.sidestep=0;}}if{if(RobotPos.GetDistanceTo(p)>{this.MoveRobot.ToPositionPID(p,}{this.MoveRobot.RobotSpin(15.0);}{intnum3=doublenum4=RobotPos.GetDistanceTo(this.Detect.Enemy_Pos);switch(num3) casecaseif(((num4>=10.0)||||{if(num4<{this.MoveRobot.InitSideStep(this.Detect.Enemy_Pos);this.sidestep=1;}}gotoLabel_04BF;case1:if(RobotPos.GetDistanceTo(p)>={if(num4<{position2=RobotPos+((VecPosition)(15.0*(pthis.MoveRobot.ToPositionPIDN(position2,100.0);if(this.Detect.MotionVelocity<0.5){}}{this.MoveRobot.ToPositionPID(this.Detect.Enemy_Pos,}}{this.MoveRobot.ToPositionPID(p,40.0);}gotoLabel_04BF;}if(RobotPos.GetDistanceTo(p)> this.MoveRobot.ToPositionPID(p,40.0);} this.MoveRobot.TurnToPositionPD(this.Detect.Enemy_Pos);}if{if{this.MoveRobot.ToPositionSaveLife(p,100.0);elseif(this.Detect.Enemy_ifDetect&&(this.Detect.Enemy_Pos.GetDistanceTo(p)> this.MoveRobot.ToPositionPID(p,40.0);}if((this.Detect.Danger_BePushed&&this.Detect.Danger_DepartCenter)&&!this.Detect.Enemy_ifDetect){if(this.AlarmCycle>{this.Detect.Danger_RedAlarm=}}}publicdoubleRVelocitySlow(double {if((this.Old_RightSpeed-Rv)>30||(this.Old_RightSpeed-Rv)<-30)return((this.Old_RightSpeed+Rv)/2);elsereturn}publicvoidStrategy()//final3.0版的决策 2010.7.153daysbeforecontest VecPositionposition2=newVecPosition(100.0,124.0);VecPositionp=newVecPosition(124.0,124.0);if(this.Detect.Danger_RedAlarm){if(RobotPos.GetDistanceTo(p)<{this.MoveRobot.RobotSpin(40.0);}{doubledistanceTo=RobotPos.GetDistanceTo(p);doublenum2=8.0;if(distanceTo<{num2=5.0;elseif(distanceTo< num2=7.0;elseif(distanceTo<{num2=8.0;elseif(distanceTo< num2=9.0;position2=RobotPos+((VecPosition)(num2*(p-RobotPos)));this.MoveRobot.ToPositionSaveLife(position2,100.0);//为什么是这个函数}if this.Detect.Danger_RedAlarm=false;this.AlarmCycle=0;}}if(this.sidestep> if(this.sidestep=={this.sidestep=0;}}if{if(RobotPos.GetDistanceTo(p)>{this.MoveRobot.ToPositionPID(p,40.0);}{this.MoveRobot.RobotSpin(15.0);}{intareaNo=doublenum4=RobotPos.GetDistanceTo(this.Detect.Enemy_Pos);switch(areaNo) casecaseif(((num4>=10.0)||||{if(num4<20.0)//{position2=RobotPos+((VecPosition)(5.0*-this.MoveRobot.ToPositionPIDN(position2,200.0);if(this.Detect.MotionVelocity<0.5){//}}elseif(num4<{position2=RobotPos+((VecPosition)(1.5*-this.MoveRobot.ToPositionPID(position2,}{this.MoveRobot.ToPositionPID(this.Detect.Enemy_Pos,}}{this.sidestep=1;}gotocasecaseif(RobotPos.GetDistanceTo(p)>={if(num4<{-this.MoveRobot.ToPositionPIDN(position2,100.0);}{this.MoveRobot.ToPositionPID(this.Detect.Enemy_Pos,}}elseif(num4>={this.MoveRobot.ToPositionPID(this.Detect.Enemy_Pos,}{position2=RobotPos+((VecPosition)(5.0*(this.Detect.Enemy_Posthis.MoveRobot.ToPositionPIDN(position2,140.0);if(this.Detect.MotionVelocity<0.05){}}goto}if(RobotPos.GetDistanceTo(p)>{this.MoveRobot.ToPositionPID(p,}{}}if{if{this.MoveRobot.ToPositionSaveLife(p,}elseif(this.Detect.Enemy_ifDetect&&(this.Detect.Enemy_Pos.GetDistanceTo(p)>{this.MoveRobot.ToPositionPID(p,}}if((this.Detect.Danger_BePushed&&this.Detect.Danger_DepartCenter)&&!this.Detect.Enemy_ifDetect){if(this.AlarmCycle>{this.Detect.Danger_RedAlarm=}}}publicvoid{for(inti=0;i<17;i++{_laserArray[i]_wushuEnv.laserArray[16i];}}publicvoid{intdoubledirection0;doubledistance=0;doublebeta;//角度for(inti=0;i<17;i++{if{distance+=_laserArray[i];direction+=i;}}if(total=={Detect.Enemy_ifDetect=false;return;}{Detect.Enemy_ifDetect=//res.count=distance=distancetotal10;10转换为cmdirection/=total;//中间的那个，来判断方向direction-=direction*=5;//direction是监测到目标的平均激光指向与小车朝向的夹角左边为负beta=VecPosition.NormalizeAngleDeg(_theta-direction);Detect.Enemy_Pos.SetX(distance*Math.Cos(beta*Math.PI/180));Detect.Enemy_Pos.SetY(distance*Math.Sin(beta*Math.PI/180));Detect.Enemy_Pos=Detect.Enemy_Pos+RobotPos;//Console.Wriine("!!!!!!!!!!!!!!!!!监测到//Console.Wriine("探测到的坐标："+Detect.Enemy_Pos.GetX()+""+//Console.Wriine("distance:"+}}publicvoid{//Console.Wriine(justBlackL().x+ "+ Console.Wriine("计数器countDown："+ 激光刷新过后周期计数stringoutput"\n"X:RobotPos.GetX().ToString()+"Y:Console.Wristringoutput1_theta.ToString();stringoutput2_leftWheelSpeed.ToString()Console.Wriine("\n关节角度左：ShoulderPitch"+_leftShoulderPitch+"