#id rev1 cycle_time_test.py 
# Fullmo MovingCap CODE - micropython example.
# original file name: cycle_time_test.py
# After five seconds, start a predefined cycle indefinitely 
# Cycle: 
# move 0 
# wait WAIT_TIME_1
# move SOFTLIMIT_MAX
# wait WAIT_TIME_2
# repeat

import sys
import mcdrive as mc

WAIT_TIME_1_MSEC = 5000
WAIT_TIME_2_MSEC = 100

def ChkReady(): # Check statusword for "target reached" and "no error" 
	while (mc.ChkReady() == 0):
		sys.wait(1)
	while (mc.ChkError() != 0):
		sys.wait(1)

# safety: only run this script if softlimit objects are set up properly
# - limit min must be 0
# - limit max must be at least 1000
softLimitMin = mc.ReadObject(0x607d, 1) 
softLimitMax = mc.ReadObject(0x607d, 2)
if ((softLimitMin != 0) or (softLimitMax - softLimitMin) < 1000):
	print("Cycle testing abort. Check softlimit objects 607Dh.1h and 607Dh.2h!") 
else:
	# reset the time count variables (Python int8 object) 
	mc.WriteObject(0x3410, 0x01, 1)
	mc.WriteObject(0x3410, 0x02, 2)
	mc.WriteObject(0x3410, 0x03, 3)
	mc.WriteObject(0x3410, 0x04, 4)
	
	# initial wait, don't surprise me with immediate movement
	sys.wait(5000)
	# switch on positioning mode
	mc.EnableDrive()
	sys.wait(1000)
	while (1):
		mc.GoPosAbs(softLimitMin)
		ChkReady()
		sys.wait(WAIT_TIME_1_MSEC)
		mc.GoPosAbs(softLimitMax)
		ChkReady()
		sys.wait(WAIT_TIME_2_MSEC)