import time

from pytrinamic.connections import ConnectionManager
from pytrinamic.modules import TMCM1161
import calibot
o1 = 0.005 #offset 1
o2 = 0.0035 #offset2
waittime = 5

connection_manager1 = ConnectionManager("--interface serial_tmcl --port COM9 --data-rate 115200")
connection_manager2 = ConnectionManager("--interface serial_tmcl --port COM12 --data-rate 115200")

with connection_manager1.connect() as my_interface1:
    with connection_manager2.connect() as my_interface2:
        module1 = TMCM1161(my_interface1)
        module2 = TMCM1161(my_interface2)
     #   Calibot.goposition(module1, module2, 0.25+0.005, 0.25+0.0035) #lets find offset
      #  Calibot.gohome(module1, module2)
      #  time.sleep(1)
        Calibot.goposition(module1, module2, 0+o1, 0.25+o2) #pos 1 (load1)
      #  time.sleep(waittime)
      #  Calibot.goposition(module1, module2, 0.25+o1, 0.25+o2) # pos 2
      #  time.sleep(waittime)
      #  Calibot.goposition(module1, module2, 0.25+o1, -0.25+o2) # pos 3
      #  time.sleep(waittime)
      #  Calibot.goposition(module1, module2, 0+o1, 0+o2) # pos 4
      #  time.sleep(waittime)
      #  Calibot.goposition(module1, module2, 0.5+o1, 0.25+o2) # pos 5
      #  time.sleep(waittime)
      #  Calibot.goposition(module1, module2, 0.25+o1, 0.5+o2) # pos 6 (load2)
    #    time.sleep(1)