w15 << Previous Next >> 字幕整理
MTB_robort

模擬情況
程式碼:
function moving(x,y)  --(副程式)計算旋轉軸需要旋轉多少
    a=0.400
    b=0.400
    c=math.pow(math.pow(x,2)+math.pow(y,2),0.5)
    s=(a+b+c)/2
    area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
    h=area/(2*c)
    deg1_base=math.atan(x/y)
    if x<0 and y<0 then
        deg1_base=deg1_base+math.pi
    end
    deg1_tri=math.asin(h/a)
    deg1=deg1_base+deg1_tri
    deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
    deg3=deg2-deg1
    sim.setJointTargetPosition(joint01,deg1)
    sim.setJointTargetPosition(joint02,-deg2)
    sim.setJointTargetPosition(joint03,deg3)
    
end

function sysCall_threadmain() --主要程式
    joint01=sim.getObjectHandle('joint1') --定義每根軸的代號
    joint02=sim.getObjectHandle('joint2') --  ""
    joint03=sim.getObjectHandle('joint3') -- ""
    jointz=sim.getObjectHandle('jointz') --  ""
    sim.setJointTargetPosition(joint01,0)
    sim.setJointTargetPosition(joint02,0)
    sim.setJointTargetPosition(joint03,0)
    sim.setJointTargetPosition(jointz,0)
    sim.setIntegerSignal("pad_switch",1)
    sim.setJointTargetPosition(jointz,-0.056)
    sim.wait(2)
    sim.wait(10000) --remove this line
    sim.setJointTargetPosition(jointz,0)
     while sim.getSimulationState()~=sim.simulation_advancing_abouttostopre do
        moving(0.2,0.7)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",0)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",1)
        sim.setJointTargetPosition(jointz,-0.056)
        sim.wait(2)
        sim.setJointTargetPosition(jointz,0)
        sim.wait(2)
        moving(-0.3,-0.55)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",0)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",1)
        sim.setJointTargetPosition(jointz,-0.056)
        sim.wait(2)
        sim.setJointTargetPosition(jointz,0)
        sim.wait(2)
        moving(0.1,0.55)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",0)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",1)
        sim.setJointTargetPosition(jointz,-0.056)
        sim.wait(2)
        sim.setJointTargetPosition(jointz,0)
        sim.wait(2)
        moving(0,0.8)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",0)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",1)
        sim.setJointTargetPosition(jointz,-0.056)
        sim.wait(2)
        sim.setJointTargetPosition(jointz,0)
        sim.wait(2)
    end
end
w15 << Previous Next >> 字幕整理