Lua 脚本实现机器人仿真环境自动移动

该 Lua 脚本实现机器人在仿真环境中自动移动，主要包括两个函数：run 和 sysCall_threadmain。

sysCall_threadmain 是主函数，负责控制机器人的移动和停止。它首先获取左右轮的句柄、地面传感器的句柄，以及机器人的句柄。然后通过不断循环读取机器人的位置来实现机器人的移动和停止。当机器人到达目标位置时，会发送一个信号给仿真环境，然后等待下一个目标位置的信号。

run 函数用于计算机器人的速度。它首先读取地面传感器的数据，然后根据传感器的数据计算机器人的偏差值，并根据偏差值计算机器人的速度。最后将速度设置给左右轮，实现机器人的移动。

具体代码如下：

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=P*error+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,}
    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[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

该脚本实现了机器人在仿真环境中自动移动的功能，可以根据需求调整目标位置和移动速度。