Program Examples:Cartesian Gearing/zh-hans

语言:	English  • 中文（简体）‎

这是使用笛卡尔齿轮传动的一个例子。 笛卡尔齿轮传动是主从功能，其中单轴运动适用于末端执行器相对平移。

'------------------------------------------------------------------------------
' File:        Puma_CG.prg
' Purpose:     cartesian gearing example
' Version:     1.00
' Author:      Eran Korkidi
' History:     25.JUL.2016
'------------------------------------------------------------------------------

import_c CG_VER		(byval as long, byval as long, byval as long) as long
import_c CG_STATUS	(byval as long, byval as long, byval as long) as long
import_c CG_SET		(byval as long, byval as long, byval as long) as long
import_c CG_UNSET	(byval as long, byval as long, byval as long) as long
import_c CG_MOD		(byval as long, byval as long, byval as long) as long
import_c CG_STOP	(byval as long, byval as long, byval as long) as long
import_c CG_START	(byval as long, byval as long, byval as long) as long
import_c CG_MASK	(byval as long, byval as long, byval as long) as long


dim shared lRobotID		as long
dim shared laxisID		as long
dim shared lResolution	as long
dim shared lGearMode	as long = 1 ' 1 = commmand, 0 = fbk
dim shared lDummy		as long = 0

dim shared jntStartPosition as joint of xyzypr = {0,45,0,0,45,0}

program

	call cartesianGearSetup
	call motionSetup
	call executMotion

end program ' <MyTask>.prg


sub executMotion

	dim dPrevA13position as double
	dim dPath as double

	Attach Puma
	Attach A13
		Puma.En = ON
		A13.En = ON
		Move PUMA jntStartPosition Vcruise=PUMA.Vmax
		call waitMotionGr
		'Move A13 0 Vcruise=A13.Vmax Abs=1
		call waitMotionAx
		Sleep 1000
		A13.Slave = ON
		Sleep 1000
		dPrevA13position = A13.Pcmd
		Moves PUMA #{100,0,0,0,0,0} Vtran=100 Abs=OFF Upar=1
		call waitMotionGr
		Moves PUMA #{-100,0,0,0,0,0} Vtran=100 Abs=OFF Upar=1
		call waitMotionGr
		Moves PUMA #{100,0,0,0,0,0} Vtran=100 Abs=OFF Upar=1
		call waitMotionGr
		Circle PUMA Angle=360 CircleCenter=jntStartPosition Vtran=100 Upar=1
		call waitMotionGr
		Sleep 1000
		A13.Slave = OFF
		dPath = A13.Pcmd - dPrevA13position
		? "Path = " dPath  ", should be " A13.GearRatio*(100*3 + 2*100*PI)
	Detach A13
	Detach Puma

end sub


sub cartesianGearSetup

	lRobotID = PUMA.ElementId
	laxisID = A13.ElementId
	lResolution = 1E5


'	? CG_STOP(lDummy, lDummy, lDummy)
	? CG_VER(lDummy, lDummy, lDummy)
	? CG_STATUS(lDummy, lDummy, lDummy)
	? CG_MASK(0b111, 0, 0) ' xyz
	? CG_UNSET(lRobotID, laxisID, 0)
	? CG_SET(lRobotID, laxisID, lResolution)
'	? CG_RESET(lRobotID, 0, 0)
	? CG_MOD(lGearMode, 0, 0)

end sub


sub motionSetup

	dim i as long 
	dim genAxis as generic axis

	for i = 1 to Sys.Naxes
		genAxis = systemAxis(i)
		genAxis.En = OFF
		Attach genAxis
		genAxis.Pedel = 2.0
		genAxis.PeMax = 10.0
		Detach genAxis
	next
	Attach A13
		A13.PextFac = lResolution
		A13.MasterSource = A13.Pext
		A13.GearRatio = 2
		A13.Slave = 0
		A13.Vmax = PUMA.Vmax
		A13.Amax = A13.Vmax * 10.0
		A13.Dmax = A13.Vmax * 10.0
		A13.Jmax = A13.Amax * 10.0
		A13.Vcruise = A13.Vmax
		A13.Acc = A13.Amax
		A13.Dec = A13.Dmax
		A13.Jerk = A13.Jmax
		A13.Vospd = A13.Vmax * 1.2
		A13.Pfac = 1E6 / 360
		A13.Vfac = A13.Pfac / 1E3
		A13.Afac = A13.Pfac / 1E6
		A13.Jfac = A13.Pfac / 1E9
	Detach A13
	
end sub

sub waitMotionGr
	while PUMA.IsMoving
		Sleep  1
	end while
end sub


sub waitMotionAx
	while A13.IsMoving
		Sleep 1
	end while
end sub