40823144 cd2021

  • Home
    • Site Map
    • reveal
    • blog
  • About
    • csave
  • CMSiMDE
  • Stage1-ag17
    • 組員
    • W3
  • Stage2-ag10
    • 組員：
    • W5
    • W6
    • W7
    • W8
    • 成品：
  • W13~W14
    • RoboDK
    • Task
      • task1
      • task2
  • W15
  • W16
  • Stage3-ag4
    • Stage3組員
    • 更改1
    • 模擬問題
  • 英文學習中
  • CoppeliaSim
  • ssh
  • heroku
  • DC推廣中心
W15 << Previous Next >> Stage3-ag4

W16

MTB_robot 的取放方塊流程規劃

1. Onshape 零組件繪製 (20%)

Onshape

2. 建立 CoppeliaSim 4.1.0 MTB robot 場景 (20%)

雲端_Google drive

3. 手臂末端加入 components-gripper-suction pad 吸盤 (20%)

4. 逆向運動學函式 (20%)

程式迴圈控制

程式碼

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.03)
    sim.wait(2)
    sim.setJointTargetPosition(jointz,0)
     while sim.getSimulationState()~=sim.simulation_advancing_abouttostopre do
        moving(0.03,0.7)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",0)
        sim.wait(2)
        sim.setIntegerSignal("pad_switch",1)
        sim.setJointTargetPosition(jointz,-0.03)
        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.03)
        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.03)
        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.03)
        sim.wait(2)
        sim.setJointTargetPosition(jointz,0)
        sim.wait(2)
    end
end

鍵盤控制

程式碼

function sysCall_init()
    joint1=sim.getObjectHandle('joint1')
    joint2=sim.getObjectHandle('joint2')
    joint3=sim.getObjectHandle('joint3')
    jointz=sim.getObjectHandle('jointz')
    sim.setJointTargetPosition(joint1,0)
    sim.setJointTargetPosition(joint2,0)
    sim.setJointTargetPosition(joint3,0)
    sim.setJointTargetPosition(jointz,0)
    deg1=0
    deg2=0
    deg3=0
end

function sysCall_actuation()
    message,auxiliaryData=sim.getSimulatorMessage()
    while message~=-1 do
        if (message==sim.message_keypress) then
            if (auxiliaryData[1]==97) then
            deg1=deg1+1
            deg3=deg2-deg1
            sim.setJointTargetPosition(joint1,deg1*math.pi/180)
            end
            if (auxiliaryData[1]==100) then
            deg1=deg1-1
            deg3=deg2-deg1
            sim.setJointTargetPosition(joint1,deg1*math.pi/180)
            end
            if (auxiliaryData[1]==119) then
            deg2=deg2+1
            deg3=deg1-deg2
            sim.setJointTargetPosition(joint2,deg2*math.pi/180)
            end
            if (auxiliaryData[1]==115) then
            deg2=deg2-1
            deg3=deg1-deg2
            sim.setJointTargetPosition(joint2,deg2*math.pi/180)
            end
            if (auxiliaryData[1]==2008) then
                sim.setJointTargetPosition(jointz,-0.03)
                sim.setIntegerSignal("pad_switch",1)
            end
            if(auxiliaryData[1]==2007) then
                sim.setJointTargetPosition(jointz,0)
            end
            if(auxiliaryData[1]==32) then
                sim.setIntegerSignal("pad_switch",0)
            end
            sim.setJointTargetPosition(joint3,deg3*math.pi/180)
        end
    message,auxiliaryData=sim.getSimulatorMessage()
    end
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details

5. Python remote API 逆向運動學函式 (20%)

程式碼

import sim as vrep
import math
import random
import time
import math

def 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 :
        deg1_base=deg1_base+math.pi
    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
    vrep.simxSetJointTargetPosition(clientID,joint01, deg1,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint02, -deg2,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint03, deg3,opmode)


print('start')

vrep.simxFinish(-1)
 
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
 
if clientID!= -1:
    print("Connected to remote API server")
    
    res=vrep.simxAddStatusbarMessage(
        clientID,"40823144",
        vrep.simx_opmode_oneshot)
    if res not in (vrep.simx_return_ok,vrep.simx_return_novalue_flag):
        print("could not add a message to the status bar.")
        
        
    opmode=vrep.simx_opmode_oneshot_wait
    STREAMING=vrep.simx_opmode_streaming
    
    vrep.simxStartSimulation(clientID,opmode)
    ret,joint01=vrep.simxGetObjectHandle(clientID,"joint1",opmode)
    ret,joint02=vrep.simxGetObjectHandle(clientID,"joint2",opmode)
    ret,joint03=vrep.simxGetObjectHandle(clientID,"joint3",opmode)
    ret,jointz=vrep.simxGetObjectHandle(clientID,"jointz",opmode)
    
    vrep.simxSetJointTargetPosition(clientID,joint01,0,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint02,0,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint03,0,opmode)
    vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
    vrep.simxSetJointTargetPosition(clientID,jointz,-0.03,opmode)
    time.sleep(1)
    vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)
    while True:
        moving(0.2,0.7)
        time.sleep(1)
        vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
        time.sleep(1)
        vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
        vrep.simxSetJointTargetPosition(clientID,jointz,-0.03,opmode)
        time.sleep(1)
        vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)
        moving(-0.3,-0.55)
        time.sleep(1)
        vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
        time.sleep(1)
        vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
        vrep.simxSetJointTargetPosition(clientID,jointz,-0.03,opmode)
        time.sleep(1)
        vrep.simxSetJointTargetPosition(clientID,jointz,0,opmode)


W15 << Previous Next >> Stage3-ag4

Copyright © All rights reserved | This template is made with by Colorlib