int Motor::run(std::string id,int speed)
{
//FF FF (ID) 04 03 20 (0000000000000000B) SM
// | Speed |D|
//speed from -1023 to 1023
if (abs(speed) > 1023)
throw std::runtime_error(std::string("Motor::run speed value larger than 1023!\n"));
int rid = find_motor_byid(id)->id;
char *buf=(char *)malloc(12*sizeof(char));
buf=(char *)memset(buf,0,12*sizeof(char));
buf=(char *)memset(buf,MOTOR_HEAD,2*sizeof(char));
rid &= 0xff;
char *motorID = (char *)&rid;
*(buf+2) = *motorID;
*(buf+3) = 0x07;
*(buf+4) = 0x03;
*(buf+5) = 0x1E;
*(buf+6) = 0xfe;
*(buf+7) = 0xf3;
*(buf+8) = speed & 255;
*(buf+9) = (speed > 0) ? (1024 + (speed & 512)) >>9 : (abs(speed) & 512) >> 9;
finish_checksum(buf,11);
this->put_in(buf,11);
free(buf);
find_motor_byid(id)->running = true;
return 0;
}
int transmit::put_in(char *content,int leng)
{
zigWBuffer *tmp = new zigWBuffer;
// FF FF Content Checksum
tmp->buffer = new char[leng+3];
memset(tmp->buffer,0xff,2);
memcpy(tmp->buffer+2,content,leng);
finish_checksum(tmp->buffer,leng);
tmp -> length = leng;
wbuffer.push_back(tmp);
return 0;
}
void Motor::set_Motor_mode(std::string id)
{
//FF FF (ID) 07 03 06 00 00 00 00 SM
int rid = find_motor_byid(id)->id;
char *buf=(char *)malloc(12*sizeof(char));
buf=(char *)memset(buf,0,12*sizeof(char));
buf=(char *)memset(buf,MOTOR_HEAD,2*sizeof(char));
rid &= 0xff;
char *motorID = (char *)&rid;
*(buf+2) = *motorID;
*(buf+3) = 0x07;
*(buf+4) = 0x03;
*(buf+5) = 0x06;
*(buf+6) = 0x00;
*(buf+7) = 0x00;
*(buf+8) = 0x00;
*(buf+9) = 0x00;
finish_checksum(buf,11);
this->put_in(buf,11);
free(buf);
}
int Motor::stop(std::string id)
{
//FF FF (ID) 04 03 20 (0000000000000000B) SM
// | Speed |D|
int rid = find_motor_byid(id)->id;
rid &= 0xff;
char *motorID = (char *)&rid;
char *buf=(char *)malloc(10*sizeof(char));
buf=(char *)memset(buf,0,10*sizeof(char));
buf=(char *)memset(buf,MOTOR_HEAD,2*sizeof(char));
*(buf+2) = *motorID;
*(buf+3) = 0x05;
*(buf+4) = 0x03;
*(buf+5) = 0x20;
*(buf+6) = 0;
*(buf+7) = 0;
finish_checksum(buf,9);
this->put_in(buf,9);
free(buf);
find_motor_byid(id)->running = false;
return 0;
}
DLLEXPORT uint16_t *trace_checksum_transport(libtrace_packet_t *packet,
uint16_t *csum) {
void *header = NULL;
uint16_t ethertype;
uint32_t remaining;
uint32_t sum = 0;
uint8_t proto = 0;
uint16_t *csum_ptr = NULL;
int plen = 0;
uint8_t safety[65536];
uint8_t *ptr = safety;
header = trace_get_layer3(packet, ðertype, &remaining);
if (header == NULL)
return NULL;
if (ethertype == TRACE_ETHERTYPE_IP) {
libtrace_ip_t *ip = (libtrace_ip_t *)header;
if (remaining < sizeof(libtrace_ip_t))
return NULL;
sum = ipv4_pseudo_checksum(ip);
} else if (ethertype == TRACE_ETHERTYPE_IPV6) {
libtrace_ip6_t *ip = (libtrace_ip6_t *)header;
if (remaining < sizeof(libtrace_ip6_t))
return 0;
sum = ipv6_pseudo_checksum(ip);
}
header = trace_get_transport(packet, &proto, &remaining);
if (proto == TRACE_IPPROTO_TCP) {
libtrace_tcp_t *tcp = (libtrace_tcp_t *)header;
header = trace_get_payload_from_tcp(tcp, &remaining);
csum_ptr = &tcp->check;
memcpy(ptr, tcp, tcp->doff * 4);
tcp = (libtrace_tcp_t *)ptr;
tcp->check = 0;
ptr += (tcp->doff * 4);
}
else if (proto == TRACE_IPPROTO_UDP) {
libtrace_udp_t *udp = (libtrace_udp_t *)header;
header = trace_get_payload_from_udp(udp, &remaining);
csum_ptr = &udp->check;
memcpy(ptr, udp, sizeof(libtrace_udp_t));
udp = (libtrace_udp_t *)ptr;
udp->check = 0;
ptr += sizeof(libtrace_udp_t);
}
else if (proto == TRACE_IPPROTO_ICMP) {
/* ICMP doesn't use the pseudo header */
sum = 0;
libtrace_icmp_t *icmp = (libtrace_icmp_t *)header;
header = trace_get_payload_from_icmp(icmp, &remaining);
csum_ptr = &icmp->checksum;
memcpy(ptr, icmp, sizeof(libtrace_icmp_t));
icmp = (libtrace_icmp_t *)ptr;
icmp->checksum = 0;
ptr += sizeof(libtrace_icmp_t);
}
else {
return NULL;
}
sum += add_checksum(safety, (uint16_t)(ptr - safety));
plen = trace_get_payload_length(packet);
if (plen < 0)
return NULL;
if (remaining < (uint32_t)plen)
return NULL;
if (header == NULL)
return NULL;
sum += add_checksum(header, (uint16_t)plen);
*csum = ntohs(finish_checksum(sum));
//assert(0);
return csum_ptr;
}
