agrobot_base/Python/bld300r/serial_test.py

import serial
import struct
import time
import crcmod

# Configuração da porta serial
ser = serial.Serial(
    port='COM3',  # Substitua pelo seu porto serial
    baudrate=9600,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_TWO,
    timeout=1
)

slave_address = 0x02  # Endereço do escravo

CodigosFuncoes = {
    "GetParametro": 0x03, 
    "SetParametro": 0x06, 
}

Parametros = {
    "RPM_SP": 0x0056,
    "RPM": 0x005F,
    "Enabled": 0x0066,
    "Breaked": 0x006A,
    "Direction": 0x006D,
    "AlarmeCode": 0x0076,
    "NumPolos": 0x0086,
    "Rampa": 0x008A,
    "VelocidadeMaxAnalog": 0x0092,
    "Current": 0x0096,
    "ControleRS485": 0x00B6,
    "Versao": 0x00BB,
    "Voltage": 0x00C8,
    "Temperature": 0x00D2
}
Parametros2 = {
    "RPM_SP": (0x0056, 2),
    "RPM": (0x005F, 2),
    "Enabled": (0x0066, 2),
    "Breaked": (0x006A, 2),
    "Direction": (0x006D, 2),
    "AlarmeCode": (0x0076, 2),
    "NumPolos": (0x0086, 2),
    "Rampa": (0x008A, 2),
    "VelocidadeMaxAnalog": (0x0092, 2),
    "Current": (0x0096, 4),
    "ControleRS485": (0x00B6, 2),
    "Versao": (0x00BB, 2),
    "Voltage": (0x00C8, 4),  # Exemplo de parâmetro que retorna 4 bytes
    "Temperature": (0x00D2, 4)  # Exemplo de parâmetro que retorna 4 bytes
}


def crc16(data: bytes):
    crc = 0xFFFF
    for pos in data:
        crc ^= pos
        for i in range(8):
            if (crc & 1) != 0:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    return crc.to_bytes(2, byteorder='little')

def montar_comando(funcao, parametro, val):
    function_code = CodigosFuncoes[funcao]
    start_address = Parametros[parametro]
    num_of_registers = val
    command = bytearray([slave_address, function_code]) + start_address.to_bytes(2, byteorder='big') + num_of_registers.to_bytes(2, byteorder='big')
    command += crc16(command)
    return command

def enviar_comando(comando):
    ser.write(comando)
    time.sleep(0.1)  # Pequena pausa para garantir o envio
    resposta = ser.read(ser.inWaiting())  # Lê todos os dados disponíveis na porta
    return resposta

def decifrar_resposta(resposta, funcao, parametro):
    # Certifique-se de que a resposta é do tipo bytes
    if isinstance(resposta, str):
        resposta = bytes.fromhex(resposta)

    resHex = resposta.hex()
    
    #print(resposta)
    #print(resposta.hex())

    # Verifica o comprimento da resposta e o código da função
    if len(resposta) >= 5 and resposta[1] == CodigosFuncoes[funcao]:
        remetente = resposta[0]
        # Extrai o número de bytes de dados
        num_bytes = resposta[2]

        # Verifica se o comprimento da resposta é consistente com o número de bytes de dados
        if len(resposta) == 5 + num_bytes:
            valor_raw = int.from_bytes(resposta[3:3 + num_bytes], byteorder='big')
            
            valor = valor_raw # * 1  # Ajuste o fator de multiplicação conforme necessário

            #print(f"Remetente: {remetente}, {parametro}: {valor}")
            return (remetente, valor)
        else:
            print("Comprimento da resposta inconsistente com o número de bytes de dados.")
    else:
        print("Resposta inválida ou código de função incorreto.")
    return (0, None)


def montar_comando_todos_parametros():
    function_code = CodigosFuncoes["GetParametro"]
    start_address = 0x0056
    num_of_registers = 32
    command = bytearray([slave_address, function_code]) + start_address.to_bytes(2, byteorder='big') + num_of_registers.to_bytes(2, byteorder='big')
    command += crc16(command)
    return command

def decifrar_resposta_todos_parametros(resposta):
    # Certifique-se de que a resposta é do tipo bytes
    if isinstance(resposta, str):
        resposta = bytes.fromhex(resposta)

    print(resposta.hex())
    
    # Verifica o comprimento da resposta e o código da função
    if len(resposta) >= 5 and resposta[1] == CodigosFuncoes["GetParametro"]:
        remetente = resposta[0]
        # Extrai o número de bytes de dados
        num_bytes = resposta[2]

        print(f"Remetente: {remetente}, num_Bytes: {num_bytes}")

        # Verifica se o comprimento da resposta é consistente com o número de bytes de dados
        if len(resposta) == 5 + num_bytes or True:
            valores = {}
            for parametro, (endereco, tamanho) in Parametros2.items():
                offset = (endereco - 0x0056) * 2 + 3
                if offset + tamanho <= len(resposta):
                    valor_raw = int.from_bytes(resposta[offset:offset + tamanho], byteorder='big')
                    valores[parametro] = valor_raw * 1  # Ajuste o fator de multiplicação conforme necessário
            
            return (remetente, valores)
        else:
            print("Comprimento da resposta inconsistente com o número de bytes de dados.")
    else:
        print("Resposta inválida ou código de função incorreto.")
    return (0, {})

try:
    while True:
        resultados = []
        remetente = 0

        res = enviar_comando(montar_comando("SetParametro", "ControleRS485", 1))
        res = enviar_comando(montar_comando("SetParametro", "VelocidadeMaxAnalog", 500))

        if (False):
            for parametro in Parametros:
                comando = montar_comando("GetParametro", parametro, 1)
                res = enviar_comando(comando)
                remetente, valor = decifrar_resposta(res, "GetParametro", parametro)
                resultados.append((parametro, valor))

            # Imprime todos os resultados em uma linha, separados por tabulações
            linha_resultados = f"Remetente: {remetente}\t" + ("\t".join(f"{parametro}: {valor}" for parametro, valor in resultados))
            print(linha_resultados)

        d = 0
        if (True):

            if (d == 0):
                d = 1
            else:
                d = 0

            time.sleep(1)
            res = enviar_comando(montar_comando("SetParametro", "Enabled", 1))
            print(res.hex())
            time.sleep(0.1)
            res = enviar_comando(montar_comando("SetParametro", "Direction", d))
            print(res)
            time.sleep(0.1)
            res = enviar_comando(montar_comando("SetParametro", "RPM_SP", 250))
            print(res.hex())
            time.sleep(5)
            res = enviar_comando(montar_comando("SetParametro", "RPM_SP", 0))
            print(res.hex())
            time.sleep(1)
            res = enviar_comando(montar_comando("SetParametro", "Enabled", 0))
            print(res.hex())
            time.sleep(1)

        time.sleep(1)
except KeyboardInterrupt:
    res = enviar_comando(montar_comando("SetParametro", "RPM_SP", 0))
    print(res.hex())
    time.sleep(0.1)
    res = enviar_comando(montar_comando("SetParametro", "Enabled", 0))
    print("Programa encerrado pelo usuário")
finally:
    ser.close()