xRot, yRot rotated coordinates for x and y, thread

For a table which has motors for x and y, 2 virtual motors have been written that allow for movements using rotated coordinates.

Note that the physical movements are executed one after the other.

This is the xRot code:

#!/bin/env python
"""
   xRot = xHW*cos(phi) + yHW*sin(phi)
   yRot = yHW*cos(phi) - xHW*sin(phi)

   xHW = xRot*cos(phi) - yRot*sin(phi)
   yHW = yRot*cos(phi) + xRot*sin(phi)
"""
import PyTango
import math, time, os
from threading import Thread
ANGLE = 45
TIME_SLEEP = 0.1

nameXHW = "motor/hexa_1/1"
nameYHW = "motor/hexa_2/1"

isBusy = False
flagDebugIO = False

class MoveThread( Thread): 
    def __init__( self, x, pX, y, pY):
        Thread.__init__( self)
        self.x = x
        self.y = y
        self.proxyX = pX
        self.proxyY = pY
    def run(self):
        global isBusy

        if flagDebugIO:
            print( "xRot.thread: starting, xHW %g, yHW %g" % ( self.x, self.y))

        isBusy = True
        self.proxyX.write_attribute( "Position", self.x) 
        time.sleep(TIME_SLEEP)
        while self.proxyX.state() == PyTango.DevState.MOVING:
            if flagDebugIO:
                print( "xRot.thread: xHW %g " % self.proxyX.Position)
            time.sleep(TIME_SLEEP)
        if flagDebugIO:
            print( "xRot.thread: xHW %g DONE" % self.proxyX.Position)
        self.proxyY.write_attribute( "Position", self.y) 
        time.sleep(TIME_SLEEP)
        while self.proxyY.state() == PyTango.DevState.MOVING:
            if flagDebugIO:
                print( "xRot.thread: yHW %g " % self.proxyY.Position)
            time.sleep(TIME_SLEEP)
        if flagDebugIO:
            print( "xRot.thread: yHW %g DONE" % self.proxyY.Position)
        isBusy = False
        if flagDebugIO:
            print( "xRot.thread: DONE")

class VM:
    #
    # init_device
    #
    def __init__( self):
        global flagDebugIO 
        self.ResultSim = None
        self.PositionSim = None
        self.proxyXHW = PyTango.DeviceProxy( nameXHW)
        self.proxyYHW = PyTango.DeviceProxy( nameYHW)

        self.proxyXHW_tango = PyTango.DeviceProxy( self.proxyXHW.TangoDevice)
        self.proxyYHW_tango = PyTango.DeviceProxy( self.proxyYHW.TangoDevice)
        
        flagDebugIO = False
        if os.isatty(1):
            flagDebugIO = True
        if flagDebugIO:
            print( "xRot.init")
        return
    #
    # dev_state
    #
    def dev_state( self):
        global isBusy
        argout = PyTango.DevState.ON
        if isBusy:
            return PyTango.DevState.MOVING
        #
        # if one device is in FAULT the VM is in FAULT too
        #
        for proxy in [self.proxyXHW, self.proxyYHW]:
            if proxy.state() == PyTango.DevState.FAULT:
                argout = PyTango.DevState.FAULT
                break
        if argout == PyTango.DevState.ON:
            #
            # if one device is MOVING the VM is MOVING too
            #
            for proxy in [self.proxyXHW, self.proxyYHW]:
                if proxy.state() == PyTango.DevState.MOVING:
                    argout = PyTango.DevState.MOVING
                    break
        return argout
    #
    # Position
    #
    def read_Position( self):
        xHW = self.proxyXHW.Position
        yHW = self.proxyYHW.Position

        xRot = xHW*math.cos( math.radians( ANGLE)) + yHW*math.sin( math.radians( ANGLE))
        yRot = yHW*math.cos( math.radians( ANGLE)) - xHW*math.sin( math.radians( ANGLE))

        #if flagDebugIO:
        #    print( "xRot.read_position: %g" % xRot)
        return xRot

    def write_Position( self, argin):
        xHW = self.proxyXHW.Position
        yHW = self.proxyYHW.Position
        # xRot = xHW*math.cos( math.radians( ANGLE)) + yHW*math.sin( math.radians( ANGLE))
        yRot = yHW*math.cos( math.radians( ANGLE)) - xHW*math.sin( math.radians( ANGLE))
        
        xRot = argin

        xHW = xRot*math.cos( math.radians( ANGLE)) - yRot*math.sin( math.radians( ANGLE))
        yHW = yRot*math.cos( math.radians( ANGLE)) + xRot*math.sin( math.radians( ANGLE))

        if xHW < self.proxyXHW_tango.UnitLimitMin or xHW > self.proxyXHW_tango.UnitLimitMax:
            PyTango.Except.throw_exception(
                "xRot",
                "requested position outside limits %g %g " % (float(attrXHW.min_value), 
                                                              float(attrXHW.max_value)),
                "VmExecutor")		
        
        if yHW < self.proxyYHW_tango.UnitLimitMin or yHW > self.proxyYHW_tango.UnitLimitMax:
            PyTango.Except.throw_exception(
                "xRot",
                "requested position outside limits %g %g " % (float(attrYHW.min_value), 
                                                              float(attrYHW.max_value)),
                "VmExecutor")		
        mvThread = MoveThread( xHW, self.proxyXHW, yHW, self.proxyYHW)
        mvThread.start()
        return 1
    #
    # UnitLimitMax
    #
    def read_UnitLimitMax( self):
        if flagDebugIO:
            print( "xRot.read_unitLimitMax %g" % self.proxyXHW_tango.UnitLimitMax)
        return self.proxyXHW_tango.UnitLimitMax*1.414

    def write_UnitLimitMax( self, argin):
        pass
    #
    # UnitLimitMin
    #
    def read_UnitLimitMin( self):
        if flagDebugIO:
            print( "xRot.read_unitLimitMin %g" % self.proxyXHW_tango.UnitLimitMin)
        return self.proxyXHW_tango.UnitLimitMin*1.414
    
    def write_UnitLimitMin( self, argin):
        pass
    #
    # CwLimit, CcwLimit
    #
    def read_CwLimit( self):
        return 0

    def read_CcwLimit( self):
        return 0
    #	
    # PositionSim	
    #	
    def read_PositionSim( self):
        return 0

    def write_PositionSim( self, argin):
        pass

    def read_ResultSim( self):
        pass

    def StopMove( self):
        self.proxyXHW.StopMove()
        self.proxyYHW.StopMove()
        return True

The is the yRot code:

#!/bin/env python
"""
   xRot = xHW*cos(phi) + yHW*sin(phi)
   yRot = yHW*cos(phi) - xHW*sin(phi)

   xHW = xRot*cos(phi) - yRot*sin(phi)
   yHW = yRot*cos(phi) + xRot*sin(phi)
"""
import PyTango
import math, time, os
from threading import Thread

ANGLE = 45
TIME_SLEEP = 0.1


nameXHW = "motor/hexa_1/1"
nameYHW = "motor/hexa_2/1"

isBusy = False
flagDebugIO = False

class MoveThread( Thread): 
    def __init__( self, x, pX, y, pY):
        Thread.__init__( self)
        self.x = x
        self.y = y
        self.proxyX = pX
        self.proxyY = pY
    def run(self):
        global isBusy

        if flagDebugIO:
            print( "yyRot.thread: starting, xHW %g, yHW %g" % ( self.x, self.y))

        isBusy = True
        self.proxyX.write_attribute( "Position", self.x) 
        time.sleep(TIME_SLEEP)
        while self.proxyX.state() == PyTango.DevState.MOVING:
            if flagDebugIO:
                print( "yRot.thread: xHW %g " % self.proxyX.Position)
            time.sleep(TIME_SLEEP)
        if flagDebugIO:
            print( "yRot.thread: xHW %g DONE" % self.proxyX.Position)
        self.proxyY.write_attribute( "Position", self.y) 
        time.sleep(TIME_SLEEP)
        while self.proxyY.state() == PyTango.DevState.MOVING:
            if flagDebugIO:
                print( "yRot.thread: yHW %g " % self.proxyY.Position)
            time.sleep(TIME_SLEEP)
        if flagDebugIO:
            print( "yRot.thread: yHW %g DONE" % self.proxyY.Position)
        isBusy = False
        if flagDebugIO:
            print( "yRot.thread: DONE")

class VM:
    #
    # init_device
    #
    def __init__( self):
        global flagDebugIO
        self.ResultSim = None
        self.PositionSim = None
        self.proxyXHW = PyTango.DeviceProxy( nameXHW)
        self.proxyYHW = PyTango.DeviceProxy( nameYHW)

        self.proxyXHW_tango = PyTango.DeviceProxy( self.proxyXHW.TangoDevice)
        self.proxyYHW_tango = PyTango.DeviceProxy( self.proxyYHW.TangoDevice)
        flagDebugIO = False
        if os.isatty(1):
            flagDebugIO = True
        if flagDebugIO:
            print( "yRot.init")
        return
    #
    # dev_state
    #
    def dev_state( self):
        global isBusy
        argout = PyTango.DevState.ON
        if isBusy:
            return PyTango.DevState.MOVING
        #
        # if one device is in FAULT the VM is in FAULT too
        #
        for proxy in [self.proxyXHW, self.proxyYHW]:
            if proxy.state() == PyTango.DevState.FAULT:
                argout = PyTango.DevState.FAULT
                break
        if argout == PyTango.DevState.ON:
            #
            # if one device is MOVING the VM is MOVING too
            #
            for proxy in [self.proxyXHW, self.proxyYHW]:
                if proxy.state() == PyTango.DevState.MOVING:
                    argout = PyTango.DevState.MOVING
                    break
                    
        return argout
    #
    # Position
    #
    def read_Position( self):
        xHW = self.proxyXHW.Position
        yHW = self.proxyYHW.Position

        xRot = xHW*math.cos( math.radians( ANGLE)) + yHW*math.sin( math.radians( ANGLE))
        yRot = yHW*math.cos( math.radians( ANGLE)) - xHW*math.sin( math.radians( ANGLE))

        #if flagDebugIO:
        #    print( "yRot.read_position: %g" % yRot)

        return yRot

    def write_Position( self, argin):
        xHW = self.proxyXHW.Position
        yHW = self.proxyYHW.Position
        xRot = xHW*math.cos( math.radians( ANGLE)) + yHW*math.sin( math.radians( ANGLE))
        #yRot = yHW*math.cos( math.radians( ANGLE)) - xHW*math.sin( math.radians( ANGLE))
        
        yRot = argin

        xHW = xRot*math.cos( math.radians( ANGLE)) - yRot*math.sin( math.radians( ANGLE))
        yHW = yRot*math.cos( math.radians( ANGLE)) + xRot*math.sin( math.radians( ANGLE))

        if xHW < self.proxyXHW_tango.UnitLimitMin or xHW > self.proxyXHW_tango.UnitLimitMax:
            PyTango.Except.throw_exception(
                "xRot",
                "requested position outside limits %g %g " % (float( attrXHW.min_value), 
                                                              float( attrXHW.max_value)),
                "VmExecutor")		
        
        if yHW < self.proxyYHW_tango.UnitLimitMin or yHW > self.proxyYHW_tango.UnitLimitMax:
            PyTango.Except.throw_exception(
                "xRot",
                "requested position outside limits %g %g " % (float( attrYHW.min_value), 
                                                              float( attrYHW.max_value)),
                "VmExecutor")		

        mvThread = MoveThread( xHW, self.proxyXHW, yHW, self.proxyYHW)
        mvThread.start()
        return 1
    #
    # UnitLimitMax
    #
    def read_UnitLimitMax( self):
        if flagDebugIO:
            print( "yRot.read_unitLimitMax %g" % self.proxyXHW_tango.UnitLimitMax)
        return self.proxyYHW_tango.UnitLimitMax*1.414

    def write_UnitLimitMax( self, argin):
        pass
    #
    # UnitLimitMin
    #
    def read_UnitLimitMin( self):
        if flagDebugIO:
            print( "yRot.read_unitLimitMax %s" % self.proxyXHW_tango.UnitLimitMin)
        return self.proxyYHW_tango.UnitLimitMin*1.414
    
    def write_UnitLimitMin( self, argin):
        pass
    #
    # CwLimit, CcwLimit
    #
    def read_CwLimit( self):
        return 0

    def read_CcwLimit( self):
        return 0
    #	
    # PositionSim	
    #	
    def read_PositionSim( self):
        return 0

    def write_PositionSim( self, argin):
        pass     

    def read_ResultSim( self):
        pass

    def StopMove( self):
        self.proxyXHW.StopMove()
        self.proxyYHW.StopMove()
        return True