V-REP机器人控制：基于视觉传感器的路径跟踪算法

function run(P,D) data2={} PD=0 error=0 for i=1,5,1 do res,data=sim.readVisionSensor(floorSensorHandles[i]) if(res>=0)then data2[i]=data[11] end end if(data2[4]<0.73) then data2[3]=0 end if(data2[2]<0.73) then data2[1]=0 end if(data2[2]<data2[4]) then if(data2[3]<data2[1]) then error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85) else error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85) end else if(data2[3]<data2[1]) then error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85) else error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85) end end PD=Perror+D(error-previousError) previousError=error previousdata=data2

if error<0.2 then
    rightV=speed
    leftV=speed
    previousError=0
elseif data2[3]<0.75 or data2[4]<0.75 then
    rightV=speed+PD*speed
    leftV=speed-PD*speed
elseif data2[1]<0.75 or data2[2]<0.75 then
    rightV=speed-PD*speed
    leftV=speed+PD*speed
end
sim.setJointTargetVelocity(leftmotor,leftV)
sim.setJointTargetVelocity(rightmotor,rightV)

end

function sysCall_threadmain() leftmotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor') rightmotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor') car=sim.getObjectHandle('P1') floorSensorHandles={-1,-1,-1,-1,-1} floorSensorHandles[1]=sim.getObjectHandle('l') floorSensorHandles[2]=sim.getObjectHandle('m') floorSensorHandles[3]=sim.getObjectHandle('r') floorSensorHandles[4]=sim.getObjectHandle('m0') floorSensorHandles[5]=sim.getObjectHandle('m2') postion1={-0.09,-1.37} ccc={-1,-1,-1} aaa=-1 bbb=-1 speed=-5 back=-1 previousError=0 previousdata={} flag={0,0,0,0,0,0,0,0} 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}} sim.setIntegerSignal('occupy2',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.setIntegerSignal('arrive2',1) sim.waitForSignal('finish2') sim.clearIntegerSignal('finish2') while true do if(sim.getIntegerSignal('occupy3')==nil) then break end end sim.clearIntegerSignal('occupy2') flag={0,1,0,0,0,0,0,0} while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do position=sim.getObjectPosition(car,-1) 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 sim.setIntegerSignal('occupy1',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive1',1) sim.waitForSignal('finish1') sim.clearIntegerSignal('finish1') while true do if(sim.getIntegerSignal('occupy2')==nil) then break end end sim.clearIntegerSignal('occupy1') flag={1,0,0,0,0,0,0,0} end 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 sim.setIntegerSignal('occupy2',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive2',1) sim.waitForSignal('finish2') sim.clearIntegerSignal('finish2') while true do if(sim.getIntegerSignal('occupy3')==nil) then break end end sim.clearIntegerSignal('occupy2') flag={0,1,0,0,0,0,0,0} end 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 sim.setIntegerSignal('occupy5',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive5',1) sim.waitForSignal('finish5') sim.clearIntegerSignal('finish5') while true do if(sim.getIntegerSignal('occupy6')==nil) then break end end sim.clearIntegerSignal('occupy5') flag={0,0,0,0,1,0,0,0} end 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 sim.setIntegerSignal('occupy6',1) sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) sim.wait(0.8) sim.setIntegerSignal('arrive6',1) sim.waitForSignal('finish6') sim.clearIntegerSignal('finish6') while true do if(sim.getIntegerSignal('occupy7')==nil) then break end end sim.clearIntegerSignal('occupy6') flag={0,0,0,0,0,1,0,0} end 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 sim.setIntegerSignal('occupy3',1) if(sim.getIntegerSignal('occupy4')==nil) then sim.clearIntegerSignal('occupy3') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy4')==nil) then break end end sim.clearIntegerSignal('occupy3') end flag={0,0,1,0,0,0,0,0} end 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 sim.setIntegerSignal('occupy4',1) if(sim.getIntegerSignal('occupy5')==nil) then sim.clearIntegerSignal('occupy4') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy5')==nil) then break end end sim.clearIntegerSignal('occupy4') end flag={0,0,0,1,0,0,0,0} end 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 sim.setIntegerSignal('occupy7',1) if(sim.getIntegerSignal('occupy8')==nil) then sim.clearIntegerSignal('occupy7') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy8')==nil) then break end end sim.clearIntegerSignal('occupy7') end flag={0,0,0,0,0,0,1,0} end 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 sim.setIntegerSignal('occupy8',1) if(sim.getIntegerSignal('occupy1')==nil) then sim.clearIntegerSignal('occupy8') else sim.setJointTargetVelocity(leftmotor,0) sim.setJointTargetVelocity(rightmotor,0) while true do if(sim.getIntegerSignal('occupy1')==nil) then break end end sim.clearIntegerSignal('occupy8') end flag={0,0,0,0,0,0,0,1} end run(0.22,0.06) end end