// 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;
}
// 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);
}
//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);
}
// 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
}
// 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);
}
// 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);
}
// 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);
}
// 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);
}
// 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;
}
// stat
byte HerkulexClass::stat(int servoID)
{
{
pSize = 0x07; //3.Packet size
pID = servoID; //4.Servo ID - 0XFE=All servos
cmd = HSTAT; //5.CMD
ck1=(pSize^pID^cmd)&0xFE;
ck2=(~(pSize^pID^cmd))&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
sendData(dataEx, pSize);
delay(2);
readData(9); // read 9 bytes from serial
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];
lenghtString=2;
ck1 = (dataEx[2]^dataEx[3]^dataEx[4]^dataEx[7]^dataEx[8]) & 0xFE;
ck2=checksum2(ck1);
if (ck1 != dataEx[5]) return -1; //checksum verify
if (ck2 != dataEx[6]) return -2;
return dataEx[7]; // return status
}
}
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;
}
int check_ip_checksum(struct iphdr *iphdr, u_char *option, int option_len) {
struct iphdr iptmp;
unsigned short sum;
memcpy(&iptmp, iphdr, sizeof(struct iphdr));
if (option_len == 0) {
sum = checksum((u_char *)&iptmp, sizeof(struct iphdr));
if (sum == 0 || sum == 0xffff) {
return 1;
}
else {
return 0;
}
}
else {
sum = checksum2((u_char *)&iptmp, sizeof(struct iphdr), option, option_len);
if (sum == 0 || sum == 0xffff) {
return 1;
}
else {
return 0;
}
}
}
// Main
int main(int argc, char **argv)
{
// ----------- Variable
char recv_packet[BUFF_SIZE+40];
struct iphdr * ip_header = (struct iphdr *) recv_packet;
struct tcphdr * tcp_header = 0;
// ----------- Start
printf("\nStart Deflector\n\n");
// ----------- Raw Socket
printf("Create Raw Socket(Receive)\n");
int recv_socket = socketRaw(IPPROTO_TCP, DEFLECTOR_PORT);
// ----------- UDP
printf("Create UDP Socket(Send)\n");
int send_socket = socket( PF_INET, SOCK_DGRAM, 0 );
struct sockaddr_in bypass_addr;
memset( &bypass_addr, 0, sizeof(bypass_addr) );
bypass_addr.sin_family = AF_INET;
bypass_addr.sin_port = htons(BYPASS_PORT);
bypass_addr.sin_addr.s_addr = inet_addr(BYPASS_ADDR);
// ----------- Main Loop
printf("Enter Main Loop\n");
while(1)
{
recvRaw( recv_socket, recv_packet, sizeof(recv_packet) );
tcp_header = (struct tcphdr *) (recv_packet + ip_header->ihl * 4);
if ( ntohs( tcp_header->dest ) == DEFLECTOR_PORT )
{
tcp_header = (struct tcphdr *) (recv_packet + ip_header->ihl * 4);
tcp_header->check = 0;
tcp_header->check = tcp_checksum(ip_header, tcp_header);
ip_header->check = 0;
ip_header->check = checksum2((unsigned short*)ip_header, sizeof(struct iphdr));
// ----------- Receive Packet
printf("\nReceive Packet\n");
#ifdef DEBUG_DEFLECTOR
printf("----------------------------------------\n");
printf( "[ 패킷을 받았습니다. ]\n" );
printf( "발신자 IP : %s\n", inet_ntoa( * (struct in_addr*) &ip_header->saddr) );
printf( "발신자 Port : %5u\n", ntohs(tcp_header->source) );
printf( "수신자 IP : %s\n", inet_ntoa( * (struct in_addr*) &ip_header->daddr) );
printf( "수신자 Port : %5u\n", ntohs(tcp_header->dest) );
printf( "IP packet size : %d\n", ntohs(ip_header->tot_len));
printf( "IP check : %d\n", ip_header->check );
printf( "IP Version : %d\n", ip_header->version ); // IP 버전 정보 출력
printf( "Time To Live : %d\n", ip_header->ttl ); // TTL 정보 출력
printf( "TCP check : %d\n", tcp_header->check );
printf( "Window Size : %d\n", tcp_header->window ); // 윈도우 사이즈 정보 출력
printf( "Flags : " ); // 플래그 정보 출력
if( tcp_header->fin == 1 ) printf( "[FIN]\n" );
if( tcp_header->syn == 1 ) printf( "[SYN]\n" );
if( tcp_header->rst == 1 ) printf( "[RST]\n" );
if( tcp_header->psh == 1 ) printf( "[PSH]\n" );
if( tcp_header->ack == 1 ) printf( "[ACK]\n" );
if( tcp_header->urg == 1 ) printf( "[URG]\n" );
printf("-----------------------------------------\n");
#endif
// ----------- Send UDP Packet
printf("Send UDP Packet\n");
sendto( send_socket, (void *) &recv_packet, sizeof(recv_packet)+1, 0, (struct sockaddr *) &bypass_addr, sizeof(bypass_addr) );
}
}
}
static int analyze_packet(int device_no, u_char *data, int size) {
u_char *ptr;
int lest;
struct ether_header *eh;
char buf[80];
int tno;
u_char hwaddr[6];
ptr = data;
lest = size;
if (lest < sizeof(struct ether_header)) {
/* Packet size must be above or equal to ethernet header's. */
debug_printf("[%d]:lest(%d)<sizeof(struct ether_header)\n", device_no, lest);
return -1;
}
eh = (struct ether_header *)ptr;
ptr += sizeof(struct ether_header);
lest -= sizeof(struct ether_header);
if (memcmp(&eh->ether_dhost, device[device_no].hwaddr, 6) != 0) {
//debug_printf("[%d]:dhost not match %s\n", device_no, ether_ntoa((u_char*)&eh->ether_dhost, buf, sizeof(buf)));
return -1;
}
//debug_printf("[%d]:dhost match %s\n", device_no, ether_ntoa((u_char*)&eh->ether_dhost, buf, sizeof(buf)));
#ifdef CONFIG_DEBUG
print_ether_header(eh, stderr);
#endif
if (ntohs(eh->ether_type) == ETHERTYPE_ARP) {
/* Recieved ARP type packet */
struct ether_arp *arp;
if (lest < sizeof(struct ether_arp)) {
/* Packet size must be above or equal to ethernet header's. */
debug_printf("[%d]:lest(%d)<sizeof(struct ether_header)\n", device_no, lest);
return -1;
}
arp = (struct ether_arp *)ptr;
ptr += sizeof(struct ether_arp);
lest -= sizeof(struct ether_arp);
if (arp->arp_op == htons(ARPOP_REQUEST)) {
debug_printf("[%d]recv:ARP REQUEST:%dbytes\n",device_no, size);
ip_2_mac(device_no, *(in_addr_t *)arp->arp_spa, arp->arp_sha);
}
if (arp->arp_op == htons(ARPOP_REPLY)) {
debug_printf("[%d]recv:ARP REPLY:%dbytes\n",device_no, size);
ip_2_mac(device_no, *(in_addr_t *)arp->arp_spa, arp->arp_sha);
}
}
else if (ntohs(eh->ether_type) == ETHERTYPE_IP) {
/* Recieved IP packet */
struct iphdr *iphdr;
u_char option[1500];
int option_len;
if (lest < sizeof(struct iphdr)) {
/* Packet size must be above or equal to ethernet header's. */
debug_printf("[%d]:lest(%d)<sizeof(struct iphdr)\n", device_no, lest);
return -1;
}
iphdr = (struct iphdr *)ptr;
ptr += sizeof(struct iphdr);
lest -= sizeof(struct iphdr);
option_len = iphdr->ihl * 4 - sizeof(struct iphdr);
if (option_len > 0) {
if (option_len >= 1500) {
debug_printf("[%d]: IP option_len(%d): too big\n", device_no, option_len);
return -1;
}
memcpy(option, ptr, option_len);
ptr += option_len;
lest -= option_len;
}
if (check_ip_checksum(iphdr, option, option_len) == 0) {
debug_printf("[%d]: bad IP checksum\n",device_no);
return -1;
}
if (iphdr->ttl - 1 == 0) {
/* TTL has expired. Router must notice edge user it by ICMP. */
debug_printf("[%d]: iphdr->ttl == 0 error\n",device_no);
send_icmp_time_exceeded(device_no, eh, iphdr, data, size);
return -1;
}
tno = get_opposite_dev(device_no);
debug_printf("[%d] %s -> ",device_no,in_addr_t2str(iphdr->saddr, buf, sizeof(buf)));
debug_printf("%s\n",in_addr_t2str(iphdr->daddr, buf, sizeof(buf)));
// debug_printf("[%d] %s\n", device_no, in_addr_t2str(device[tno].netmask.s_addr, buf, sizeof(buf)));
// debug_printf("[%d] subnet:%s\n", device_no, in_addr_t2str(device[tno].subnet.s_addr, buf, sizeof(buf)));
if ((iphdr->daddr & device[tno].netmask.s_addr) == device[tno].subnet.s_addr) {
/* Same subnet network */
IP2MAC *ip2mac;
debug_printf("[%d]:%s to target segment\n", device_no, in_addr_t2str(iphdr->daddr, buf, sizeof(buf)));
debug_printf("[%d]:%s\n",device_no, in_addr_t2str(device[device_no].addr.s_addr, buf, sizeof(buf)));
if (iphdr->daddr == device[tno].addr.s_addr) {
debug_printf("[%d]:recv:myaddr\n",device_no);
return -1;
}
ip2mac = ip_2_mac(tno, iphdr->daddr, NULL);
if (ip2mac->flag == FLAG_NG || ip2mac->sd.dno != 0) {
debug_printf("[%d]Ip2Mac: error or sending\n", device_no);
append_send_data(ip2mac, 1, iphdr->daddr, data, size);
return -1;
}
else {
memcpy(hwaddr, ip2mac->hwaddr, 6);
}
}
else {
/* Differenct subnet network */
IP2MAC *ip2mac;
debug_printf("[%d]:%s to different segment\n", device_no, in_addr_t2str(iphdr->daddr, buf, sizeof(buf)));
ip2mac = ip_2_mac(tno, next_router.s_addr, NULL);
if (ip2mac->flag == FLAG_NG || ip2mac->sd.dno != 0) {
debug_printf("[%d]:Ip2Mac: error or sending\n", device_no);
append_send_data(ip2mac, 1, next_router.s_addr, data, size);
return -1;
}
else {
memcpy(hwaddr, ip2mac->hwaddr, 6);
}
}
/* Finally we can send packet to next router. */
memcpy(eh->ether_dhost, hwaddr, 6);
memcpy(eh->ether_shost, device[tno].hwaddr, 6);
iphdr->ttl--;
iphdr->check = 0;
iphdr->check = checksum2((u_char *)iphdr, sizeof(struct iphdr), option, option_len);
write(device[tno].soc, data, size);
}
return 0;
}
// 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);
}
