Closed mdecourse closed 6 years ago
使用方法: 開啟空白場景檔案, 納入 robot-mobile-e-puck.ttm 後, 將 threaded 程式換成下列版本
-- This is the Epuck principal control script. It is threaded
-- FUNCTION MOVE
-- Moving the robot
-- distance : distance (meters)
-- velLeft : angular speed left wheel
-- velRight : angular speed rigth wheel
move = function(distance, velLeft, velRight)
simSetJointTargetVelocity(leftMotor, velLeft)
simSetJointTargetVelocity(rightMotor, velRight)
local i = 0
local distanceParcourue = 0
local p = simGetObjectPosition(ePuckBase, -1)
local p1 = simGetObjectPosition(ePuckBase, -1)
-- time-out : variable i
while (i < 1000 and distanceParcourue < distance) do
p1 = simGetObjectPosition(ePuckBase, -1)
distanceParcourue = distanceMes(p[1], p[2], p1[1], p1[2])
--simAddStatusbarMessage('parcourue :' .. distanceParcourue .. ' - Attendue :' .. distance)
i = i + 1
simSwitchThread()
end
simSetJointTargetVelocity(leftMotor,0)
simSetJointTargetVelocity(rightMotor,0)
end
-- FUNCTION TURNLEFT
-- Make a left turn (90°)
-- vel : angular speed of wheels
turnLeft=function(vel)
local rotAmount=-math.pi/2
local sign=rotAmount/math.abs(rotAmount)
simSetJointTargetVelocity(leftMotor,vel*sign*0.5)
simSetJointTargetVelocity(rightMotor,-vel*sign*0.5)
local previousAngle=simGetObjectOrientation(ePuckBase,-1)[3]
local rot=0
local i = 0
while (i < 1000) do --and (math.abs(rot) < math.pi/2)
local angle=simGetObjectOrientation(ePuckBase,-1)[3]
local da=angle-previousAngle
if da>=0 then
da=math.mod(da+math.pi,2*math.pi)-math.pi
else
da=math.mod(da-math.pi,2*math.pi)+math.pi
end
rot=rot+da
previousAngle=angle
if math.abs(rot)>math.pi/2 then
break
end
simSwitchThread()
end
simSetJointTargetVelocity(leftMotor,0)
simSetJointTargetVelocity(rightMotor,0)
end
-- FUNCTION TURNRIGHT
-- Make a right turn (90°)
-- vel : angular speed of wheels
turnRight=function(vel)
local rotAmount=math.pi/2
local sign=rotAmount/math.abs(rotAmount)
simSetJointTargetVelocity(leftMotor,vel*sign*0.5)
simSetJointTargetVelocity(rightMotor,-vel*sign*0.5)
local previousAngle=simGetObjectOrientation(ePuckBase,-1)[3]
local rot=0
local i = 0
while (i < 1000) do --and (math.abs(rot) < math.pi/2)
local angle=simGetObjectOrientation(ePuckBase,-1)[3]
local da=angle-previousAngle
if da>=0 then
da=math.mod(da+math.pi,2*math.pi)-math.pi
else
da=math.mod(da-math.pi,2*math.pi)+math.pi
end
rot=rot+da
previousAngle=angle
if math.abs(rot)>math.pi/2 then
break
end
simSwitchThread()
end
simSetJointTargetVelocity(leftMotor,0)
simSetJointTargetVelocity(rightMotor,0)
end
-- FONCTION UTURN
-- Make a half turn (180°)
-- vel : angular speed of wheels
uTurn=function(vel)
local rotAmount=math.pi/2
local sign=rotAmount/math.abs(rotAmount)
simSetJointTargetVelocity(leftMotor,vel*sign*0.5)
simSetJointTargetVelocity(rightMotor,-vel*sign*0.5)
local previousAngle=simGetObjectOrientation(ePuckBase,-1)[3]
local rot=0
local i = 0
while (i < 1000) do --and (math.abs(rot) < math.pi/2)
local angle=simGetObjectOrientation(ePuckBase,-1)[3]
local da=angle-previousAngle
if da>=0 then
da=math.mod(da+math.pi,2*math.pi)-math.pi
else
da=math.mod(da-math.pi,2*math.pi)+math.pi
end
rot=rot+da
previousAngle=angle
if math.abs(rot)>math.pi then
break
end
simSwitchThread()
end
simSetJointTargetVelocity(leftMotor,0)
simSetJointTargetVelocity(rightMotor,0)
end
-- FUNCTION DISTANCE
-- Measure the absolute distance between point A(xa, yx) and B(xb, yb)
-- xa : coordinates x of first point (A)
-- ya : coordinates y of first point (A)
-- xb : coordinates x of second point (B)
-- yb : coordinates y of second point (B)
-- return : absolute distance between points
distanceMes = function(xa, ya, xb, yb)
return math.sqrt( math.pow((xb - xa), 2) + math.pow((yb - ya), 2) )
end
-- FUNCTION SENSORS
-- Save actual state (true/false) of sensors
sensors = function()
local boolSensors = 0x0
local sc=simGetObjectSizeFactor(bodyElements)
local r = 0
local d = 0
local i=0
for i=1,8,1 do
--r,d = simCheckProximitySensor(proxSens[i], sim_handle_all)
r,d = simReadProximitySensor(proxSens[i])
mask = math.pow(2, (i-1))
notMask = simBoolXor32(mask, 0xFFFFFFFF)
-- if res > 0 and dist < 0.05*s then
if (r > 0) then
boolSensors = simBoolOr32(boolSensors, mask)
--simAddStatusbarMessage('sens[' .. i .. '] - ' .. r .. ' - ' .. d)
else
boolSensors = simBoolAnd32(boolSensors, notMask)
--simAddStatusbarMessage('sens[' .. i .. ']')
end
end
return boolSensors
end
-- NOT USED
moveDistance = function(distance, vel, vel2)
local i = 0
local distanceParcourue = 0
local p = simGetObjectPosition(ePuckBase, -1)
local p1 = simGetObjectPosition(ePuckBase, -1)
bodyElements=simGetObjectHandle('ePuck_bodyElements')
s=simGetObjectSizeFactor(bodyElements)
simSetJointTargetVelocity(leftMotor,vel)
simSetJointTargetVelocity(rightMotor,vel)
velLeft=vel
velRight=vel
maxVel = vel
while (i < 1000 and distanceParcourue < distance) do
noDetectionDistance=0.05*s --0.05
proxSensDist={noDetectionDistance,noDetectionDistance,noDetectionDistance,noDetectionDistance,noDetectionDistance,noDetectionDistance,noDetectionDistance,noDetectionDistance}
for i=1,8,1 do
res,dist=simReadProximitySensor(proxSens[i])
if (res>0) and (dist<noDetectionDistance) then
proxSensDist[i]=dist
end
end
if (proxSensDist[1] < 0.2*noDetectionDistance) then
velLeft=velLeft + maxVel * 0
velRight=velRight + maxVel * -0.05 * (1-(proxSensDist[2]/noDetectionDistance))
--velRight=velRight + maxVel * -0.05
elseif (proxSensDist[6] < 0.2*noDetectionDistance) then
velLeft=velLeft + maxVel * -0.05 * (1-(proxSensDist[4]/noDetectionDistance))
--velLeft=velLeft + maxVel * -0.05
velRight=velRight + maxVel * 0
else
velLeft=vel
velRight=vel
end
p1 = simGetObjectPosition(ePuckBase, -1)
distanceParcourue = distanceMes(p[1], p[2], p1[1], p1[2])
i = i + 1
simSetJointTargetVelocity(leftMotor,velLeft)
simSetJointTargetVelocity(rightMotor,velRight)
simSwitchThread()
end --while
simSetJointTargetVelocity(leftMotor,0)
simSetJointTargetVelocity(rightMotor,0)
end --function
actualizeLEDs=function()
if (relLedPositions==nil) then
relLedPositions={{-0.0343,0,0.0394},{-0.0297,0.0171,0.0394},{0,0.0343,0.0394},
{0.0297,0.0171,0.0394},{0.0343,0,0.0394},{0.0243,-0.0243,0.0394},
{0.006,-0.0338,0.0394},{-0.006,-0.0338,0.0394},{-0.0243, -0.0243,0.0394}}
end
if (drawingObject) then
simRemoveDrawingObject(drawingObject)
end
type=sim_drawing_painttag+sim_drawing_followparentvisibility+sim_drawing_spherepoints+
sim_drawing_50percenttransparency+sim_drawing_itemcolors+sim_drawing_itemsizes+
sim_drawing_backfaceculling+sim_drawing_emissioncolor
drawingObject=simAddDrawingObject(type,0,0,bodyElements,27)
m=simGetObjectMatrix(ePuckBase,-1)
itemData={0,0,0,0,0,0,0}
simSetLightParameters(ledLight,0)
for i=1,9,1 do
if (ledColors[i][1]+ledColors[i][2]+ledColors[i][3]~=0) then
p=simMultiplyVector(m,relLedPositions[i])
itemData[1]=p[1]
itemData[2]=p[2]
itemData[3]=p[3]
itemData[4]=ledColors[i][1]
itemData[5]=ledColors[i][2]
itemData[6]=ledColors[i][3]
simSetLightParameters(ledLight,1,{ledColors[i][1],ledColors[i][2],ledColors[i][3]})
for j=1,3,1 do
itemData[7]=j*0.003
simAddDrawingObjectItem(drawingObject,itemData)
end
end
end
end
getLightSensors=function()
data=simReceiveData(0,'EPUCK_lightSens')
if (data) then
lightSens=simUnpackFloatTable(data)
end
return lightSens
end
threadFunction=function()
while simGetSimulationState()~=sim_simulation_advancing_abouttostop do
st=simGetSimulationTime()
velLeft=0
velRight=0
move(1, 10, 10)
turnLeft(1)
move(1, 10, 10)
turnLeft(1)
move(1, 10, 10)
actualizeLEDs()
simSwitchThread() -- Don't waste too much time in here (simulation time will anyway only change in next thread switch)
end
end
-- Put some initialization code here:
simSetThreadSwitchTiming(200) -- We will manually switch in the main loop
bodyElements=simGetObjectHandle('ePuck_bodyElements')
leftMotor=simGetObjectHandle('ePuck_leftJoint')
rightMotor=simGetObjectHandle('ePuck_rightJoint')
ePuck=simGetObjectHandle('ePuck')
ePuckBase=simGetObjectHandle('ePuck_base')
ledLight=simGetObjectHandle('ePuck_ledLight')
proxSens={-1,-1,-1,-1,-1,-1,-1,-1}
for i=1,8,1 do
proxSens[i]=simGetObjectHandle('ePuck_proxSensor'..i)
end
maxVel=120*math.pi/180
ledColors={{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}}
-- Braitenberg weights for the 4 front prox sensors (avoidance):
braitFrontSens_leftMotor={1,2,-2,-1}
-- Braitenberg weights for the 2 side prox sensors (following):
braitSideSens_leftMotor={-1,0}
-- Braitenberg weights for the 8 sensors (following):
braitAllSensFollow_leftMotor={-3,-1.5,-0.5,0.8,1,0,0,-4}
braitAllSensFollow_rightMotor={0,1,0.8,-0.5,-1.5,-3,-4,0}
braitAllSensAvoid_leftMotor={0,0.5,1,-1,-0.5,-0.5,0,0}
braitAllSensAvoid_rightMotor={-0.5,-0.5,-1,1,0.5,0,0,0}
-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
simAddStatusbarMessage('Lua runtime error: '..err)
end
-- Put some clean-up code here:
for i=1,9,1 do
ledColors[i]={0,0,0} -- no light
end
actualizeLEDs()
以程式控制行走車
行走車: https://github.com/mdecourse/vecp2018/blob/gh-pages/final/vecp_w10_epuck_maze.ttt 請下載後, 以 V-rep 開啟後, 修改控制程式, 讓行走車可順利抵達目標點.