aswi2021virtual-surreality-gitlab/deltarobot-test-server/mbServer.py @ 6e6f12dc

#!/usr/bin/env python
"""
Pymodbus Server With Updating Thread
--------------------------------------------------------------------------

This is an example of having a background thread updating the
context while the server is operating. This can also be done with
a python thread::

    from threading import Thread

    thread = Thread(target=updating_writer, args=(context,))
    thread.start()
"""
# --------------------------------------------------------------------------- #
# import the modbus libraries we need
# --------------------------------------------------------------------------- #
from pymodbus.server.asynchronous import StartTcpServer
from pymodbus.device import ModbusDeviceIdentification
from pymodbus.datastore import ModbusSequentialDataBlock
from pymodbus.datastore import ModbusSlaveContext, ModbusServerContext

# --------------------------------------------------------------------------- #
# import the twisted libraries we need
# --------------------------------------------------------------------------- #
from twisted.internet.task import LoopingCall

# --------------------------------------------------------------------------- #
# configure the service logging
# --------------------------------------------------------------------------- #
import logging
logging.basicConfig()
log = logging.getLogger()
log.setLevel(logging.ERROR)

# random numbers
import random

# global constants

# discrete input
# poloha klicku
BIT_BKEY = 32789
# bezpecnostni tlacitko
BIT_SAFETY_BTN = 32785

# holding regs
# aktualni poloha
REG_X = 32768
REG_Y = 32769
REG_Z = 32770
REG_A = 32771
#pozice vektoru, kam se tlaci
REG_ACT_X = 32772
REG_ACT_Y = 32773
REG_ACT_Z = 32774
#pozice vektoru, kam se ma tlacit
REG_DIR_X = 32776
REG_DIR_Y = 32777
REG_DIR_Z = 32778

#data s krivkou
REG_INDEX = 32867
REG_LEN = 32866
REG_DATA = 32868
# data s kvalitou cviceni
REG_Q_1 = 32780
REG_Q_TOT = 32781
REG_Q_PART = 32782

#coils
#info bity 
BIT_RDY = 32871
BIT_PC_TO_PLC = 32869
BIT_PLC_TO_PC = 32870
BIT_MEASUREMENT_TO_PC = 32874
#posun motoru
BIT_M_UP = 32788
BIT_M_DOWN = 32789
# uceni
BIT_TEACH = 32780
# reset pozice
BIT_RESET_POS = 32769
# prehravani naucene krivky
BIT_BPLAY = 32772
#cviceni naucene kriky
BIT_BPLAY_ASSIST = 32773
#priznak ukonceni jednoho cyklu
BIT_PART_FINISHED = 32873
#adresy pro testovaci zmenu DI
BIT_BKEY_CHANGE = 32950
BIT_SBTN_CHANGE = 32951
#test stop coil
BIT_STOP = 32838

# modbus reg
MB_CO = 1
MB_DI = 2
MB_HR = 3
MB_IR = 4

# --------------------------------------------------------------------------- #
# define your callback process
# --------------------------------------------------------------------------- #
def prepareMbRegVals(values):
    vals = []
    for val in values:
        if val < 0:
            val = val & 0xFFFF
        vals.append(val)
    return vals

def updating_writer(a,):
    """ A worker process that runs every so often and
    updates live values of the context. It should be noted
    that there is a race condition for the update.

    :param arguments: The input arguments to the call
    """
    global bit_teach, bit_play, bit_rdy, bit_data_change, bit_motor, learned_curve, index_curve, meas_array
    global cycles_no, quality, array_pos

    reset_pos = [0,0,4000]
    
    log.debug("updating the context")
    context = a[0]
    
    
    brdy = context[0].getValues(MB_CO, BIT_RDY, 1)
    data_change = context[0].getValues(MB_CO, BIT_PC_TO_PLC, 2)
    measurement_get = context[0].getValues(MB_CO, BIT_MEASUREMENT_TO_PC, 1)
    motor = context[0].getValues(MB_CO, BIT_M_UP, 2)
    bteach = context[0].getValues(MB_CO, BIT_TEACH, 1)
    bplay = context[0].getValues(MB_CO, BIT_BPLAY, 2)

    if (context[0].getValues(MB_CO, BIT_RESET_POS, 1) == [1]):
        print("Position reset")
        context[0].setValues(MB_CO, BIT_RESET_POS, [0])
        context[0].setValues(MB_HR, REG_X, reset_pos)
    if (bteach != bit_teach):
        if (bteach == [1]):
            bplay = [0, 0]
            context[0].setValues(MB_CO, BIT_BPLAY, bplay)
            learned_curve = []
            array_pos = 0
            f = open("curve.txt")
            for line in f:
                line.rstrip()
                lineStr = line.split(";")
                onePoint = [int(i) for i in lineStr]
                learned_curve.extend(prepareMbRegVals(onePoint))
                print(onePoint)
            print(len(learned_curve)//4)
        else:
            index_curve = 0
    if (motor != bit_motor):
        if (motor[0] == 1):
            context[0].setValues(MB_CO, BIT_STOP, [1])
            print("Up")
        elif (motor[1] == 1):
            context[0].setValues(MB_CO, BIT_STOP, [1])
            print("Down")
        else:
            context[0].setValues(MB_CO, BIT_STOP, [0])
            print("Stop")
    if (bit_data_change != data_change):
        if (data_change == [0,0]):
            brdy = [0]
            context[0].setValues(MB_CO, BIT_RDY, brdy)
    if (data_change[0] == 1):
        if (bit_rdy != brdy) and (brdy == [1]):
            val = context[0].getValues(MB_HR, REG_LEN, 2)
            print("<- Len: " + str(val[0]) + ", Index: " + str(val[1]))
            points = val[0]-val[1]
            if (points > 25):
                points = 25
            learned_curve.extend(context[0].getValues(MB_HR, REG_DATA, points*4))
            brdy = [0]
            context[0].setValues(MB_CO, BIT_RDY, brdy)
    if (data_change[0] != bit_data_change[0]):
        if (data_change[0] == 0):
            index_curve = 0
            print("curve finish")
        else:
            bplay = [0, 0]
            context[0].setValues(MB_CO, BIT_BPLAY, bplay)
            learned_curve = []
    if (data_change[1] == 1):
        if (brdy == [0]):
            index_to = index_curve + 25
            if (len(learned_curve)-(index_curve*4) < 100):
                index_to = len(learned_curve)//4
            context[0].setValues(MB_HR, REG_DATA, learned_curve[index_curve*4:index_to*4])
            brdy = [1]
            context[0].setValues(MB_HR, REG_LEN, [len(learned_curve)//4])
            context[0].setValues(MB_HR, REG_INDEX, [index_curve])
            context[0].setValues(MB_CO, BIT_RDY, brdy)
            print("-> Len: " + str(len(learned_curve)) + ", Index: " + str(index_curve) + ", to: " + str(index_to))
            index_curve += 25
            if (index_curve*4 > len(learned_curve)):
                context[0].setValues(MB_CO, BIT_PLC_TO_PC, [0])
                index_curve = 0
    if (measurement_get == [1]):
        if (brdy == [0]):
            index_to = index_curve + 10
            if (len(meas_array)-(index_curve*10) < 100):
                index_to = len(meas_array)//10
            context[0].setValues(MB_HR, REG_DATA, meas_array[index_curve*10:index_to*10])
            brdy = [1]
            context[0].setValues(MB_HR, REG_LEN, [len(meas_array)//10])
            context[0].setValues(MB_HR, REG_INDEX, [index_curve])
            context[0].setValues(MB_CO, BIT_RDY, brdy)
            print("-> Len: " + str(len(meas_array)) + ", Index: " + str(index_curve) + ", to: " + str(index_to))
            index_curve += 10
            if (index_curve*10 > len(learned_curve)):
                context[0].setValues(MB_CO, BIT_MEASUREMENT_TO_PC, [0])
                index_curve = 0
            print("measure")
    if (bplay != bit_play):
        cycles_no = 0
        array_pos = 0
        quality = [0,0,0]
    if (bit_play[1] == 1):
        onePoint = learned_curve[array_pos*4:array_pos*4+3]
        context[0].setValues(MB_HR, REG_X, onePoint)
        
        values = [random.randint(-500,500),random.randint(-500,500),random.randint(-200,200),0]
        onePoint = prepareMbRegVals(values)
        context[0].setValues(MB_HR, REG_ACT_X, onePoint)
        values = [random.randint(-200,200),random.randint(-200,200),random.randint(-70,70),0]
        onePoint = prepareMbRegVals(values)
        context[0].setValues(MB_HR, REG_DIR_X, onePoint)
        cycles_no += 1
        array_pos += 3
        if (array_pos*4 > len(learned_curve)):
            array_pos = 0
        if (array_pos == 0):
            quality[0] = (random.randint(45,100));
            context[0].setValues(MB_CO, BIT_PART_FINISHED, [1])
        if (cycles_no%25 == 0):
            quality[1] = (random.randint(45,100));
        quality[2] = (random.randint(45,100));
        context[0].setValues(MB_HR, REG_Q_1, quality)
    else:
        if (bteach == [1]):
            #learned_curve.extend(onePoint);
            onePoint = learned_curve[array_pos*4:array_pos*4+4]
            context[0].setValues(MB_HR, REG_X, onePoint)
            array_pos += 8
            if (array_pos > len(learned_curve)//4):
                context[0].setValues(MB_CO, BIT_TEACH, [0])
        else:
            values = [random.randint(-1000,1000),random.randint(-1000,1000),random.randint(5000,8000),0]
            onePoint = prepareMbRegVals(values)
            context[0].setValues(MB_HR, REG_X, onePoint)
            context[0].setValues(MB_HR, REG_X, reset_pos)
    
    #test: set input register using testing coils
    #bkey_change = context[0].getValues(MB_CO, BIT_BKEY_CHANGE, 2)
    bkey_change = [1,1]
    context[0].setValues(MB_DI, BIT_BKEY, [bkey_change[0]])
    context[0].setValues(MB_DI, BIT_SAFETY_BTN, [bkey_change[1]])
        
    bit_rdy = brdy
    bit_data_change = data_change
    bit_motor = motor
    bit_teach = bteach
    bit_play = bplay

def run_updating_server():
    # ----------------------------------------------------------------------- # 
    # initialize your data store
    # ----------------------------------------------------------------------- # 
    
    store = ModbusSlaveContext(
        di=ModbusSequentialDataBlock(32768, [0]*200),
        co=ModbusSequentialDataBlock(32768, [0]*200),
        hr=ModbusSequentialDataBlock(32768, [0]*200),
        ir=ModbusSequentialDataBlock(32768, [0]*200),zero_mode=True)
    context = ModbusServerContext(slaves=store, single=True)

    store.setValues(MB_DI, BIT_SAFETY_BTN, [1])
    store.setValues(MB_DI, BIT_BKEY, [1])
    # ----------------------------------------------------------------------- # 
    # initialize the server information
    # ----------------------------------------------------------------------- # 
    identity = ModbusDeviceIdentification()
    identity.VendorName = 'pymodbus'
    identity.ProductCode = 'PM'
    identity.VendorUrl = 'http://github.com/bashwork/pymodbus/'
    identity.ProductName = 'pymodbus Server'
    identity.ModelName = 'pymodbus Server'
    identity.MajorMinorRevision = '1.0'

    # ----------------------------------------------------------------------- # 
    # run the server you want
    # ----------------------------------------------------------------------- # 
    time = 0.333  # 333 mseconds delay
    loop = LoopingCall(f=updating_writer, a=(context,))
    loop.start(time, now=False) # initially delay by time
    StartTcpServer(context, identity=identity, address=("localhost", 1502))


bit_teach = 0
bit_rdy = [0]
bit_data_change = [0,0]
bit_motor = [0,0]
bit_play = [0,0]
learned_curve = []
index_curve = 0
cycles_no = 0
array_pos = 0
quality = [0,0,0]
# array with measurement values created #
meas_array = [(j) for j in range(270)]

if __name__ == "__main__":
    run_updating_server()