Simulating a Robot in CoppeliaSim: Navigation and Synchronization

function sysCall_threadmain() \n\t\tleftmotor=sim.getObjectHandle("Pioneer_p3dx_leftMotor")\n\t\trightmotor=sim.getObjectHandle("Pioneer_p3dx_rightMotor")\n\t\tcar=sim.getObjectHandle("P1")\n\t\tfloorSensorHandles={-1,-1,-1,-1,-1}\n\t\tfloorSensorHandles[1]=sim.getObjectHandle("l")\n\t\tfloorSensorHandles[2]=sim.getObjectHandle("m")\n\t\tfloorSensorHandles[3]=sim.getObjectHandle("r")\n\t\tfloorSensorHandles[4]=sim.getObjectHandle("m0")\n\t\tfloorSensorHandles[5]=sim.getObjectHandle("m2")\n\t\tpostion1={-0.09,-1.37}\n\t\tccc={-1,-1,-1}\n\t\taaa=-1\n\t\tbbb=-1\n\t\tspeed=-5\n\t\tback=-1\n\t\tpreviousError=0\n\t\tpreviousdata={}\n\t\tflag={0,0,0,0,0,0,0,0}\n\t\tstop={{-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\t\tsim.setIntegerSignal('occupy2',1)\n\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\tsim.setIntegerSignal('arrive2',1)\n\t\tsim.waitForSignal('finish2')\n\t\tsim.clearIntegerSignal('finish2')\n\t\twhile true do\n\t\t\tif(sim.getIntegerSignal('occupy3')==nil) then\n\t\t\t\tbreak\n\t\t\tend\n\t\tend\n\t\tsim.clearIntegerSignal('occupy2')\n\t\tflag={0,1,0,0,0,0,0,0}\n\t\twhile sim.getSimulationState()~=sim.simulation_advancing_abouttostop do\n\t\t\tposition=sim.getObjectPosition(car,-1)\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy1',1)\n\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\tsim.wait(0.8)\n\t\t\t\tsim.setIntegerSignal('arrive1',1)\n\t\t\t\tsim.waitForSignal('finish1')\n\t\t\t\tsim.clearIntegerSignal('finish1')\n\t\t\t\twhile true do\n\t\t\t\t\tif(sim.getIntegerSignal('occupy2')==nil) then\n\t\t\t\t\t\tbreak\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\t\tsim.clearIntegerSignal('occupy1')\n\t\t\t\tflag={1,0,0,0,0,0,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy2',1)\n\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\tsim.wait(0.8)\n\t\t\t\tsim.setIntegerSignal('arrive2',1)\n\t\t\t\tsim.waitForSignal('finish2')\n\t\t\t\tsim.clearIntegerSignal('finish2')\n\t\t\t\twhile true do\n\t\t\t\t\tif(sim.getIntegerSignal('occupy3')==nil) then\n\t\t\t\t\t\tbreak\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\t\tsim.clearIntegerSignal('occupy2')\n\t\t\t\tflag={0,1,0,0,0,0,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy5',1)\n\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\tsim.wait(0.8)\n\t\t\t\tsim.setIntegerSignal('arrive5',1)\n\t\t\t\tsim.waitForSignal('finish5')\n\t\t\t\tsim.clearIntegerSignal('finish5')\n\t\t\t\twhile true do\n\t\t\t\t\tif(sim.getIntegerSignal('occupy6')==nil) then\n\t\t\t\t\t\tbreak\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\t\tsim.clearIntegerSignal('occupy5')\n\t\t\t\tflag={0,0,0,0,1,0,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy6',1)\n\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\tsim.wait(0.8)\n\t\t\t\tsim.setIntegerSignal('arrive6',1)\n\t\t\t\tsim.waitForSignal('finish6')\n\t\t\t\tsim.clearIntegerSignal('finish6')\n\t\t\t\twhile true do\n\t\t\t\t\tif(sim.getIntegerSignal('occupy7')==nil) then\n\t\t\t\t\t\tbreak\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\t\tsim.clearIntegerSignal('occupy6')\n\t\t\t\tflag={0,0,0,0,0,1,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy3',1)\n\t\t\t\tif(sim.getIntegerSignal('occupy4')==nil) then\n\t\t\t\t\tsim.clearIntegerSignal('occupy3')\n\t\t\t\telse\n\t\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\t\twhile true do\n\t\t\t\t\t\tif(sim.getIntegerSignal('occupy4')==nil) then\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tsim.clearIntegerSignal('occupy3')\n\t\t\t\tend\n\t\t\t\tflag={0,0,1,0,0,0,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy4',1)\n\t\t\t\tif(sim.getIntegerSignal('occupy5')==nil) then\n\t\t\t\t\tsim.clearIntegerSignal('occupy4')\n\t\t\t\telse\n\t\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\t\twhile true do\n\t\t\t\t\t\tif(sim.getIntegerSignal('occupy5')==nil) then\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tsim.clearIntegerSignal('occupy4')\n\t\t\t\tend\n\t\t\t\tflag={0,0,0,1,0,0,0,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy7',1)\n\t\t\t\tif(sim.getIntegerSignal('occupy8')==nil) then\n\t\t\t\t\tsim.clearIntegerSignal('occupy7')\n\t\t\t\telse\n\t\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\t\twhile true do\n\t\t\t\t\t\tif(sim.getIntegerSignal('occupy8')==nil) then\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tsim.clearIntegerSignal('occupy7')\n\t\t\t\tend\n\t\t\t\tflag={0,0,0,0,0,0,1,0}\n\t\t\tend\n\t\t\tif (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\t\t\t\tsim.setIntegerSignal('occupy8',1)\n\t\t\t\tif(sim.getIntegerSignal('occupy1')==nil) then\n\t\t\t\t\tsim.clearIntegerSignal('occupy8')\n\t\t\t\telse\n\t\t\t\t\tsim.setJointTargetVelocity(leftmotor,0)\n\t\t\t\t\tsim.setJointTargetVelocity(rightmotor,0)\n\t\t\t\t\twhile true do\n\t\t\t\t\t\tif(sim.getIntegerSignal('occupy1')==nil) then\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tsim.clearIntegerSignal('occupy8')\n\t\t\t\tend\n\t\t\t\tflag={0,0,0,0,0,0,0,1}\n\t\t\tend\n\t\t\trun(0.22,0.06)\n\t\tend\n\tend