
# Drive to R2

###### Init - DO NOT MODIFY THE CODE BELOW ######
global sys, inspect, os, traceback
import sys, inspect, os, traceback


def startTest(testName, DEVICE, params):
    # by default, assume the test failed
    ret = 'Test failed'
    success = False
    # plot name to be given to the scan. Use: scan.setPlotTitle(plotName)
    plotName = DEVICE + ' - ' + testName
    # put the whole custom code under try/catch
    try:
        # get the path of this script
        testPath = inspect.getfile(inspect.currentframe())
        # init the testing tool class. It can be sued in the following ways:
        test = TestingTool(testName, testPath, DEVICE, params)
        samplePeriod = 0.1

######### WRITE YOUR CODE HERE BELOW #############
        scan = ManualScan(['time [1/'+ str(1/samplePeriod) + ' s]'], ['Motor Pos (IST3:2)', 'Poti Position (IST2:1)',
                                    'Motor Status (STA:1)', 'Inkr (INKR:2)', 'InkrRb (INKRRB:2)',
                                    'Diameter (DIAM:2)', 'Com (COM:2)', 'Logical Position (IST:2)',
                                    'Motor Pos Raw (IST3:1)','Poti Pos From Beam (IST1:2)',
                                    'Poti Raw (POSA:1)',  'Poti Ref1 Position (REF1:1)', 'Poti Ref2 Position (REF2:1)'])
        scan.setPlotTitle(plotName)
        scan.start()

        try:
            idMotorStatus       = Channel(DEVICE + ':STA:1'     , type = 'd') # DSP device_status reg
            idLogicalPosition   = Channel(DEVICE + ':IST:2'     , type = 'd') # Shows current position in logical units as calculated from motor step counter [1..n]
            idPotiRaw           = Channel(DEVICE + ':POSA:1'    , type = 'd') # poti raw data [ADC units]
            idMotorPositionRaw  = Channel(DEVICE + ':IST3:1'    , type = 'd') # shows current position in steps as as obtained from motor step counter [steps]
            idInkr              = Channel(DEVICE + ':INKR:2'    , type = 'd')  # move relative distance (positive means towards R2) [mm]
            idInkrRb            = Channel(DEVICE + ':INKRRB:2'  , type = 'd')  # readback of move relative distance (positive means towards R2) [mm]
            idDiameter          = Channel(DEVICE + ':DIAM:2'    , type = 'd')  # collimator diameter [mm]
            idCom               = Channel(DEVICE + ':COM:2'     , type = 'd')  # current position as from motor step counter [mm]
            idMotorPosition     = Channel(DEVICE + ':IST3:2'    , type = 'd')  # current position as from motor step counter [mm]
            idPotiPosFromBeam   = Channel(DEVICE + ':IST1:2'    , type = 'd')  # current position from beam as from potentiometer [mm]
            idPotiPosition      = Channel(DEVICE + ':IST2:1'    , type = 'd')  # current position as from potentiometer [mm]
            idPotiRef1Position  = Channel(DEVICE + ':REF1:1'    , type = 'd')  # R1 position as from potentiometer [mm]
            idPotiRef2Position  = Channel(DEVICE + ':REF2:1'    , type = 'd')  # R2 position as from potentiometer [mm]

            idCom.put('4', timeout=None) # go to R2
            sleep( samplePeriod ) # Settling time
        except:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            test.sendFeedback( ret, success)
            return

        #scan for a while to collect data
        for setpoint1 in range(0, 10000):
            sleep( samplePeriod ) # Settling time

            MotorStatus         = idMotorStatus.get()
            LogicalPosition     = idLogicalPosition.get()
            PotiRaw             = idPotiRaw.get()
            MotorPositionRaw    = idMotorPositionRaw.get()
            Com                 = idCom.get()
            MotorPosition       = idMotorPosition.get()
            PotiPosFromBeam     = idPotiPosFromBeam.get()
            PotiPosition        = idPotiPosition.get()
            PotiRef1Position    = idPotiRef1Position.get()
            PotiRef2Position    = idPotiRef2Position.get()
            Diameter            = idDiameter.get()
            Inkr                = idInkr.get()
            InkrRb              = idInkrRb.get()


            scan.append([setpoint2], [setpoint2],
                        [MotorPosition, PotiPosition, MotorStatus, Inkr, InkrRb, Diameter, Com, LogicalPosition,
                         MotorPositionRaw, PotiPosFromBeam, PotiRaw, PotiRef1Position, PotiRef2Position])

            if  (MotorStatus & int('1',2))==0: #device finished calibration (bit#1)
                #give the device some time before stating that it has really finished
                countDeviceInModeIdle = countDeviceInModeIdle +1
                if countDeviceInModeIdle == 10:
                    break
            else:
                countDeviceInModeIdle = 0

            #check if any error bit is raised
            if  bool(int(MotorStatus) & int('10000',2)): #error: abort test
                test.sendFeedback('Motor switched off (bit#4)', False)
                return
            if  bool(int(MotorStatus) & int('100000',2)): #error: abort test
                test.sendFeedback('No motor link (bit#5)', False)
                return
            if  bool(int(MotorStatus) & int('1000000',2)): #error: abort test
                test.sendFeedback('No poti link (bit#6)', False)
                return
            if  bool(int(MotorStatus) & int('10000000',2)): #error: abort test
                test.sendFeedback('Calibration error (bit#7)', False)
                return
            if  bool(int(MotorStatus) & int('100000000',2)): #error: abort test
                test.sendFeedback('Cannot get to R1 (bit#8)', False)
                return
            if  bool(int(MotorStatus) & int('1000000000',2)): #error: abort test
                test.sendFeedback('Cannot get to R2 (bit#9)', False)
                return
            if  bool(int(MotorStatus) & int('10000000000',2)): #error: abort test
                test.sendFeedback('Position measurement mismatch (bit#10)', False)
                return
            if  bool(int(MotorStatus) & int('100000000000',2)): #error: abort test
                test.sendFeedback('Movement timeout (bit#11)', False)
                return

        #Closing channels
        idMotorStatus.close()
        idLogicalPosition.close()
        idPotiRaw.close()
        idMotorPositionRaw.close()
        idCom.close()
        idMotorPosition.close()
        idPotiPosFromBeam.close()
        idPotiPosition.close()
        idPotiRef1Position.close()
        idPotiRef2Position.close()
        idDiameter.close()
        idInkr.close()
        idInkrRb.close()
        scan.end()

        ret = 'Moved to R2'
        success = True
        scan.end()

################ END OF YOUR CODE ################
###### Final - DO NOT MODIFY THE CODE BELOW ######

        # just in case the feedback was forgotten
        test.sendFeedback(ret, success)
    except:
        # generic error handler
        ret = traceback.format_exc()
        success = False
        test.sendFeedback(ret, success)

# launch the test
startTest(test, device, parameters)
