Cartesian Gearing

From SoftMC-Wiki
Revision as of 10:13, 20 February 2018 by Itay (talk | contribs)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

TOP2.png
Language: English

Introduction

The Cartesian Gearing (CG) feature enables users to move additional axis proportionally to traveled path of the robot’s tool-tip.
Slaving an axis to it's Pext that is geared to the robot's incremental cartesian change - the path variable (L) in cartesian space is computed each motion sampling period.
The variable keeps track of the traveled path in cartesian space (X,Y,Z). The value is accumulative, which means that if a same line segment is traveled back and forth the L will be increased by the double of the segment length.

Additionally, the ratio between the traveled path and the length-of-the path variable L can be changed for each movement separately. This means that user can assign a nodal ratio parameter for each movement differently.

The L variable is available as external position (Ax.Pext) of an axis. The axis is an arbitrary axis that can be of the same robot or any external axis in the system.


Cartesian Gearing

User Functions

The following function are available for Cartesian Gearing Setup:

Function Description Notes
CG_VER(0,0,0) Returns the current version number
CG_STATUS(0,0,0) Prints on telnet connection internal CG status variables
CG_SET(int rid, int aid, int pf) Setting up CG relationship, if everything is OK a zero value is returned.
rid – robot ID Available as ElementID group property
aid – axis ID Available as ElementID axis property
pf - integer value Returns the number of counts per one millimeter traveled path In order to get Pext in millimeters the same value should be assigned to pExtFac
CG_UNSET(int rbt_id, int axis_handle,0) Removing CG relationship, undoes the CG_SET'
CG_SET(int rbt_id, int value,0) Set the L to a new value (offset). The value given here is in Pext counts
CG_STOP (0,0,0) Stopping the CG process at the current value of L. Stopping is performed immediately, without deceleration profile
CG_START(0,0,0) Re-starting the CG process that was stopped by CG_STOP command
CG_MOD(mod,0,0) Setting the CG mode (0 – command, default, 1- feedback) if the L variable is computed from the command (setpoint) or the feedback Cartesian coordinates (here).
CG_MASK(mask,0,0) Defining the operational space in which L is computed. For example: if mask = 1 then only changes in X coordinate will be included in computation of L.
Possible values will be:
Mask  Binary Value   Operational Space 
0 0 None
1 1 X
2 10 Y
3 11 XY
4 100 Z
5 101 ZX
6 110 ZY
7 111 ZYX

Multi Tool Cartesian Gearing

In case the robot has more than a single tool, then user can assign axis per tool.

Function Description Notes
CG_ASSIGN_TOOL(int axisHandle, SYS_POINT * pntTool) Tool number and offset. These offset are later used for calculating cartGear
CG_CLEAR_ALL_TOOLS(void) Set null value (zeros) in all tools


Program Example

Language: English

Here is an example of using cartesian gearing for Puma robot



'------------------------------------------------------------------------------
' 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