vrep小车寻迹代码
vrep小车寻迹代码
前段时间的大作业,负责的寻迹部分,现在把这部分的代码贴上来,具体建模的过程b站上有很多教学视频,就不赘述了。
注意要使黑色轨迹略高于地面,同时视觉传感器的蓝色线框部分高于黑色轨迹。
function sysCall_init()LeftMotor=sim.getObjectHandle('leftmotor')--马达RightMotor=sim.getObjectHandle('rightmotor')floorSenorHandles={-1,-1}floorSenorHandles[1]=sim.getObjectHandle('v0')--视觉传感器floorSenorHandles[2]=sim.getObjectHandle('v1')speed=1back=-1endfunction sysCall_actuation() senorReading={false,false}for i=1,2,1 doresult,data=sim.readVisionSensor(floorSenorHandles[i])if (result>=0) thensenorReading[i]=(data[11]<0.3)--判断是否检测到黑线endendprint(senorReading)rightV=speedleftV=speedif senorReading[1] then --右边检测到黑线,左拐leftV=0.03*speedrightV=5*speedendif senorReading[2] then--左边检测到黑线,右拐leftV=5*speedrightV=0.03*speedendif (back<sim.getSimulationTime()) then sim.setJointTargetVelocity(LeftMotor,leftV)sim.setJointTargetVelocity(RightMotor,rightV)endend
--下面的都保持原样,无需修改
function sysCall_sensing()-- put your sensing code here
endfunction sysCall_cleanup()-- do some clean-up here
end-- See the user manual or the available code snippets for additional callback functions and details