#id rev2 flatTRACK standard ramp testing
# Fullmo MovingCap CODE - micropython example.
# original file name: standard_ramp_testing.py
# Internal tests for various standard motion profiles
# 2024-08-02 o.heggelbacher
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)

def doRamp(runNo, target, speed, acc, dec):
	mc.SetAcc(acc)
	mc.SetDec(dec)
	mc.SetPosVel(speed)
	mc.WriteObject(0x340c, 0x01, runNo)
	mc.GoPosAbs(target)
	ChkReady()
	mc.WriteObject(0x340c, 0x01, 0)
	sys.wait(2000)
	mc.WriteObject(0x340c, 0x01, -runNo)
	mc.GoPosAbs(0)
	ChkReady()
	mc.WriteObject(0x340c, 0x01, 0)
	sys.wait(2000)

def doSetOfRamps():
	doRamp(1, 400000, 500000, 2500, 2500)
	doRamp(1, 400000, 500000, 500, 500)
	doRamp(1, 400000, 500000, 100, 100)

	doRamp(1, 400000, 100000, 500, 500)
	doRamp(1, 400000, 100000, 100, 100)
	doRamp(1, 400000, 100000, 20, 20)

	doRamp(1, 100000, 20000, 100, 100)
	doRamp(1, 100000, 20000, 20, 20)
	doRamp(1, 100000, 20000, 10, 10)

	doRamp(1, 10000, 2000, 20, 20)
	doRamp(1, 10000, 2000, 10, 10)

# safety: only run this script if softlimit objects are set up properly
# - limit min must be 0
# - limit max must be at least 400000
softLimitMin = mc.ReadObject(0x607d, 1) 
softLimitMax = mc.ReadObject(0x607d, 2)
if (softLimitMin != 0 or softLimitMax < 400000 or softLimitMax > 999999):
	print("flatTRACK ramp testing abort. Check softlimit objects 607Dh.1h and 607Dh.2h!") 

else:
	# reset the feedback variable (Python int8 object) 
	mc.WriteObject(0x340c, 0x01, 0)
	# initial wait, don't surprise me with immediate movement
	sys.wait(5000)
	# switch on positioning mode
	mc.EnableDrive()

	# back to start
	mc.SetAcc(10)
	mc.SetDec(10)
	mc.SetPosVel(40000)
	mc.GoPosAbs(0)
	ChkReady()
	
	while (1):
		doSetOfRamps()