#id driveSimplePosTest.py 2024-07-25 oh
# Fullmo MovingCap CODE - micropython example.
# original file name: driveSimplePosTest.py
# This is a simple positioning demo for a MovingCap turnTRACK 349 
# rotary drive.
# it will set the "feed constant" to 360, so one motor (optionally: gear) 
# revolution corresponds to a position change of 360°
import sys
import mcdrive as mc

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

# initial wait, don't surprise me with immediate movement
sys.wait(2000)

# Set 6092h.01h Feed constant to 360 --> one turn = 360°
mc.WriteObject(0x6092, 0x01, 360)

# general init
mc.EnableDrive()
mc.SetAcc(500)
mc.SetDec(500)
mc.SetPosVel(360)
# and go...
while(1):
	mc.GoPosAbs(0)
	ChkReady()
	mc.GoPosAbs(1080)
	ChkReady()
	mc.GoPosAbs(360) 
	ChkReady()