#id rev5 shortTRACK_gyroscope.py gyroscope demo
# Fullmo MovingCap CODE - micropython example.
# original file name: shortTRACK_gyroscope.py
# MovingCap shortTRACK demo - detect horizontal/vertical orientation and adjust motion: 
# horizontal/flat - go back to default speed
# positive direction points to sky - decrease speed
# negative direction points to ground - increase speed
# precondition: 
# - Firmware v53.02.00.19_SRABBIT or higher 
# - Softlimit objects 607Dh.1h and 607D.2h set
import sys
import mcdrive as mc
		
# wait and integrate over current / force feedback
def waitAndCheckCurrent(delay):
	i = 0 
	sum = 0
	while (i < delay):
		sum = sum + mc.ReadObject(0x6078,0)
		i = i + 1
		sys.wait(1)
	return sum

# safety: only run this script if softlimit objects are set up properly
softLimitMin = mc.ReadObject(0x607d, 1) 
softLimitMax = mc.ReadObject(0x607d, 2)

if ((softLimitMin == 0 and softLimitMax == 0) or softLimitMax <= (softLimitMin + 20000)):
	print("flatTRACK demo abort. Check softlimit objects 607Dh.1h and 607Dh.2h!") 
else: 
	# initial wait, don't surprise me with immediate movement
	sys.wait(5000)

	# general init
	mc.EnableDrive()
	sys.wait(2000)
	# and go...	
	avg = 0
	speed = 37500
	shortWait = 500
	mc.SetPosVel(speed)
	mc.GoPosAbs((softLimitMin + softLimitMax) // 2)
	sys.wait(1000)
	while(1):
		mc.GoPosAbs(softLimitMin + 5000)
		sys.wait(shortWait * 3)
		mc.GoPosAbs(softLimitMin + 10000)
		avg = waitAndCheckCurrent(shortWait)
		mc.GoPosAbs(softLimitMin + 20000)
		avg = avg + waitAndCheckCurrent(shortWait)
		mc.GoPosAbs(softLimitMax - 5000)
		avg = avg + waitAndCheckCurrent(shortWait * 3)
		avg = avg // (shortWait * 8)
		# speed control with gravity
		if (avg < -15):
			if (speed > 9375):
				speed = speed // 2
		elif (avg > 20):
			if (speed < 2400000):
				speed = speed * 2
		else:
			speed = 37500
		mc.SetPosVel(speed)
		# adjust wait times a bit
		if speed < 37500:
			shortWait = 500 * 37500 // speed
		elif speed > 150000:
			shortWait = 250
		else:
			shortWait = 500