视觉传感器数据分析与电机控制算法 - 机器人运动控制函数run(P, D)

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+PDspeed leftV=speed-PDspeed elseif data2[1]<0.75 or data2[2]<0.75 then rightV=speed-PDspeed leftV=speed+PDspeed end sim.setJointTargetVelocity(leftmotor,leftV) sim.setJointTargetVelocity(rightmotor,rightV) end