# This file is used by your post processor (Mitsubishi)
# to generate the appropriate file for your robot.
# You will find your robot program in the same folder where this file is located.
# To avoid showing this file after program generation you can select:
#   Tools->Options->tab Other , then, uncheck "Show programs in Notepad"

import sys
import os
sys.path.append(os.path.abspath(r"""/home/thomas/RoboDK/Posts/""")) # temporarily add path to POSTS folder

from Mitsubishi import *

def p(x,y,z,r,p,w):
  a = r*math.pi/180.0
  b = p*math.pi/180.0
  c = w*math.pi/180.0
  ca = math.cos(a)
  sa = math.sin(a)
  cb = math.cos(b)
  sb = math.sin(b)
  cc = math.cos(c)
  sc = math.sin(c)
  return Mat([[cb*ca,ca*sc*sb-cc*sa,sc*sa+cc*ca*sb,x],[cb*sa,cc*ca+sc*sb*sa,cc*sb*sa-ca*sc,y],[-sb,cb*sc,cc*cb,z],[0.0,0.0,0.0,1.0]])

def make_program():
  print('Total instructions: 5')
  r = RobotPost(r"""Mitsubishi""",r"""Niryo One""",6, axes_type=['R','R','R','R','R','R'], ip_com=r"""127.0.0.1""", api_port=20500, prog_ptr=94462757124880, robot_ptr=94462779089328)

  r.ProgStart(r"""Punkt""")
  r.RunMessage(r"""Program generated by RoboDK v5.2.0 for Niryo One on 20/01/2021 11:50:35""", True)
  r.RunMessage(r"""Using nominal kinematics.""", True)
  r.setZoneData(1.000)
  r.setSpeed(1000.000)
  r.setFrame(p(133.564,-59.6792,0,0,0,0),2,r"""Frame 2""")
  r.setTool(p(5,0,20,0,0,0),-1,r"""finger""")
  r.RunMessage(r"""Anzeigen finger""", True)
  r.MoveJ(None,[-37.1221,96.5168,-41.0742,58.0626,110.019,28.7748],None)
  r.MoveL(p(152,-8.4641,9.52394e-07,-90,-90,0),[-22.1323,87.0085,-15.5974,68.9185,96.8977,17.3039],[0,0,0])
  r.setSpeed(50.000)
  r.MoveL(p(152,11.5359,0,-90,-90,0),[-18.6073,86.1301,-12.7845,72.1226,95.2469,15.8286],[0,0,0])
  r.MoveL(p(152,-8.4641,9.52394e-07,-90,-90,0),[-22.1323,87.0085,-15.5974,68.9185,96.8977,17.3039],[0,0,0])
  r.setSpeed(1000.000)
  r.MoveL(p(152,-108.464,5.71436e-06,-90,-90,0),[-37.1221,96.5168,-41.0742,58.0626,110.019,28.7748],[0,0,0])
  r.ProgFinish(r"""Punkt""")
  r.ProgSave(r"""/home/thomas/Dokumente/RoboDK/""",r"""Punkt""",False,[r"""gedit"""])

if __name__ == "__main__":
  make_program()