机器人视觉传感器数据处理和控制函数 - run(P,D)

///'function 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///