from vcScript import *

from readProgramsFromCsv import *

import vcMatrix

def recordJoints(robotController, routine, point):

motionTarget = robotController.createTarget()
motionTarget.MotionType = VC_MOTIONTARGET_MT_JOINT
motionTarget.UseJoints = True
jv = motionTarget.JointValues
motionTarget.BaseName = robotController.Bases[point.base].Name
motionTarget.ToolName = robotController.Tools[point.tool].Name
for i in xrange(min(len(point.allJointValues), len(jv))):
jv[i] = point.allJointValues[i]
motionTarget.JointValues = jv
robotController.moveImmediate(motionTarget)
if point.interpolationType == 'PTP':
statement = routine.addStatement(VC_STATEMENT_PTPMOTION)
statement.JointSpeed = point.speed
else:
statement = routine.addStatement(VC_STATEMENT_LINMOTION)
statement.MaxSpeed = point.speed
statement.readIn()
position = statement.Positions[0]
position.Name = point.name

def recordPosition(robotController, routine, point):

x = point.coord[0]
y = point.coord[1]
z = point.coord[2]
rx = point.coord[5]
ry = point.coord[4]
rz = point.coord[3]
mtx = vcMatrix.new()
mtx.translateRel(x, y, z)
mtx.rotateRelZ(rz)
mtx.rotateRelY(ry)
mtx.rotateRelX(rx)

if point.interpolationType == 'PTP':
statement = routine.addStatement(VC_STATEMENT_PTPMOTION)
statement.JointSpeed = point.speed
else:
statement = routine.addStatement(VC_STATEMENT_LINMOTION)
statement.MaxSpeed = point.speed
statement.Base = robotController.Bases[point.base]
statement.Tool = robotController.Tools[point.tool]
position = statement.Positions[0]
position.PositionInReference = mtx
position.Name = point.name
ejv = position.ExternalJointValues
for i in xrange(min(len(point.externalJointValues), len(ejv))):
ejv[i] = point.externalJointValues[i]
position.setExternalJoints(ejv)
position.Configuration = calConfiguration(point.status)
motionTarget = robotController.createTarget()
statement.writeToTarget(motionTarget)
robotController.moveImmediate(motionTarget)
 statement.readIn()
position = statement.Positions[0]
turnPosition(position, point.turn)

def addSyncStatement(comps, routine, point):

statement = routine.addStatement(VC_STATEMENT_PROG_SYNC)
statement.SyncComponents = comps
statement.SyncMessage = point.name
statement.WaitSync = True

def getRobotComponents():
app = getApplication()
robotComponents = []
for component in app.Components:
cat = component.getProperty('Category')
if 'Robots' == cat.Value:
robotComponents.append(component)
robotComponents.sort(key=lambda x: x.getProperty('Name').Value)
return robotComponents

def setRobotStop(stopButtons, robotComponents):

for i in range(len(robotComponents)):
name = robotComponents[i].getProperty('Name')
print(name.Value),
robotExecutor = robotComponents[i].findBehavioursByType("rRobotExecutor")
[0]
robotExecutor.IsEnabled = not stopButtons[i].Value
print('Move' if robotExecutor.IsEnabled else 'Stop')

def toCreateOnChanged(toCreate):
stopButtons = getStopButtons()
if toCreate.Value == True:
for button in stopButtons:
button.Value = True

def createSubroutines(robotComponent, robotId, components):

robotExecutor = robotComponent.findBehavioursByType("rRobotExecutor")[0]
robotController = robotExecutor.Controller
robotProgram = robotExecutor.Program
# 从文件中读取所有轨迹程序，传入机器人唯一 ID 作为参数
subprograms = getAllPrograms(robotId)
prgNames = [key for key in subprograms.keys()]
prgNames.sort(key=lambda x: (int(x[2:]), x[:2]))
for prgName in prgNames:
# 根据文件名创建子程序
subroutine = robotProgram.findRoutine(prgName)
if subroutine:
subroutine.clear()
if not subroutine:
subroutine = robotProgram.addRoutine(prgName)
# 将所有轨迹指令加入到子程序
for point in subprograms[prgName]:
if point.pType == 'E6AXIS':
recordJoints(robotController, subroutine, point)
 elif point.pType == 'E6POS':
recordPosition(robotController, subroutine, point)
else:
addSyncStatement(components, subroutine, point)

def deleteSubroutines(robotComponent):
robotExecutor = robotComponent.findBehavioursByType("rRobotExecutor")[0]
robotProgram = robotExecutor.Program
subroutines = robotProgram.Routines
for subroutine in subroutines:
robotProgram.deleteRoutine(subroutine)

def writeMainRoutine(robotComponent):
robotExecutor = robotComponent.findBehavioursByType("rRobotExecutor")[0]
robotProgram = robotExecutor.Program
mainRoutine = robotProgram.MainRoutine
mainRoutine.clear()

statement = mainRoutine.addStatement(VC_STATEMENT_SETBIN)
statement.OutputPort = 21
statement.OutputValue = True

for subroutine in robotProgram.Routines:

statement = mainRoutine.addStatement(VC_STATEMENT_CALL)
statement.Routine = subroutine

def getStopButtons():
comp = getComponent()
robots = getRobotComponents()
stopButtons = []
for i in range(len(robots)):
stopButtonName = 'Robot' + str(i + 1) + 'Stop'
stopButton = comp.getProperty(stopButtonName)
if not stopButton:
stopButton = comp.createProperty(VC_BOOLEAN, stopButtonName)
stopButtons.append(stopButton)
return stopButtons

def initStopButtons():
for pr in comp.Properties:
if pr.Type == 'Boolean':
if pr.Name.startswith('Robot') and pr.Name.endswith('Stop'):
num = int(pr.Name[5])
if num > len(robots):
comp.deleteProperty(pr)

def createAllRobotSubroutines(robots):
for i in range(len(robots)):
if i+1 < len(robots):
otherRobots = robots[:i] + robots[i + 1:]
else:
otherRobots = robots[:i]
deleteSubroutines(robots[i])
createSubroutines(robots[i], i + 1, otherRobots)
 writeMainRoutine(robots[i])

def OnRun():
if toCreate.Value == True:
createAllRobotSubroutines(robots)
app = getApplication()
sim = app.getSimulation()
sim.reset()

def OnReset():
setRobotStop(stopButtons, robots)

comp = getComponent()
robots = getRobotComponents()
stopButtons = getStopButtons()
toCreate = comp.getProperty('ToCreate')
toCreate.OnChanged = toCreateOnChanged
initStopButtons()