1.3 Vrep例程之BubbleRob扩展（循迹移动）

---

扩展例程1.1的BubbleRob使得小车能沿设定路线移动，最终效果图

---

创建视觉传感器

• 首先创建要放在BubbleRob上的三个视觉传感器，即 [Menu bar --> Add --> Vision sensor --> Orthographic type]，然后修改其性质，如图
  
• 当然，为了循迹，每个视觉传感器都要面向地面，则设置它们姿态，即在姿态对话框中设置 [180;0;0] for the Alpha-Beta-Gamma items

---

• 下面要读取视觉传感器数据，我们这里的情况较简单，因为我们视觉传感器只有one pixel，所以我们只要查询视觉传感器所读到图像的平均强度值就行了。对于复杂情况，要设置传感器的filters。

We have several possibilities to read a vision sensor. Since our vision sensor has just one pixel and operates in an easy way, we will simply query the average intensity value of the image read by our vision sensor. For more complex cases, we could have set-up several vision sensor filters.

---

接着，修改层次树，attach三个视觉传感器，如图

---

接着设置三个视觉传感器的位置，按如下绝对坐标设置
left sensor: [0.2;0.042;0.018]
middle sensor: [0.2;0;0.018]
right sensor: [0.2;-0.042;0.018]

---

设置路径

具体操作：[Menu bar --> Add --> Path --> Circle type]，接着我们有两种方法调整路径

1. Ctrl+选择控制点，拖拽
2. 进入path编辑模式，有更高的灵活性（推荐）
   

---

搞掂path形状，接着uncheck Show orientation of points, Show path line and Show current position on path （在path properties中）接着点击 Show path shaping dialog，这将会打开path shaping对话框.，点击 Path shaping enabled，设置type为horizontal segmen以及Scaling factor为 4.0，最后一步为了避免Z轴冲突（floor的Z轴位置与path的Z轴位置冲突，影响视觉传感器结果），所以将path的Z轴向上提0.5mm

---

绑定脚本

脚本定义小车循迹行为，创建一个child script，代码如下

function speedChange_callback(ui,id,newVal)
    speed=minMaxSpeed[1]+(minMaxSpeed[2]-minMaxSpeed[1])*newVal/100
end

function sysCall_init()
    -- This is executed exactly once, the first time this script is executed
    bubbleRobBase=sim.getObjectAssociatedWithScript(sim.handle_self)
    leftMotor=sim.getObjectHandle("leftMotor")
    rightMotor=sim.getObjectHandle("rightMotor")
    noseSensor=sim.getObjectHandle("sensingNose")
    minMaxSpeed={50*math.pi/180,300*math.pi/180}
    backUntilTime=-1 -- Tells whether bubbleRob is in forward or backward mode
    floorSensorHandles={-1,-1,-1}
    floorSensorHandles[1]=sim.getObjectHandle("leftSensor")
    floorSensorHandles[2]=sim.getObjectHandle("middleSensor")
    floorSensorHandles[3]=sim.getObjectHandle("rightSensor")
    -- Create the custom UI:
        xml = '<ui title="'..sim.getObjectName(bubbleRobBase)..' speed" closeable="false" resizeable="false" activate="false">'..[[
        <hslider minimum="0" maximum="100" onchange="speedChange_callback" id="1"/>
        <label text="" style="* {margin-left: 300px;}"/>
        </ui>
        ]]
    ui=simUI.reate(xml)
    speed=(minMaxSpeed[1]+minMaxSpeed[2])*0.5
    simUI.setSliderValue(ui,1,100*(speed-minMaxSpeed[1])/(minMaxSpeed[2]-minMaxSpeed[1]))
end

function sysCall_actuation()
    result=sim.readProximitySensor(noseSensor)
    if (result>0) then backUntilTime=sim.getSimulationTime()+4 end

    -- read the line detection sensors:
    sensorReading={false,false,false}
    for i=1,3,1 do
        result,data=sim.readVisionSensor(floorSensorHandles[i])
        if (result>=0) then
            sensorReading[i]=(data[11]<0.3) -- data[11] is the average of intensity of the image
        end
        print(sensorReading[i])
    end

    -- compute left and right velocities to follow the detected line:
    rightV=speed
    leftV=speed
    if sensorReading[1] then
        leftV=0.03*speed
    end
    if sensorReading[3] then
        rightV=0.03*speed
    end
    if sensorReading[1] and sensorReading[3] then
        backUntilTime=sim.getSimulationTime()+2
    end

    if (backUntilTime<sim.getSimulationTime()) then
        -- When in forward mode, we simply move forward at the desired speed
        sim.setJointTargetVelocity(leftMotor,leftV)
        sim.setJointTargetVelocity(rightMotor,rightV)
    else
        -- When in backward mode, we simply backup in a curve at reduced speed
        sim.setJointTargetVelocity(leftMotor,-speed/2)
        sim.setJointTargetVelocity(rightMotor,-speed/8)
    end
end

function sysCall_cleanup()
	simUI.destroy(ui)
end

此外，在调试过程中可将视觉传感器的视图与floating view绑定，便于观察