// 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); }