# testFilterWheel01
import random
from EZStepper import OpenSerial
from FilterWheel import FilterWheel
# Parameters for 22-position wheel
comport = 'COM8' # Change to the COM port on your computer
TimeOut = 60 # Duration of longest move
ControllerName = 1 # Name (number) of controller on this port. Set by DIP switch in controller during assembly. Do not change.
FilterPositions = 22 # Number of filter positions in the wheel.
MotorSteps = 200 # Physical steps in the motor; typically 200.
HoldCurrent = 10 # current when not moving; in % of half of full current (2A for EZHR17EN); typically 5-10
RunCurrent = 95 # current when moving; in % of full current (2A for EZHR17EN); typically 50-95%
StepDivide = 16 # number of microsteps per step; typically 16 for filter wheels.
OffsetFromHome = -2 # offset from home to the position of the first filter in microsteps - used for fine alignment (typically < +/-4)
SlewVelocity = 50000 # slew (max) velocity in microsteps/sec (typically 5000 - 50000)
InitVelocity = 500 # initial velocity in microsteps/sec (typically 100 - 500) 
Ramp = 3 # acceleration (deceleration) in microsteps/sec^2 (typically 1 - 3)
# maximal values were determined with a wheel fully loaded with aluminum blanks 4 mm thick

# %% Open port and create instance of FilterWheel object; then home the wheel
SerialPort = OpenSerial(comport)
ret = SerialPort.is_open
fw = FilterWheel(SerialPort, TimeOut=TimeOut, ControllerName=ControllerName, FilterPositions=FilterPositions, MotorSteps=MotorSteps, \
                 HoldCurrent=HoldCurrent, RunCurrent=RunCurrent, StepDivide=StepDivide, SlewVelocity=SlewVelocity, InitVelocity=InitVelocity, \
                 Ramp=Ramp, OffsetFromHome=OffsetFromHome)
errh = fw.WheelHome()

# %% Random moves
nrep = 100
for i in range(nrep):
    fnum_new = int(random.random()*FilterPositions + 0.5)
    status = fw.WheelMove(fnum_new) # start moving to new filter position
    errw = fw.WheelWait() # wait for completion of the move
    fnum = fw.fnum
    fnum_read = fw.WheelPos() # this is normally not needed; just checking that controller position corresponds to software position
    if(fnum != fnum_read):
        print('fnum=' + str(fnum) + ', fnum_read=' + str(fnum_read))

# %% Close port
SerialPort.close()
ret = SerialPort.is_open