#id rev4 flatTRACK demo
# Fullmo MovingCap CODE - micropython example.
# original file name: flatTRACK_demo.py
# Example application for MovingCap flatTRACK 100 with firmware v53.01.07.xx_FLAT 
import sys
import mcdrive as mc

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

# only run this script if softlimit is set up properly
softLimitMin = mc.ReadObject(0x607d, 1) 
softLimitMax = mc.ReadObject(0x607d, 2)
if ((softLimitMax - softLimitMin < 50000) or softLimitMax > 999999):
	sys.Exit("flatTRACK demo abort. Check softlimit objects 607Dh.1h and 607D.2h!") 

# initial wait, don't surprise me with immediate movement
sys.wait(5000)
# switch on positioning mode
mc.EnableDrive()
# and go...
while(1):
	mc.SetAcc(150)
	mc.SetDec(150)
	mc.SetPosVel(10000)
	mc.GoPosAbs(softLimitMin + 5000)
	ChkReady()
	sys.wait(2000)

	mc.SetAcc(150)
	mc.SetDec(150)
	mc.SetPosVel(50000)
	mc.GoPosAbs(softLimitMin + 20000)
	ChkReady()
	sys.wait(2000)
	
	mc.SetAcc(200)
	mc.SetDec(200)
	mc.SetPosVel(100000)
	mc.GoPosAbs(softLimitMin + 20000)
	ChkReady()
	i = 0
	while (i < 15):
		i = i + 1
		sys.wait(200)
		mc.GoPosAbs(softLimitMin + 20000 + i * 3000)
	sys.wait(2000)
	
	mc.SetAcc(50)
	mc.SetDec(50)
	mc.SetPosVel(30000)
	mc.GoPosAbs(softLimitMax - 5000)
	ChkReady()
	sys.wait(5000)