# -*- coding: utf-8 -*-
"""
Created on April 3 2019

@author: Max Erick Busse-Grawitz, maxon motor ag, Switzerland
"""

import time
import clr #pythonnet, manually installed with a downloaded wheel and pip
import ctypes #module to open dll files
clr.AddReference('EposCmd.Net')
EposCmd64 = ctypes.CDLL('.\EposCmd64.dll')
from EposCmd.Net.VcsWrapper import Device

# configure desired motion profile in the lines below
velocity = 83 #rpm profile speed
acceleration = 3e5 #rpm/s, up to 1e7 would be possible
deceleration = 3e5 #rpm/s
number_of_cycles = 100
angle = 40 #degrees
delay = angle/6/velocity #delay in seconds. This determines the angle to be reached, approximately

pos0 = 0 #start position
pos1 = 100000 #put in a big number, bigger than the desired end position


#---------------------------------------------------------------------------------------------------------------------------------------

#---------------------------------------------------------------------------------------------------------------------------------------
print("START")
print()

Device.Init()
nodeID = 1
baudrate = 1000000
timeout = 500
errorCode = 0
absolute = True #as opposed to relative
immediately = True #does not wait for execution of previous command
timewait = 2000 #maxtime for position reach


keyHandle, error = Device.VcsOpenDevice('EPOS4', 'MAXON SERIAL V2', 'USB', 'USB0', errorCode) #opens EPOS
Device.VcsSetProtocolStackSettings(keyHandle, baudrate, timeout, errorCode) #set baudrate
Device.VcsClearFault(keyHandle, nodeID, errorCode) #clear all faults
Device.VcsActivateProfilePositionMode(keyHandle, nodeID, errorCode) #activate profile position mode
Device.VcsSetPositionProfile(keyHandle, nodeID, velocity, acceleration, deceleration, errorCode) #set profile parameters
Device.VcsSetEnableState(keyHandle, nodeID, errorCode) #enable device
  
#go to initial position before start
Device.VcsMoveToPosition(keyHandle, nodeID, pos0, True, True, errorCode)
Device.VcsWaitForTargetReached(keyHandle, nodeID, timewait, errorCode)



start = time.time()

n = 0
while  n < number_of_cycles :

    Device.VcsMoveToPosition(keyHandle, nodeID, pos1, absolute, immediately, errorCode)
    time.sleep(delay)
#    Device.VcsWaitForTargetReached(keyHandle, nodeID, timewait, errorCode)
    Device.VcsMoveToPosition(keyHandle, nodeID, pos0, absolute, immediately, errorCode)
    time.sleep(delay)
#    Device.VcsWaitForTargetReached(keyHandle, nodeID, timewait, errorCode)
    n+=1


Device.VcsMoveToPosition(keyHandle, nodeID, 0, True, True, errorCode)
Device.VcsWaitForTargetReached(keyHandle, nodeID, timewait, errorCode)

Device.VcsSetDisableState(keyHandle, nodeID, errorCode)
Device.VcsCloseDevice(keyHandle, errorCode)

end = time.time()
print('\nElapsed time: ', int((end-start)/60), 'min', int((end-start)%60), 's')

print()
print("END")