void reset() {
rpos.base=0;
rpos.shoulder=0;
rpos.elbow=0;
rpos.pitch=0;
rpos.wrist=0;
rpos.grip=0;
rsend("NT");
rsend("SP9");
rsend("GP7,5,3");
}
void
RsocketsTransmitter::operator()() {
try {
// send the messages
cout << "Thread " << index << ": Sending messages..." << endl;
int flags{0};
int sent;
struct pollfd fds;
fds.events = POLLOUT;
fds.fd = sock;
while(true) {
int rc = rpoll(&fds, 1, -1);
assert(rc != 1);
MessageItem rawMsg;
parentPool.msgQueue.wait_dequeue(rawMsg);
if (rawMsg.first != nullptr && rawMsg.second != 0) {
sent = rsend(sock, rawMsg.first, rawMsg.second, flags);
assert(sent != -1);
} else {
break;
}
}
cout << "Finished." << endl;
} catch (const std::system_error &e) {
std::cerr << e.what() << std::endl;
return;
}
}
static int send_msg(int size)
{
struct pollfd fds;
int offset, ret;
if (verify)
format_buf(buf, size);
if (use_async) {
fds.fd = rs;
fds.events = POLLOUT;
}
for (offset = 0; offset < size; ) {
if (use_async) {
ret = do_poll(&fds, poll_timeout);
if (ret)
return ret;
}
ret = rsend(rs, buf + offset, size - offset, flags);
if (ret > 0) {
offset += ret;
} else if (errno != EWOULDBLOCK && errno != EAGAIN) {
perror("rsend");
return ret;
}
}
return 0;
}
void rmove(int joint,int step) {
char *buf=NULL;
char *buf2=NULL;
int i;
buf=(char *)calloc(1024,1);
buf2=(char *)calloc(1024,1);
strcat (buf,"MI");
for(i=1;i<=3;i++) {
if(joint==i) {
sprintf(buf2,"%d",step);
strcat(buf,buf2);
}
else strcat(buf,"0");
strcat(buf,",");
}
switch (joint) {
case 4: sprintf(buf2,"%d,%d",step,-step);break;
case 5: sprintf(buf2,"%d,%d",step,step);break;
default: sprintf(buf2,"0,0");break;
}
strcat(buf,buf2);
strcat(buf,",0");
rsend(buf);
free(buf);free(buf2);
}
int
fullsend( int sock, const char *buf, int buflen, int flags )
{
int sent = 0, lastsend = 0;
while (sent < buflen) {
errno = 0;
lastsend = rsend(sock, buf+sent*sizeof(char), buflen-sent, flags);
if (lastsend < 0) {
/* no need for timeout check, when remote side closes the
socket, an error will be returned */
if (errno==EAGAIN || errno==EINTR) {
continue;
} else {
return sent;
}
} else if (lastsend == 0) {
return sent;
} else {
sent += lastsend;
}
}
return sent;
}
void store_position(int posnum) {
char *buf=NULL;
buf=(char *)calloc(1024,1);
posdata[posnum]=rpos;
posdata[posnum].saved=1;
show_panel(2,itoa(posnum));
sprintf(buf,"HE%d",posnum);
rsend(buf);
free(buf);
}
void recall_position(int posnum) {
char *buf=NULL;
int t_base,
t_shoulder,
t_elbow,
t_pitch,
t_wrist;
buf=(char *)calloc(1024,1);
rsend( posdata[posnum].grip==0 ? "GO":"GC" );
t_base =posdata[posnum].base -rpos.base;
t_shoulder=posdata[posnum].shoulder-rpos.shoulder;
t_elbow =posdata[posnum].elbow -rpos.elbow;
t_pitch =posdata[posnum].pitch -rpos.pitch;
t_wrist =posdata[posnum].wrist -rpos.wrist;
rpos=posdata[posnum];
sprintf(buf,"MI%d,%d,%d,%d,%d,0",t_base,t_shoulder,t_elbow,t_wrist+t_pitch,t_wrist-t_pitch);
rsend(buf);
free(buf);
show_panel(8,itoa(posnum));
}
void run_program() {
int i,lastpos=0,total_positions=0,sent_positions=0;
char *buf=NULL;
buf=(char *)calloc(255,1);
rsend("NW");
for(i=1;i<=POS_SPACE;i++) { if(posdata[i].saved==1) total_positions++; }
for(i=1;i<=POS_SPACE;i++) {
if(posdata[i].saved==1) {
sprintf(buf,"PS %d,%d,%d,%d,%d,%d,0",i
,posdata[i].base
,posdata[i].shoulder
,posdata[i].elbow
,posdata[i].wrist+posdata[i].pitch
,posdata[i].wrist-posdata[i].pitch);
show_panel(3,itoa((sent_positions++*100)/total_positions));
if((i==1) || (posdata[i-1].grip != posdata[i].grip) ) {
if(posdata[i].grip) rsend("GF1");
else rsend("GF0");
}
rsend(buf);
}
}
sent_positions=0;
for(i=1;i<=POS_SPACE;i++) {
if(posdata[i].saved==1) {
lastpos = i > lastpos ? i : lastpos;
sprintf(buf,"%d MO %d",i,i);
show_panel(4,itoa((sent_positions++*100)/total_positions));
rsend(buf);
}
}
sprintf(buf,"%d ED",i+1);
rsend(buf);
rpos=posdata[lastpos];
rsend("RN 1");
free(buf);
show_panel(5,"");
}
void vm_process(VM *vm) {
int a, b, opcode;
opcode = vm->image[vm->ip];
switch(opcode) {
case VM_NOP:
break;
case VM_LIT:
vm->sp++;
vm->ip++;
TOS = vm->image[vm->ip];
break;
case VM_DUP:
vm->sp++;
vm->data[vm->sp] = NOS;
break;
case VM_DROP:
DROP
break;
case VM_SWAP:
a = TOS;
TOS = NOS;
NOS = a;
break;
case VM_PUSH:
vm->rsp++;
TORS = TOS;
DROP
break;
case VM_POP:
vm->sp++;
TOS = TORS;
vm->rsp--;
break;
case VM_CALL:
vm->ip++;
vm->rsp++;
TORS = vm->ip;
vm->ip = vm->image[vm->ip] - 1;
if (vm->ip < 0)
vm->ip = IMAGE_SIZE;
else {
if (vm->image[vm->ip+1] == 0)
vm->ip++;
if (vm->image[vm->ip+1] == 0)
vm->ip++;
}
break;
case VM_JUMP:
vm->ip++;
vm->ip = vm->image[vm->ip] - 1;
if (vm->ip < 0)
vm->ip = IMAGE_SIZE;
else {
if (vm->image[vm->ip+1] == 0)
vm->ip++;
if (vm->image[vm->ip+1] == 0)
vm->ip++;
}
break;
case VM_RETURN:
vm->ip = TORS;
vm->rsp--;
break;
case VM_GT_JUMP:
vm->ip++;
if(NOS > TOS)
vm->ip = vm->image[vm->ip] - 1;
DROP DROP
break;
case VM_LT_JUMP:
vm->ip++;
if(NOS < TOS)
vm->ip = vm->image[vm->ip] - 1;
DROP DROP
break;
case VM_NE_JUMP:
vm->ip++;
if(TOS != NOS)
vm->ip = vm->image[vm->ip] - 1;
DROP DROP
break;
case VM_EQ_JUMP:
vm->ip++;
if(TOS == NOS)
vm->ip = vm->image[vm->ip] - 1;
DROP DROP
break;
case VM_FETCH:
TOS = vm->image[TOS];
break;
case VM_STORE:
vm->image[TOS] = NOS;
DROP DROP
break;
case VM_ADD:
NOS += TOS;
DROP
break;
case VM_SUB:
NOS -= TOS;
DROP
break;
case VM_MUL:
NOS *= TOS;
DROP
break;
case VM_DIVMOD:
a = TOS;
b = NOS;
TOS = b / a;
NOS = b % a;
break;
case VM_AND:
a = TOS;
b = NOS;
DROP
TOS = a & b;
break;
case VM_OR:
a = TOS;
b = NOS;
DROP
TOS = a | b;
break;
case VM_XOR:
a = TOS;
b = NOS;
DROP
TOS = a ^ b;
break;
case VM_SHL:
a = TOS;
b = NOS;
DROP
TOS = b << a;
break;
case VM_SHR:
a = TOS;
b = NOS;
DROP
TOS = b >>= a;
break;
case VM_ZERO_EXIT:
if (TOS == 0) {
DROP
vm->ip = TORS;
vm->rsp--;
}
break;
case VM_INC:
TOS += 1;
break;
case VM_DEC:
TOS -= 1;
break;
case VM_IN:
a = TOS;
TOS = vm->ports[a];
vm->ports[a] = 0;
break;
case VM_OUT:
vm->ports[0] = 0;
vm->ports[TOS] = NOS;
DROP DROP
break;
case VM_WAIT:
if (vm->ports[0] == 1)
break;
/* Input */
if (vm->ports[0] == 0 && vm->ports[1] == 1) {
vm->ports[1] = dev_getch();
vm->ports[0] = 1;
}
/* Output (character generator) */
if (vm->ports[2] == 1) {
dev_putch(TOS); DROP
vm->ports[2] = 0;
vm->ports[0] = 1;
}
if (vm->ports[4] != 0) {
vm->ports[0] = 1;
switch (vm->ports[4]) {
case 1: vm_save_image(vm, vm->filename);
vm->ports[4] = 0;
break;
case 2: file_add(vm);
vm->ports[4] = 0;
break;
case -1: vm->ports[4] = file_handle(vm);
break;
case -2: vm->ports[4] = file_readc(vm);
break;
case -3: vm->ports[4] = file_writec(vm);
break;
case -4: vm->ports[4] = file_closehandle(vm);
break;
case -5: vm->ports[4] = file_getpos(vm);
break;
case -6: vm->ports[4] = file_seek(vm);
break;
case -7: vm->ports[4] = file_size(vm);
break;
default: vm->ports[4] = 0;
}
}
/* Capabilities */
if (vm->ports[5] != 0) {
vm->ports[0] = 1;
switch(vm->ports[5]) {
case -1: vm->ports[5] = IMAGE_SIZE;
break;
case -2: vm->ports[5] = 0;
break;
case -3: vm->ports[5] = 0;
break;
case -4: vm->ports[5] = 0;
break;
case -5: vm->ports[5] = vm->sp;
break;
case -6: vm->ports[5] = vm->rsp;
break;
case -7: vm->ports[5] = 0;
break;
case -8: vm->ports[5] = time(NULL);
break;
case -9: vm->ports[5] = 0;
vm->ip = IMAGE_SIZE;
break;
default: vm->ports[5] = 0;
}
}
if (vm->ports[8] != 0) {
vm->ports[0] = 1;
switch (vm->ports[8]) {
case -1: rsocket(vm);
vm->ports[8] = 0;
break;
case -2: rbind(vm);
vm->ports[8] = 0;
break;
case -3: rlisten(vm);
vm->ports[8] = 0;
break;
case -4: raccept(vm);
vm->ports[8] = 0;
break;
case -5: rclose(vm);
vm->ports[8] = 0;
break;
case -6: rsend(vm);
vm->ports[8] = 0;
break;
case -7: rrecv(vm);
vm->ports[8] = 0;
break;
case -8: rconnect(vm);
vm->ports[8] = 0;
break;
default: vm->ports[8] = 0;
}
vm->ports[8] = 0;
}
break;
default:
vm->rsp++;
TORS = vm->ip;
vm->ip = vm->image[vm->ip] - 1;
if (vm->ip < 0)
vm->ip = IMAGE_SIZE;
else {
if (vm->image[vm->ip+1] == 0)
vm->ip++;
if (vm->image[vm->ip+1] == 0)
vm->ip++;
}
break;
}
vm->ports[3] = 1;
}
ssize_t send(int socket, const void *buf, size_t len, int flags)
{
int fd;
return (fd_fork_get(socket, &fd) == fd_rsocket) ?
rsend(fd, buf, len, flags) : real.send(fd, buf, len, flags);
}
void main_loop() {
char c;
int rinc=100,storage_pos=1;
while (c!='Q') {
display_pos();
c=getch();
if (c==27) c=getch();
move(20,0);
//printw("keycode: %d ",c);
switch (c)
{
case 32: reset();break; /* SPACE */
case 104: /* h */
case 72: helpscreen();break; /* H */
case 82: rsend("RS");break; /* R */
case 121: /* y */
case 122: rmove_base(rinc);break; /* z */
case 120: rmove_base(-rinc);break; /* x */
case 113: rmove_shoulder(rinc);break; /* q */
case 97: rmove_shoulder(-rinc);break; /* a */
case 119: rmove_elbow(rinc);break; /* w */
case 115: rmove_elbow(-rinc);break; /* s */
case 101: rmove_pitch(rinc);break; /* e */
case 100: rmove_pitch(-rinc);break; /* d */
case 114: rmove_turnhand(rinc);break; /* r */
case 116: rmove_turnhand(-rinc);break; /* t */
case 102: rmove_grip(1);break; /* f */
case 103: rmove_grip(0);break; /* g */
case 45: /* - */
rinc -= rinc/2;
if (rinc < MIN_INC) rinc=MIN_INC;
show_panel(6,itoa(rinc));
break;
case 43: /* + */
rinc = rinc < 100 ? 100:rinc+100;
if (rinc > MAX_INC) rinc= MAX_INC;
show_panel(6,itoa(rinc));
break;
case 68: posdata[storage_pos].saved=0; /* D */
show_panel(7,itoa(storage_pos));break;
case 44: storage_pos+=49; /* , */
case 5: storage_pos++ ; /* CRSR_left */
if (storage_pos > POS_SPACE) storage_pos=POS_SPACE;
show_panel(7,itoa(storage_pos));
break;
case 46: storage_pos-=49; /* . */
case 4: storage_pos-- ; /* CRSR_right */
if (storage_pos < 1) storage_pos=1;
show_panel(7,itoa(storage_pos));
break;
case 2: store_position(storage_pos);break; /* CRSR_down */
case 3: recall_position(storage_pos);break;/* CRSR_up */
case 71: rsend("RN 1");break; /* SHIFT_G */
case 7: run_program();break; /* CTRL_G */
case 65 : load_posdata(); /* CTRL_CRSR_UP" */
show_panel(7,itoa(storage_pos));break;
case 66 : save_posdata();break; /* CTRL_CRSR_DOWN */
}
}
}
void rmove_grip(int grip) {
rpos.grip=grip;
if (grip==0) rsend("GO");
else rsend("GC");
}