Protocol2PacketHandler.c

/*
* Protocol2PacketHandler.c
*
*  Created on: 2016. 5. 4.
*      Author: leon
*/

#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "dynamixel_sdk/Protocol2PacketHandler.h"

#define TXPACKET_MAX_LEN    (4*1024)
#define RXPACKET_MAX_LEN    (4*1024)

///////////////// for Protocol 2.0 Packet /////////////////
#define PKT_HEADER0             0
#define PKT_HEADER1             1
#define PKT_HEADER2             2
#define PKT_RESERVED            3
#define PKT_ID                  4
#define PKT_LENGTH_L            5
#define PKT_LENGTH_H            6
#define PKT_INSTRUCTION         7
#define PKT_ERROR               8
#define PKT_PARAMETER0          8

///////////////// Protocol 2.0 Error bit /////////////////
#define ERRNUM_RESULT_FAIL      1       // Failed to process the instruction packet.
#define ERRNUM_INSTRUCTION      2       // Instruction error
#define ERRNUM_CRC              3       // CRC check error
#define ERRNUM_DATA_RANGE       4       // Data range error
#define ERRNUM_DATA_LENGTH      5       // Data length error
#define ERRNUM_DATA_LIMIT       6       // Data limit error
#define ERRNUM_ACCESS           7       // Access error

#define ERRBIT_ALERT            128     //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.

void PrintTxRxResult2(int result)
{
    switch (result)
    {
    case COMM_SUCCESS:
        printf("[TxRxResult] Communication success.\n");
        break;

    case COMM_PORT_BUSY:
        printf("[TxRxResult] Port is in use!\n");
        break;

    case COMM_TX_FAIL:
        printf("[TxRxResult] Failed transmit instruction packet!\n");
        break;

    case COMM_RX_FAIL:
        printf("[TxRxResult] Failed get status packet from device!\n");
        break;

    case COMM_TX_ERROR:
        printf("[TxRxResult] Incorrect instruction packet!\n");
        break;

    case COMM_RX_WAITING:
        printf("[TxRxResult] Now recieving status packet!\n");
        break;

    case COMM_RX_TIMEOUT:
        printf("[TxRxResult] There is no status packet!\n");
        break;

    case COMM_RX_CORRUPT:
        printf("[TxRxResult] Incorrect status packet!\n");
        break;

    case COMM_NOT_AVAILABLE:
        printf("[TxRxResult] Protocol does not support This function!\n");
        break;

    default:
        break;
    }
}

void PrintRxPacketError2(UINT8_T error)
{
    if (error & ERRBIT_ALERT)
        printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n");

    int _error = error & ~ERRBIT_ALERT;

    switch (_error)
    {
    case 0:
        break;

    case ERRNUM_RESULT_FAIL:
        printf("[RxPacketError] Failed to process the instruction packet!\n");
        break;

    case ERRNUM_INSTRUCTION:
        printf("[RxPacketError] Undefined instruction or incorrect instruction!\n");
        break;

    case ERRNUM_CRC:
        printf("[RxPacketError] CRC doesn't match!\n");
        break;

    case ERRNUM_DATA_RANGE:
        printf("[RxPacketError] The data value is out of range!\n");
        break;

    case ERRNUM_DATA_LENGTH:
        printf("[RxPacketError] The data length does not match as expected!\n");
        break;

    case ERRNUM_DATA_LIMIT:
        printf("[RxPacketError] The data value exceeds the limit value!\n");
        break;

    case ERRNUM_ACCESS:
        printf("[RxPacketError] Writing or Reading is not available to target address!\n");
        break;

    default:
        printf("[RxPacketError] Unknown error code!\n");
        break;
    }
}

int GetLastTxRxResult2(int port_num)
{
    return packetData[port_num].communication_result_;
}
UINT8_T GetLastRxPacketError2(int port_num)
{
    return packetData[port_num].error_;
}

void SetDataWrite2(int port_num, UINT16_T data_length, UINT16_T data_pos, UINT32_T data)
{
    packetData[port_num].data_write_ = (UINT8_T *)realloc(packetData[port_num].data_write_, (data_pos + data_length) * sizeof(UINT8_T));

    switch (data_length)
    {
    case 1:
        packetData[port_num].data_write_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
        break;

    case 2:
        packetData[port_num].data_write_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
        packetData[port_num].data_write_[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
        break;

    case 4:
        packetData[port_num].data_write_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
        packetData[port_num].data_write_[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
        packetData[port_num].data_write_[data_pos + 2] = DXL_LOBYTE(DXL_HIWORD(data));
        packetData[port_num].data_write_[data_pos + 3] = DXL_HIBYTE(DXL_HIWORD(data));
        break;

    default:
        printf("[Set Data Write] failed... ");
        break;
    }
}
UINT32_T GetDataRead2(int port_num, UINT16_T data_length, UINT16_T data_pos)
{
    switch (data_length)
    {
    case 1:
        return packetData[port_num].data_read_[data_pos + 0];

    case 2:
        return DXL_MAKEWORD(packetData[port_num].data_read_[data_pos + 0], packetData[port_num].data_read_[data_pos + 1]);

    case 4:
        return DXL_MAKEDWORD(DXL_MAKEWORD(packetData[port_num].data_read_[data_pos + 0], packetData[port_num].data_read_[data_pos + 1])
            , DXL_MAKEWORD(packetData[port_num].data_read_[data_pos + 2], packetData[port_num].data_read_[data_pos + 3]));

    default:
        printf("[Set Data Read] failed... ");
        return 0;
    }
}

unsigned short UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size)
{
    UINT16_T i, j;
    UINT16_T crc_table[256] = { 0x0000,
        0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027,
        0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D,
        0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B,
        0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9,
        0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF,
        0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5,