// get the speed for one servo - values betweeb -1023 <--> 1023
int HerkulexClass::getSpeed(int servoID) {
int speedy = 0;
pSize = 0x09; // 3.Packet size 7-58
pID = servoID; // 4. Servo ID
cmd = HRAMREAD; // 5. CMD
data[0]=0x40; // 8. Address
data[1]=0x02; // 9. Lenght
lenghtString=2; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address
dataEx[8] = data[1]; // Length
sendData(dataEx, pSize);
delay(1);
readData(13);
pSize = dataEx[2]; // 3.Packet size 7-58
pID = dataEx[3]; // 4. Servo ID
cmd = dataEx[4]; // 5. CMD
data[0]=dataEx[7];
data[1]=dataEx[8];
data[2]=dataEx[9];
data[3]=dataEx[10];
data[4]=dataEx[11];
data[5]=dataEx[12];
lenghtString=6;
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
if (ck1 != dataEx[5]) return -1;
if (ck2 != dataEx[6]) return -1;
speedy = ((dataEx[10]&0xFF)<<8) | dataEx[9];
return speedy;
}
// write registry in the EEP memory (ROM): one byte
void HerkulexClass::writeRegistryEEP(int servoID, int address, int writeByte)
{
pSize = 0x0A; // 3.Packet size 7-58
pID = servoID; // 4. Servo ID - 253=all servos
cmd = HEEPWRITE; // 5. CMD
data[0]=address; // 8. Address
data[1]=0x01; // 9. Lenght
data[2]=writeByte; // 10. Write error=0
lenghtString=3; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address 52
dataEx[8] = data[1]; // Length
dataEx[9] = data[2]; // Value1
dataEx[10]= data[3]; // Value2
sendData(dataEx, pSize);
}
// move all servo with the same execution time
void HerkulexClass::actionAll(int pTime)
{
if ((pTime <0) || (pTime > 2856)) return;
pSize = 0x08 + conta; // 3.Packet size 7-58
cmd = HSJOG; // 5. CMD SJOG Write n servo with same execution time
playTime=int((float)pTime/11.2);// 8. Execution time
pID=0xFE^playTime;
ck1=checksum1(moveData,conta); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
pID=0xFE;
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = playTime; // Execution time
for (int i=0; i < conta; i++)
dataEx[i+8]=moveData[i]; // Variable servo data
sendData(dataEx, pSize);
conta=0; //reset counter
}
// LED - see table of colors
void HerkulexClass::setLed(int servoID, int valueLed)
{
pSize = 0x0A; // 3.Packet size 7-58
pID = servoID; // 4. Servo ID
cmd = HRAMWRITE; // 5. CMD
data[0] = 0x35; // 8. Address 53
data[1] = 0x01; // 9. Lenght
data[2] = valueLed; // 10.LedValue
lenghtString=3; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address
dataEx[8] = data[1]; // Length
dataEx[9] = data[2]; // Value
sendData(dataEx, pSize);
}
// setID - Need to restart the servo
void HerkulexClass::set_ID(int ID_Old, int ID_New)
{
pSize = 0x0A; // 3.Packet size 7-58
pID = ID_Old; // 4. Servo ID OLD - original servo ID
cmd = HEEPWRITE; // 5. CMD
data[0]=0x06; // 8. Address
data[1]=0x01; // 9. Lenght
data[2]=ID_New; // 10. ServoID NEW
lenghtString=3; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address 52
dataEx[8] = data[1]; // Length
dataEx[9] = data[2]; // Value
sendData(dataEx, pSize);
}
// clearError
void HerkulexClass::clearError(int servoID)
{
pSize = 0x0B; // 3.Packet size 7-58
pID = servoID; // 4. Servo ID - 253=all servos
cmd = HRAMWRITE; // 5. CMD
data[0]=0x30; // 8. Address
data[1]=0x02; // 9. Lenght
data[2]=0x00; // 10. Write error=0
data[3]=0x00; // 10. Write detail error=0
lenghtString=4; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address 52
dataEx[8] = data[1]; // Length
dataEx[9] = data[2]; // Value1
dataEx[10]= data[3]; // Value2
sendData(dataEx, pSize);
}
// ACK - 0=No Replay, 1=Only reply to READ CMD, 2=Always reply
void HerkulexClass::ACK(int valueACK)
{
pSize = 0x0A; // 3.Packet size 7-58
pID = 0xFE; // 4. Servo ID
cmd = HRAMWRITE; // 5. CMD
data[0]=0x34; // 8. Address
data[1]=0x01; // 9. Lenght
data[2]=valueACK; // 10.Value. 0=No Replay, 1=Only reply to READ CMD, 2=Always reply
lenghtString=3; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address 52
dataEx[8] = data[1]; // Length
dataEx[9] = data[2]; // Value
sendData(dataEx, pSize);
}
//Set Overload PWM Threshold
void HerkulexClass::setOverloadPWMThreshold(int servoID)
{
pSize = 0x0B; // 3.Packet size
pID = servoID; // 4. Servo ID
cmd = HRAMWRITE; // 5. CMD
data[0]=0x12; // 8. Address
data[1]=0x02; // 9. Lenght
data[2]=0x7F; // 10. MSB
data[3]=0xFE; // 11. LSB
lenghtString=4; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0];// Address
dataEx[8] = data[1];// Length
dataEx[9] = data[2];// Data
dataEx[10] = data[3];// Data
sendData(dataEx, pSize);
}
// model - 1=0101 - 2=0201
byte HerkulexClass::model()
{
pSize = 0x09; // 3.Packet size 7-58
pID = 0xFE; // 4. Servo ID
cmd = HEEPREAD; // 5. CMD
data[0]=0x00; // 8. Address
data[1]=0x01; // 9. Lenght
lenghtString=2; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = data[0]; // Address
dataEx[8] = data[1]; // Length
sendData(dataEx, pSize);
delay(1);
readData(9);
pSize = dataEx[2]; // 3.Packet size 7-58
pID = dataEx[3]; // 4. Servo ID
cmd = dataEx[4]; // 5. CMD
data[0]=dataEx[7]; // 8. 1st byte
lenghtString=1; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
if (ck1 != dataEx[5]) return -1; //checksum verify
if (ck2 != dataEx[6]) return -2;
return dataEx[7]; // return status
}
bool IoPacket::raw2packet(char* new_buffer) {
char new_cksum1, new_cksum2;
if (new_buffer == NULL) {
if (this->buffer == NULL) {
status=1;
return false;
}
} else {
this->buffer=new_buffer;
}
size = (unsigned char)buffer[2];
pid = buffer[3];
cmd = buffer[4];
cksum1 = buffer[5];
new_cksum1=checksum1(buffer,size);
cksum2 = buffer[6];
new_cksum2=checksum2(new_cksum1);
if ((new_cksum1!=cksum1) || (new_cksum2!=cksum2)) {
status=0;
return false;
}
status=1;
if (size> 7) {
if (this->cmd==ACK_STAT) {
status_error=buffer[7];
status_detail=buffer[8];
} else {
data_addr=buffer[7];
data_length=buffer[8];
}
}
if (size>9) {
memcpy(data,buffer+9,data_length);
status_error=buffer[size-2];
status_detail=buffer[size-1];
status=2;
} else {
status_error=0;
status_detail=0;
status=0;
}
return true;
}
char* IoPacket::serialize() {
//header
if (size == 0) {
return NULL;
}
if (buffer != NULL) {
free(buffer);
}
// printf("Hello Serialize!\n");
buffer=(char*)malloc(size*sizeof(char));
buffer[0] = HEADER;
buffer[1] = HEADER;
buffer[2] = size;
buffer[3] = pid;
buffer[4] = cmd;
//data
if (size>7) {
buffer[7] = data_addr;
buffer[8] = data_length;
}
unsigned char i;
if (size>9) {
for (i=0;i<(data_length);i++) {
buffer[9+i] = data[i];
}
}
if (size-data_length>9) {
buffer[size-2]=status_error;
buffer[size-1]=status_detail;
}
//checksums
buffer[5] = checksum1(buffer, size);
buffer[6] = checksum2(buffer[5]);
this->cksum1=buffer[5];
this->cksum2=buffer[6];
return buffer;
}
std::vector<uint8_t> HerkuleX::build_packet(uint8_t servo_id,
Command::e command, std::vector<uint8_t> optional_data) {
uint8_t pkt_size = BASIC_PKT_SIZE + optional_data.size();
std::vector<uint8_t> buffer(pkt_size, 0);
buffer[0] = 0xFF; // Packet Header
buffer[1] = 0xFF; // Packet Header
buffer[2] = pkt_size; // Packet Size
buffer[3] = servo_id; // Servo ID
buffer[4] = command; // Command
std::vector<uint8_t>::iterator insert = buffer.begin();
std::advance(insert, BASIC_PKT_SIZE);
buffer.insert(insert, optional_data.begin(), optional_data.end());
buffer[5] = checksum1(buffer);
buffer[6] = checksum2(buffer[5]);
return buffer;
}
bool HerkuleX::is_right_packet(std::vector<uint8_t> buffer) {
if (buffer.size() < 7) {
return false;
}
if (buffer.size() != buffer[2]) {
//ROS_WARN("Invalid packet length! %s", buffer);
return false;
}
if (checksum1(buffer) != buffer[5]) {
//ROS_WARN("Invalid packet checksum1! %s", buffer);
return false;
}
if (checksum2(buffer[5]) != buffer[6]) {
//ROS_WARN("Invalid packet checksum2! %s", buffer);
return false;
}
return true;
}
// move one servo at goal position 0 - 1024
void HerkulexClass::moveOne(int servoID, int Goal, int pTime, int iLed)
{
if (Goal > 1023 || Goal < 0) return; // speed (goal) non correct
if ((pTime <0) || (pTime > 2856)) return;
// Position definition
int posLSB=Goal & 0X00FF; // MSB Pos
int posMSB=(Goal & 0XFF00) >> 8; // LSB Pos
//led
int iBlue=0;
int iGreen=0;
int iRed=0;
switch (iLed) {
case 1:
iGreen=1;
break;
case 2:
iBlue=1;
break;
case 3:
iRed=1;
break;
}
int SetValue=iGreen*4+iBlue*8+iRed*16; //assign led value
playTime=int((float)pTime/11.2); // 8. Execution time
pSize = 0x0C; // 3.Packet size 7-58
cmd = HSJOG; // 5. CMD
data[0]=posLSB; // 8. speedLSB
data[1]=posMSB; // 9. speedMSB
data[2]=SetValue; // 10. Mode=0;
data[3]=servoID; // 11. ServoID
pID=servoID^playTime;
lenghtString=4; // lenghtData
ck1=checksum1(data,lenghtString); //6. Checksum1
ck2=checksum2(ck1); //7. Checksum2
pID=servoID;
dataEx[0] = 0xFF; // Packet Header
dataEx[1] = 0xFF; // Packet Header
dataEx[2] = pSize; // Packet Size
dataEx[3] = pID; // Servo ID
dataEx[4] = cmd; // Command Ram Write
dataEx[5] = ck1; // Checksum 1
dataEx[6] = ck2; // Checksum 2
dataEx[7] = playTime; // Execution time
dataEx[8] = data[0];
dataEx[9] = data[1];
dataEx[10] = data[2];
dataEx[11] = data[3];
sendData(dataEx, pSize);
}
