void setBaudRate(int fd, uint8_t nodeId, uint32_t baudRate)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x17;
p.byteCount = 0x04;
u32ToStr(baudRate, p.data);
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void setDGain(int fd, uint8_t nodeId, uint16_t D)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x02;
p.byteCount = 0x02;
u16ToStr(D, p.data);
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void setProfiledVelocitySetpoint(int fd, uint8_t nodeId, int32_t vel)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x10;
p.byteCount = 0x04;
s32ToStr(vel, p.data);
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void setNodeId(int fd, uint8_t nodeId, uint8_t newNodeId)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x15;
p.byteCount = 0x01;
p.data[0] = newNodeId;
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void setProfiledRelativePositionSetpoint(int fd, uint8_t nodeId, int64_t pos)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x12;
p.byteCount = 0x08;
s64ToStr(pos, p.data);
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void profiledMoveToAbsolutePosition(int fd, uint8_t nodeId, int64_t pos)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x0B;
p.byteCount = 0x08;
s64ToStr(pos, p.data);
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void zoSms::setProfiledRelativePositionSetpoint(uint8_t nodeId, int64_t pos)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x12;
p.byteCount = 8;
s64ToStr(pos,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void zoSms::setProfiledVelocitySetpoint(uint8_t nodeId, int32_t vel)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x10;
p.byteCount = 4;
s32ToStr(vel,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void zoSms::profiledMoveToAbsolutePosition(uint8_t nodeId, int64_t pos)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x0B;
p.byteCount = 8;
s64ToStr(pos,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void zoSms::setDurationForCurrentLimit(uint8_t nodeId, uint16_t dur)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x06;
p.byteCount = 2;
u16ToStr(dur,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void zoSms::setPIDgainD(uint8_t nodeId, uint16_t curr)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x02;
p.byteCount = 2;
u16ToStr(curr,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void zoSms::setProfileAcceleration(uint8_t nodeId, uint32_t accel)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x03;
p.byteCount = 4;
u32ToStr(accel,p.data);
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void setErrorReaction(int fd, uint8_t nodeId, uint8_t resp[])
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x1C;
p.byteCount = 0x14;
for (int i=0; i<20; i++)
{
p.data[i] = resp[i];
}
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void zoSms::setNodeID(uint8_t oldNodeId, uint8_t newNodeId)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = oldNodeId;
p.ownNodeID = 1;
p.commandID = 0x15;
p.byteCount = 1;
p.data[0]=newNodeId;
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
void setDigitalOutputs(int fd, uint8_t nodeId, bool do1, bool do2, bool do3)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 0x01;
p.commandID = 0x14;
p.byteCount = 0x01;
p.data[0] = 0;
if(do1)
p.data[0] |= 0x01;
if(do2)
p.data[0] |= 0x02;
if(do3)
p.data[0] |= 0x04;
p.lrc = calcLRC(&p);
if( putPacketSerial(fd, &p) )
getResponse(fd, &p);
}
void zoSms::configureDigitalIOs(uint8_t nodeId,bool dio1,bool dio2,bool dio3)
{
ZO_PROTOCOL_PACKET p;
p.addressedNodeID = nodeId;
p.ownNodeID = 1;
p.commandID = 0x13;
p.byteCount = 1;
p.data[0]=0;
if(dio1)
p.data[0] |= 0x01;
if(dio2)
p.data[0] |= 0x02;
if(dio3)
p.data[0] |= 0x04;
p.lrc = calcLRC(&p);
if( ha.putPacket(&p) )
getResponse(&p);
}
