Newer
Older
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
packetData[port_num].txpacket_[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_SYNC_READ;
packetData[port_num].txpacket_[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length);
for (_s = 0; _s < param_length; _s++)
packetData[port_num].txpacket_[PKT_PARAMETER0 + 4 + _s] = packetData[port_num].data_write_[_s];
TxPacket2(port_num);
if (packetData[port_num].communication_result_ == COMM_SUCCESS)
SetPacketTimeout(port_num, (UINT16_T)((11 + data_length) * param_length));
}
void SyncWriteTxOnly2(int port_num, UINT16_T start_address, UINT16_T data_length, UINT16_T param_length)
{
UINT8_T _s;
packetData[port_num].communication_result_ = COMM_TX_FAIL;
packetData[port_num].txpacket_ = (UINT8_T *)realloc(packetData[port_num].txpacket_, param_length + 14);
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
// 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_ID] = BROADCAST_ID;
packetData[port_num].txpacket_[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_SYNC_WRITE;
packetData[port_num].txpacket_[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length);
packetData[port_num].txpacket_[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length);
for (_s = 0; _s < param_length; _s++)
packetData[port_num].txpacket_[PKT_PARAMETER0 + 4 + _s] = packetData[port_num].data_write_[_s];
TxRxPacket2(port_num);
}
void BulkReadTx2(int port_num, UINT16_T param_length)
{
UINT8_T _s;
int _i;
packetData[port_num].communication_result_ = COMM_TX_FAIL;
packetData[port_num].txpacket_ = (UINT8_T *)realloc(packetData[port_num].txpacket_, param_length + 10);
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
// 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_ID] = BROADCAST_ID;
packetData[port_num].txpacket_[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_BULK_READ;
for (_s = 0; _s < param_length; _s++)
packetData[port_num].txpacket_[PKT_PARAMETER0 + _s] = packetData[port_num].data_write_[_s];
TxPacket2(port_num);
if (packetData[port_num].communication_result_ == COMM_SUCCESS)
{
int _wait_length = 0;
for (_i = 0; _i < param_length; _i += 5)
_wait_length += DXL_MAKEWORD(packetData[port_num].data_write_[_i + 3], packetData[port_num].data_write_[_i + 4]) + 10;
SetPacketTimeout(port_num, (UINT16_T)_wait_length);
}
}
void BulkWriteTxOnly2(int port_num, UINT16_T param_length)
{
UINT8_T _s;
packetData[port_num].communication_result_ = COMM_TX_FAIL;
packetData[port_num].txpacket_ = (UINT8_T *)realloc(packetData[port_num].txpacket_, param_length + 10);
// 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_ID] = BROADCAST_ID;
packetData[port_num].txpacket_[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_BULK_WRITE;
for (_s = 0; _s < param_length; _s++)
packetData[port_num].txpacket_[PKT_PARAMETER0 + _s] = packetData[port_num].data_write_[_s];
TxRxPacket2(port_num);
}