协程控制程序：基于视觉传感器和PD控制的机器人运动控制

function sysCall_init()\n corout=coroutine.create(coroutineMain)\nend\n\nfunction sysCall_actuation()\n if coroutine.status(corout)~='dead' then\n local ok,errorMsg=coroutine.resume(corout)\n if errorMsg then\n error(debug.traceback(corout,errorMsg),2)\n end\n end\nend\n\nfunction run(P,D)\n data2={}\n PD=0\n error=0\n for i=1,5,1 do\n res,data=sim.readVisionSensor(floorSensorHandles[i])\n if(res>=0)then\n data2[i]=data[11]\n end\n end\n if(data2[4]<0.73) then\n data2[3]=0\n end\n if(data2[2]<0.73) then\n data2[1]=0\n end\n if(data2[2]<data2[4]) then\n if(data2[3]<data2[1]) then\n error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85)\n else\n error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85)\n end\n else\n if(data2[3]<data2[1]) then\n error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85)\n else\n error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85)\n end\n end\n PD=Perror+D(error-previousError)\n previousError=error\n previousdata=data2\n \n if error<0.2 then\n rightV=speed\n leftV=speed\n previousError=0\n elseif data2[3]<0.75 or data2[4]<0.75 then\n rightV=speed+PDspeed\n leftV=speed-PDspeed\n elseif data2[1]<0.75 or data2[2]<0.75 then\n rightV=speed-PDspeed\n leftV=speed+PDspeed\n end\n sim.setJointTargetVelocity(leftmotor,leftV)\n sim.setJointTargetVelocity(rightmotor,rightV)\nend\n\nfunction coroutineMain()\n leftmotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor')\n rightmotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor')\n car=sim.getObjectHandle('P1')\n floorSensorHandles={-1,-1,-1,-1,-1}\n floorSensorHandles[1]=sim.getObjectHandle('l')\n floorSensorHandles[2]=sim.getObjectHandle('m')\n floorSensorHandles[3]=sim.getObjectHandle('r')\n floorSensorHandles[4]=sim.getObjectHandle('m0')\n floorSensorHandles[5]=sim.getObjectHandle('m2')\n postion1={-0.09,-1.37}\n ccc={-1,-1,-1}\n aaa=-1\n bbb=-1\n speed=-5\n back=-1\n previousError=0\n previousdata={}\n flag={0,0,0,0,0,0,0,0}\n stop={{-0.1,0.099},{-0.731,0.098},{-1.302,0.104},{-1.469,1.263},{-0.782,1.299},{0.238,1.296},{0.822,1.284},{0.6,0.131}}\n sim.setInt32Signal('occupy2',1)\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n sim.setInt32Signal('arrive2',1)\n sim.waitForSignal('finish2')\n sim.clearInt32Signal('finish2')\n while true do\n if(sim.getInt32Signal('occupy3')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy2')\n flag={0,1,0,0,0,0,0,0}\n while true do\n position=sim.getObjectPosition(car,-1)\n if (math.abs(position[1]-stop[1][1])<0.05)and(math.abs(position[2]-stop[1][2])<0.05) and (flag[1]==0) then\n sim.setInt32Signal('occupy1',1)\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n sim.wait(0.8)\n sim.setInt32Signal('arrive1',1)\n sim.waitForSignal('finish1')\n sim.clearInt32Signal('finish1')\n while true do\n if(sim.getInt32Signal('occupy2')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy1')\n flag={1,0,0,0,0,0,0,0}\n end\n if (math.abs(position[1]-stop[2][1])<0.05)and(math.abs(position[2]-stop[2][2])<0.05) and (flag[2]==0) then\n sim.setInt32Signal('occupy2',1)\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n sim.wait(0.8)\n sim.setInt32Signal('arrive2',1)\n sim.waitForSignal('finish2')\n sim.clearInt32Signal('finish2')\n while true do\n if(sim.getInt32Signal('occupy3')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy2')\n flag={0,1,0,0,0,0,0,0}\n end\n if (math.abs(position[1]-stop[5][1])<0.05)and(math.abs(position[2]-stop[5][2])<0.05) and (flag[5]==0) then\n sim.setInt32Signal('occupy5',1)\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n sim.wait(0.8)\n sim.setInt32Signal('arrive5',1)\n sim.waitForSignal('finish5')\n sim.clearInt32Signal('finish5')\n while true do\n if(sim.getInt32Signal('occupy6')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy5')\n flag={0,0,0,0,1,0,0,0}\n end\n if (math.abs(position[1]-stop[6][1])<0.05)and(math.abs(position[2]-stop[6][2])<0.05) and (flag[6]==0) then\n sim.setInt32Signal('occupy6',1)\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n sim.wait(0.8)\n sim.setInt32Signal('arrive6',1)\n sim.waitForSignal('finish6')\n sim.clearInt32Signal('finish6')\n while true do\n if(sim.getInt32Signal('occupy7')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy6')\n flag={0,0,0,0,0,1,0,0}\n end\n if (math.abs(position[1]-stop[3][1])<0.05)and(math.abs(position[2]-stop[3][2])<0.05) and (flag[3]==0) then\n sim.setInt32Signal('occupy3',1)\n if(sim.getInt32Signal('occupy4')==nil) then\n sim.clearInt32Signal('occupy3')\n else\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n while true do\n if(sim.getInt32Signal('occupy4')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy3')\n end\n flag={0,0,1,0,0,0,0,0}\n end\n if (math.abs(position[1]-stop[4][1])<0.05)and(math.abs(position[2]-stop[4][2])<0.05) and (flag[4]==0) then\n sim.setInt32Signal('occupy4',1)\n if(sim.getInt32Signal('occupy5')==nil) then\n sim.clearInt32Signal('occupy4')\n else\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n while true do\n if(sim.getInt32Signal('occupy5')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy4')\n end\n flag={0,0,0,1,0,0,0,0}\n end\n if (math.abs(position[1]-stop[7][1])<0.05)and(math.abs(position[2]-stop[7][2])<0.05) and (flag[7]==0) then\n sim.setInt32Signal('occupy7',1)\n if(sim.getInt32Signal('occupy8')==nil) then\n sim.clearInt32Signal('occupy7')\n else\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n while true do\n if(sim.getInt32Signal('occupy8')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy7')\n end\n flag={0,0,0,0,0,0,1,0}\n end\n if (math.abs(position[1]-stop[8][1])<0.05)and(math.abs(position[2]-stop[8][2])<0.05) and (flag[8]==0) then\n sim.setInt32Signal('occupy8',1)\n if(sim.getInt32Signal('occupy1')==nil) then\n sim.clearInt32Signal('occupy8')\n else\n sim.setJointTargetVelocity(leftmotor,0)\n sim.setJointTargetVelocity(rightmotor,0)\n while true do\n if(sim.getInt32Signal('occupy1')==nil) then\n break\n end\n end\n sim.clearInt32Signal('occupy8')\n end\n flag={0,0,0,0,0,0,0,1}\n end\n run(0.22,0.06)\n end\nend