Program Examples:Group Blending

From SoftMC-Wiki
Jump to: navigation, search
Language: English  • 中文（简体）‎

here is an example of using blended motion. we have a simulated XY table that moves in a rectangle contour. the motion is done with different blendingFactor: 0 / 1 / 2

config.prg: 

'------------------------------------------------------------------------------
' File:        CONFIG.PRG
' Purpose:     simulated XY configuration file
' Version:     1.00
' Author:      Eran Korkidi
' Description:
'------------------------------------------------------------------------------


Sys.NumberAxes = 14
common shared XYtable			as group Axnm=a1 Axnm=a2 Model=1 'of xy
common shared xSimulatedSystem	as long = 1
common shared xRealSystem		as long = 0
common shared xActualSystem		as long = 0	'	1 = simulated  ,  0 = real
common shared dVelocityLimit	as double
common shared dDrivePeMax		as double
common shared dSec2msec			as double = 1000
 
common shared num_Of_Motion_Drives 	as long =0

program

	Load ETHERCAT.LIB
	Load EC_USER.LIB
	Load EC_CDHD.LIB
	Load ECX_DIO8.LIB
	Load EC_AI8ME.LIB
	Load CPX_FB38.LIB
	Load EC_HCNC.LIB
	Load EK1100.LIB
	Load TP_PARAM.LIB
	Load TP_SIM.LIB
	Load TP.LIB

	A7.AxisName = CONV

	sys.DoubleFormat = 1
	sys.NoMotion = 1
	Sys.Name = "Train_2_XY"
	Print "MC is running: " Sys.Name

end program



autoexec.prg: 

'------------------------------------------------------------------------------
' File:			AUTOEXEC.PRG
' Purpose:		autoexec for XYportal
' Version:		1.00
' Author:		Eran Korkidi
' Description:
' History:		Created:  30.NOV.2015
'------------------------------------------------------------------------------

common shared xDoneAxSetupPrg	as long = 0 
common shared xDoneGroupPrg		as long = 0 
common shared xDoneConvPrg		as long = 0 

program continue

	if xActualSystem = xRealSystem	then
		Load EC_SETUP.PRG
		Stas EC_SETUP.PRG
		while EC_SETUP.PRG.State <> 10
			Sleep 10
		end while
		Unload EC_SETUP.PRG
	end if

	Load XY_setup.PRG
	Stas XY_setup.PRG
	while (NOT xDoneGroupPrg)
		Sleep 1
	end while

	call TP_INIT

terminate program


XY_SETUP.PRG: 

'-----------------------------------------------------------------------
' Program:		SetupGantry.prg 
' Author:		Eran Korkidi
' Description:	system setup  - GANTRY XYZ
' History:		14.APR.2015 Eran Korkidi created
'-----------------------------------------------------------------------
'

dim shared myprf			as long = -1
dim shared gearRat_X		as double = 1.0
dim shared gearRat_Y		as double = 1.0

dim shared dAccCurrnetFactor	as double
dim shared dJointVelFactor		as double
dim shared dRobotVelFactor		as double
dim shared lElementID			as double

dim shared Jvmax as double = 360.0
dim shared AccVratio as double = 30.0

dim shared axisPitch[2] as double

program

	dim omega	as double = 10.0
	dim i as long
	dim k as long

	call setupInit

	if	xActualSystem = xSimulatedSystem then
		for i = 1 to 2
			systemAxis(i).Simulated = TRUE
			systemAxis(i).Dadd = i
			Pos_Units[i] = 360000
		next
	else
		for i = 1 to 2
			if NOT systemAxis(i).Dadd then
				systemAxis(i).Simulated = TRUE
				systemAxis(i).Dadd = i
			end if
			Pos_Units[i] = 360000/axisPitch[i]/360
		next
	end if

	with XYtable

		En = FALSE
		while En
			Sleep 10
		end while
 
		Attach
			Vord = 100
			Print "STX Gantry - geometric setup"

			J1.MotionBusVelocityScale = 0
			J1.MotionBusVelocityBase = 1000 ' counts per sec matches CDHD
			J2.MotionBusVelocityScale = 0
			J2.MotionBusVelocityBase = 1000 ' counts per sec matches CDHD

			J1.Abs = TRUE
			J2.Abs = TRUE
			J1.axisType = 1
			J2.axisType = 1
			J1.Pfac = gearRat_X * Pos_Units[J1.Dadd]
			J2.Pfac = gearRat_Y * Pos_Units[J2.Dadd]
			J1.Vfac = J1.Pfac/1000
			J2.Vfac = J2.Pfac/1000
			J1.Afac = J1.Vfac/1000
			J2.Afac = J2.Vfac/1000
			J1.Jfac = J1.Afac/1000
			J2.Jfac = J2.Afac/1000
			J1.Direction = 1
			J2.Direction = 1
			J1.Disp = 0.0
			J2.Disp = 0.0
			J1.Vmax = Jvmax 'mm/sec	'	TBD
			J2.Vmax = Jvmax 'mm/sec	'	TBD
			J1.Vospd = 1.2 * J1.Vmax
			J2.Vospd = 1.2 * J2.Vmax
			J1.Vcruise = 0.5 * J1.Vmax
			J2.Vcruise = 0.5 * J2.Vmax
			J1.Amax = J1.Vmax * AccVratio
			J2.Amax = J2.Vmax * AccVratio
			J1.Dmax = J1.Amax
			J2.Dmax = J2.Amax
			J1.Acc = J1.Amax
			J2.Acc = J2.Amax
			J1.Dec = J1.Dmax
			J2.Dec = J2.Dmax
			J1.Jmax = omega * J1.Amax
			J2.Jmax = omega * J2.Amax
			J1.Jerk = J1.Jmax
			J2.Jerk = J2.Jmax
			J1.Smooth = -1
			J2.Smooth = -1
			J1.prftype = myprf
			J2.prftype = myprf
			A1.PMin = -1.0
			A1.PMax =  400.0
			J1.PMin = -1.0
			J1.PMax =  400.0
			A2.PMin = -1.0
			A2.PMax =  400.0
			J2.PMin = -1.0
			J2.PMax =  400.0
			J1.PmaxEn = TRUE
			J2.PmaxEn = TRUE
			J1.PminEn = TRUE
			J2.PminEn = TRUE
			J1.positionerrorsettle = 0.1
			J2.positionerrorsettle = 0.1
			J1.PeMax = 20  'mm
			J2.PeMax = 20  'mm
			PeMax = Sqrt(J1.PeMax^2 + J2.PeMax^2 )  ' follwing error in mm (envelope)
			J1.PositionRollOverEnable=0
			J2.PositionRollOverEnable=0

			Smooth = -1
			prftype = myprf
			Vfac = 1/1000
			Afac = Vfac/1000
			Jfac = Afac/1000
			Vmax = Jvmax
			Amax = max(J1.Amax,J2.Amax)
			Dmax = max(J1.Dmax,J2.Dmax)
			Jmax = max(J1.Jmax,J2.Jmax)

			J1.PositionErrorDelay = 5.07
			J2.PositionErrorDelay = 5.07
			for i = 1 to 2
			  for k = 1 to 2
				if i = k then
				  cplg[i][k] = 1
				else
				  cplg[i][k] = 0
				end if
			  next
			next
			Coupled = 1
			Abs = 1
			ConfigGroup
			Vcruise = 0.5 * Vmax
			Acc = Amax
			Dec = Dmax
			Jmax = Omega * Amax
			Jerk= Jmax
			PositionErrorSettle = 0.025 ' 25um

		Detach
	end with

	Print ""
	Print "-------------------------"
	Print "XY portal - setup is done"
	Print "-------------------------"
	Print ""

end program


function max(byval a as double, byval b as double ) as double
  if a > b then
    max = a
  else
    max = b
  end if
end function


function min(byval a as double, byval b as double ) as double
  if a < b then
    min = a
  else
    min = b
  end if
end function


sub setupInit
	axisPitch[1] = 1
	axisPitch[2] = 1
end sub


XY_Blend.PRG: 

'------------------------------------------------------------------------------
' File:        <MyTask>.prg
' Purpose:     Just an example of a module header
' Version:     1.00
' Author:      <MyFullName>
' Description:
' History:     YYYY-MM-DD  <MyShortCut>  V1.00
'              Created
'------------------------------------------------------------------------------

' module global "constants"


' module global variables


program

	dim i as long 

	Attach XYTABLE
		XYTABLE.En = 1
		while NOT XYTABLE.En
			Sleep 1
		end while 
		call move1
		call move2
	Detach XYTABLE

end program ' <MyTask>.prg


sub move1

	dim i as long
	for i = 1 to 3
		Move XYTABLE {300,300}
		Move XYTABLE {0,0}
	next
	while XYTABLE.IsMoving
		Sleep 1
	end while	
	
end sub


sub move2

	XYTABLE.BlendingFactor = 80
	XYTABLE.Cp = 20

	Move XYTABLE {0,0}
	while XYTABLE.IsMoving
		Sleep 1
	end while
	
	XYTABLE.BlendingMethod = 0
	Record blend_0.rec 10000 Gap = 1 RecData = A1.Pcmd, A2.Pcmd, A1.Vcmd, A2.Vcmd
	RecordOn
		Move XYTABLE {0,100}
		Move XYTABLE {100,100}
		Move XYTABLE {100,0}
		Move XYTABLE {0,0}
		while XYTABLE.IsMoving
			Sleep 1
		end while
	RecordClose

	XYTABLE.BlendingMethod = 1
	Record blend_1.rec 10000 Gap = 1 RecData = A1.Pcmd, A2.Pcmd, A1.Vcmd, A2.Vcmd
	RecordOn
		Move XYTABLE {0,100}
		Move XYTABLE {100,100}
		Move XYTABLE {100,0}
		Move XYTABLE {0,0}
		while XYTABLE.IsMoving
			Sleep 1
		end while
	RecordClose

	XYTABLE.BlendingMethod = 2
	Record blend_2.rec 10000 Gap = 1 RecData = A1.Pcmd, A2.Pcmd, A1.Vcmd, A2.Vcmd
	RecordOn
		Move XYTABLE {0,100}
		Move XYTABLE {100,100}
		Move XYTABLE {100,0}
		Move XYTABLE {0,0}
		while XYTABLE.IsMoving
			Sleep 1
		end while
	RecordClose

end sub
Retrieved from "http://softmc.servotronix.com/index.php?title=Program_Examples:Group_Blending&oldid=129858"