333 lines
7.6 KiB
C++
333 lines
7.6 KiB
C++
#include "ServoCommand.h"
|
|
#include "stdio.h"
|
|
#include <string.h>
|
|
#include <bitset>
|
|
#include "CRC16.h"
|
|
|
|
MitsubishiCommand::MitsubishiCommand(unsigned char addr)
|
|
:m_addr(addr)
|
|
{
|
|
}
|
|
|
|
|
|
MitsubishiCommand::~MitsubishiCommand()
|
|
{
|
|
}
|
|
|
|
|
|
int MitsubishiCommand::GetRequestSequence( unsigned char* buffer)
|
|
{
|
|
buffer[0] = 0x01;
|
|
buffer[1] = m_addr;
|
|
buffer[2] = 0x30;
|
|
buffer[3] = 0x32;
|
|
buffer[4] = 0x02;
|
|
buffer[5] = 0x39;
|
|
buffer[6] = 0x31;
|
|
buffer[7] = 0x03;
|
|
unsigned short sum = 0;
|
|
for (int i = 1; i < 8;++i) {
|
|
sum += buffer[i];
|
|
}
|
|
sum = sum % 0x100;
|
|
char sbuffer[10];
|
|
sprintf_s(sbuffer, sizeof(sbuffer), "%02X", sum);
|
|
buffer[8] = sbuffer[0];
|
|
buffer[9] = sbuffer[1];
|
|
return 10;
|
|
}
|
|
|
|
bool MitsubishiCommand::Verify(unsigned char* buffer, int len)
|
|
{
|
|
if (len != 14)return false;
|
|
if (buffer[0] != 0x02)return false;
|
|
if (buffer[1] != m_addr )return false;
|
|
if (buffer[2] != 'A')return false;
|
|
if (buffer[len - 3] != 0x03)return false;
|
|
unsigned short sum = 0;
|
|
for (int i = 1; i < len-2; ++i) {
|
|
sum += buffer[i];
|
|
}
|
|
sum = sum % 0x100;
|
|
char sbuffer[10];
|
|
sprintf_s(sbuffer, sizeof(sbuffer), "%02X", sum);
|
|
if (buffer[len - 2] != sbuffer[0]) return false;
|
|
if (buffer[len - 1] != sbuffer[1])return false;
|
|
|
|
|
|
unsigned char tempb[8];
|
|
for (int i = 0; i < 8;++i) {
|
|
if (buffer[3 + i]>='0' && buffer[3 + i]<='9') {
|
|
tempb[i] = buffer[3 + i] - '0';
|
|
}
|
|
else if (buffer[3 + i] >= 'A' && buffer[3 + i] <= 'Z') {
|
|
tempb[i] = buffer[3 + i] - 'A'+10;
|
|
}
|
|
}
|
|
MitsubishiValue sv;
|
|
for (int i = 0; i < 4;i++) {
|
|
sv.bvalue[3 - i] = tempb[i * 2] * 16 + tempb[i * 2 + 1];
|
|
}
|
|
m_value = sv.ivalue;
|
|
return true;
|
|
}
|
|
|
|
SanyoCommand::SanyoCommand(unsigned char addr, std::string ctrl, std::string code, std::string group, std::string param)
|
|
:m_addr(addr)
|
|
,m_ctrl(ctrl)
|
|
,m_code(code)
|
|
,m_group(group)
|
|
,m_param(param)
|
|
{
|
|
|
|
}
|
|
|
|
SanyoCommand::~SanyoCommand() {
|
|
|
|
}
|
|
|
|
int SanyoCommand::ConstReturnSize()
|
|
{
|
|
return m_ResponseLength;
|
|
}
|
|
|
|
int SanyoCommand::GetRequestSequence(unsigned char* buffer)
|
|
{
|
|
buffer[0] = 0x02;
|
|
buffer[1] = m_addr;
|
|
buffer[2] = m_ctrl[0];
|
|
buffer[3] = m_ctrl[1];
|
|
buffer[4] = m_code[0];
|
|
buffer[5] = m_code[1];
|
|
buffer[6] = m_group[0];
|
|
buffer[7] = m_group[1];
|
|
buffer[8] = m_param[0];
|
|
buffer[9] = m_param[1];
|
|
buffer[10] = 0x03;
|
|
|
|
unsigned char rel = 0;
|
|
for (int i = 1; i < 11; ++i) {
|
|
rel = rel^buffer[i];
|
|
}
|
|
buffer[11] = rel;
|
|
return 12;
|
|
}
|
|
|
|
SanyoReadEncoderPosCommand::SanyoReadEncoderPosCommand(unsigned char addr, std::string ctrl, std::string code,
|
|
std::string group, std::string param)
|
|
:SanyoCommand(addr,ctrl,code,group,param)
|
|
{
|
|
m_ResponseLength = 24;
|
|
}
|
|
|
|
SanyoReadEncoderPosCommand::~SanyoReadEncoderPosCommand()
|
|
{
|
|
|
|
}
|
|
|
|
bool SanyoReadEncoderPosCommand::Verify(unsigned char* buffer, int len)
|
|
{
|
|
if (len != m_ResponseLength)return false;
|
|
if (buffer[0] != 0x02)return false;
|
|
if (buffer[1] != m_addr)return false;
|
|
if (buffer[2] != 0x38)return false;
|
|
if (buffer[3] != 0x30)return false;
|
|
if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
|
|
if (buffer[22] != 0x03)return false;
|
|
unsigned char rel = 0;
|
|
for (int i = 1; i < m_ResponseLength-1; ++i) {
|
|
rel = rel^buffer[i];
|
|
}
|
|
if (buffer[m_ResponseLength - 1] != rel)return false;
|
|
|
|
unsigned char tempa[16];
|
|
for (int i = 0; i < 16; ++i) {
|
|
if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
|
|
tempa[i] = buffer[6 + i] - '0';
|
|
}
|
|
else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
|
|
tempa[i] = buffer[6 + i] - 'A' + 10;
|
|
}
|
|
}
|
|
unsigned char tempb[8] = {
|
|
tempa[0] * 16 + tempa[1] ,
|
|
tempa[2] * 16 + tempa[3] ,
|
|
tempa[4] * 16 + tempa[5] ,
|
|
tempa[6] * 16 + tempa[7] ,
|
|
tempa[8] * 16 + tempa[9] ,
|
|
tempa[10] * 16 + tempa[11] ,
|
|
tempa[12] * 16 + tempa[13] ,
|
|
tempa[14] * 16 + tempa[15]
|
|
};
|
|
SanyoValue_Int64 value;
|
|
for (int i = 0; i < 8; i++) {
|
|
value.bvalue[7 - i] = tempb[i];
|
|
}
|
|
int64_t realValue = value.lvalue;
|
|
std::bitset<64> bitValut(realValue);
|
|
std::bitset<16> bitcyc;
|
|
std::bitset<17> bitsin;
|
|
for (int i = 0; i < 16;i++) {
|
|
bitcyc[i] = bitValut[17 + i];
|
|
}
|
|
for (int i = 0; i < 17; i++) {
|
|
bitsin[i] = bitValut[i];
|
|
}
|
|
m_cycles =(short) bitcyc.to_ulong();
|
|
m_singal = (int)bitsin.to_ulong();
|
|
m_value =(long) m_cycles * ABS_CYCLE + m_singal;
|
|
return true;
|
|
}
|
|
|
|
|
|
SanyoReadLoadTorqueCommand::SanyoReadLoadTorqueCommand(unsigned char addr, std::string ctrl, std::string code,
|
|
std::string group, std::string param)
|
|
:SanyoCommand(addr, ctrl, code, group, param)
|
|
{
|
|
m_ResponseLength = 12;
|
|
}
|
|
|
|
SanyoReadLoadTorqueCommand::~SanyoReadLoadTorqueCommand()
|
|
{
|
|
|
|
}
|
|
|
|
bool SanyoReadLoadTorqueCommand::Verify(unsigned char* buffer, int len)
|
|
{
|
|
if (len != m_ResponseLength)return false;
|
|
if (buffer[0] != 0x02)return false;
|
|
if (buffer[1] != m_addr)return false;
|
|
if (buffer[2] != 0x38)return false;
|
|
if (buffer[3] != 0x30)return false;
|
|
if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
|
|
if (buffer[10] != 0x03)return false;
|
|
unsigned char rel = 0;
|
|
for (int i = 1; i < m_ResponseLength-1; ++i) {
|
|
rel = rel^buffer[i];
|
|
}
|
|
if (buffer[m_ResponseLength-1] != rel)return false;
|
|
|
|
unsigned char tempa[4];
|
|
for (int i = 0; i < 4; ++i) {
|
|
if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
|
|
tempa[i] = buffer[6 + i] - '0';
|
|
}
|
|
else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
|
|
tempa[i] = buffer[6 + i] - 'A' + 10;
|
|
}
|
|
}
|
|
unsigned char tempb[2] = {
|
|
tempa[0] * 16 + tempa[1] ,
|
|
tempa[2] * 16 + tempa[3]
|
|
};
|
|
SanyoValue_Short value;
|
|
for (int i = 0; i < 2; i++) {
|
|
value.bvalue[1 - i] = tempb[i];
|
|
}
|
|
m_Value= value.svalue;
|
|
return true;
|
|
}
|
|
|
|
SanyoReadAlarmCommand::SanyoReadAlarmCommand(unsigned char addr, std::string ctrl, std::string code)
|
|
:SanyoCommand(addr, ctrl, code, " ", " ")
|
|
{
|
|
m_ResponseLength = 10;
|
|
}
|
|
|
|
SanyoReadAlarmCommand::~SanyoReadAlarmCommand()
|
|
{
|
|
|
|
}
|
|
|
|
bool SanyoReadAlarmCommand::Verify(unsigned char* buffer, int len)
|
|
{
|
|
if (len != m_ResponseLength)return false;
|
|
if (buffer[0] != 0x02)return false;
|
|
if (buffer[1] != m_addr)return false;
|
|
if (buffer[2] != 0x38)return false;
|
|
if (buffer[3] != 0x30)return false;
|
|
if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
|
|
if (buffer[8] != 0x03)return false;
|
|
unsigned char rel = 0;
|
|
for (int i = 1; i < m_ResponseLength - 1; ++i) {
|
|
rel = rel^buffer[i];
|
|
}
|
|
if (buffer[m_ResponseLength - 1] != rel)return false;
|
|
|
|
unsigned char tempa[2];
|
|
for (int i = 0; i < 2; ++i) {
|
|
if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
|
|
tempa[i] = buffer[6 + i] - '0';
|
|
}
|
|
else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
|
|
tempa[i] = buffer[6 + i] - 'A' + 10;
|
|
}
|
|
}
|
|
unsigned char tempb = tempa[0] * 16 + tempa[1];
|
|
m_Value = tempb;
|
|
return true;
|
|
}
|
|
|
|
int SanyoReadAlarmCommand::GetRequestSequence(unsigned char* buffer)
|
|
{
|
|
buffer[0] = 0x02;
|
|
buffer[1] = m_addr;
|
|
buffer[2] = m_ctrl[0];
|
|
buffer[3] = m_ctrl[1];
|
|
buffer[4] = m_code[0];
|
|
buffer[5] = m_code[1];
|
|
buffer[6] = 0x03;
|
|
unsigned char rel = 0;
|
|
for (int i = 1; i < 7; ++i) {
|
|
rel = rel^buffer[i];
|
|
}
|
|
buffer[7] = rel;
|
|
return 8;
|
|
}
|
|
|
|
SiemensCommand::SiemensCommand(unsigned char addr)
|
|
:m_addr(addr)
|
|
{
|
|
|
|
}
|
|
|
|
SiemensCommand::~SiemensCommand()
|
|
{
|
|
}
|
|
|
|
int SiemensCommand::GetRequestSequence(unsigned char* buffer)
|
|
{
|
|
buffer[0] = m_addr;
|
|
buffer[1] = 0x03;
|
|
buffer[2] = 0x01;
|
|
buffer[3] = 0x5F;
|
|
buffer[4] = 0x00;
|
|
buffer[5] = 0x02;
|
|
|
|
unsigned short crc = CRC16(buffer, 6);
|
|
buffer[6] = (crc & 0xFF00) >> 8;
|
|
buffer[7] = (crc & 0x00FF);
|
|
|
|
return 8;
|
|
}
|
|
|
|
bool SiemensCommand::Verify(unsigned char* buffer, int len)
|
|
{
|
|
if (len != 9) return false;
|
|
if (buffer[0] != m_addr) return false;
|
|
if (buffer[1] != 0x03) return false;
|
|
if (buffer[2] != 0x04) return false;
|
|
|
|
unsigned short crc = CRC16(buffer, 7);
|
|
if (((((unsigned short)buffer[7]) << 8) + buffer[8]) != crc) return false;
|
|
|
|
unsigned char tmp[4];
|
|
tmp[0] = buffer[6];
|
|
tmp[1] = buffer[5];
|
|
tmp[2] = buffer[4];
|
|
tmp[3] = buffer[3];
|
|
m_value = *(long *)tmp;
|
|
|
|
return true;
|
|
}
|