int base;

int shoulder;

int elbow;

int pitch;

int wrist;

int grip;

int saved;

move(2,0); printw("Base :%5d (y-x)",rpos.base);

move(3,0); printw("Shoulder :%5d (q-a)",rpos.shoulder);

move(4,0); printw("Elbow :%5d (w-s)",rpos.elbow);

move(5,0); printw("HandPitch:%5d (e-d)",rpos.pitch);

move(6,0); printw("HandTurn :%5d (r-t)",rpos.wrist);

move(7,0); printw("Hand : %s (f-g)",rpos.grip ? "ZU ":"AUF");

int i;

switch (what) {

case 0: display_pos();break;

case 1: // current command

move(22,0);

printw("cmd: %s ",value);break;

case 3: // progess for sending Data

move(11,0);printw("Sende Daten... ");

move(11,18);printw("%s %%",value);break;

case 4: // progess for sending program

move(11,0);printw("Sende Programm... ");

move(11,18);printw("%s %%",value);break;

case 5: // data transfer complete

for (i=2;i<=4;i++) { move(i,30);printw(" "); }

move(11,0); printw("Gesendet. Starte Programm...");break;

case 6: // increment changed

move(21,1); printw("inc: %s ",value);break;

case 2: // Position saved

case 8: // Position loaded

move(11,0);

printw("Position #%s %s ",value,what == 8 ? "geladen. ":"gespeichert.");

case 7: // storage positions

move(20,25); printw("|%4s",value);

move (21,15);

int curpos=atoi(value);

for (i=curpos-10;i<=curpos+10;i++) {

if(i>0 && i<=POS_SPACE)

printw("%c",posdata[i].saved ? 'X':'.');

else printw(" ");

}

}

refresh();

FILE *fp;

show_panel(1,cmd);

fp=fopen(DEV_OUT,"w");

fputs(cmd ,fp);

fputs("\n",fp);

fclose(fp);

usleep(10000);

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");

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

if ((rpos.base+step <= MAX_BASE) && (rpos.base+step >= MIN_BASE)) {

rpos.base+=step;

rmove(1,step);

} else {

if(rpos.base+step > MAX_BASE) { rmove(1,MAX_BASE-rpos.base);

rpos.base=MAX_BASE; }

if(rpos.base+step < MIN_BASE) { rmove(1,MIN_BASE-rpos.base);

rpos.base=MIN_BASE; }

}

if ((rpos.shoulder+step <= MAX_SHOULDER) && (rpos.shoulder+step >= MIN_SHOULDER)) {

rpos.shoulder+=step;

rmove(2,step);

} else {

if(rpos.shoulder+step > MAX_SHOULDER) { rmove(2,MAX_SHOULDER-rpos.shoulder);

rpos.shoulder=MAX_SHOULDER; }

if(rpos.shoulder+step < MIN_SHOULDER) { rmove(2,MIN_SHOULDER-rpos.shoulder);

rpos.shoulder=MIN_SHOULDER; }

}

if ((rpos.elbow+step <= MAX_ELBOW) && (rpos.elbow+step >= MIN_ELBOW)) {

rpos.elbow+=step;

rmove(3,step);

} else {

if(rpos.elbow+step > MAX_ELBOW) { rmove(3,MAX_ELBOW-rpos.elbow);

rpos.elbow=MAX_ELBOW; }

if(rpos.elbow+step < MIN_ELBOW) { rmove(3,MIN_ELBOW-rpos.elbow);

rpos.elbow=MIN_ELBOW; }

}

step/=2;

if ((rpos.pitch+step <= MAX_PITCH) && (rpos.pitch+step >= MIN_PITCH)) {

rpos.pitch+=step;

rmove(4,step);

} else {

if(rpos.pitch+step > MAX_PITCH) { rmove(4,MAX_PITCH-rpos.pitch);

rpos.pitch=MAX_PITCH; }

if(rpos.pitch+step < MIN_PITCH) { rmove(4,MIN_PITCH-rpos.pitch);

rpos.pitch=MIN_PITCH; }

}

if ((rpos.wrist+step <= MAX_TURNHAND) && (rpos.wrist+step >= MIN_TURNHAND)) {

rpos.wrist+=step;

rmove(5,step);

} else {

if(rpos.wrist+step > MAX_TURNHAND) { rmove(5,MAX_TURNHAND-rpos.wrist);

rpos.wrist=MAX_TURNHAND; }

if(rpos.wrist+step < MIN_TURNHAND) { rmove(5,MIN_TURNHAND-rpos.wrist);

rpos.wrist=MIN_TURNHAND; }

}

rpos.grip=grip;

if (grip==0) rsend("GO");

else rsend("GC");

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

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

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,"");

char *filename=NULL;

FILE *fp;

filename=(char *)calloc(1024,1);

move(12,0); printw("Save Posdata to: ");refresh();

echo();nocbreak();

getstr(filename);

noecho();cbreak();

fp=fopen(filename,"w");

move(12,0);printw(" ");

refresh();

fwrite((void *)posdata,sizeof(robot_pos),POS_SPACE,fp);

fclose(fp);

move(12,0);printw(" ");

refresh();

free(filename);

char *filename=NULL;

FILE *fp;

filename=(char *)calloc(1024,1);

move(12,0); printw("Daten laden von: ");refresh();

echo();nocbreak();

getstr(filename);

noecho();cbreak();

fp=fopen(filename,"r");

fread((void *)posdata,sizeof(robot_pos),POS_SPACE,fp);

fclose(fp);

move(12,0);printw(" ");

refresh();

free(filename);

char c;

WINDOW * win;

win=newwin(0,0,0,0);

mvwprintw(win,0,0,"HILFE --- drücke 'q' zum verlassen");

mvwprintw(win,2,0,"Der Roboter wird immer mit voller Geschwindigkeit bewegt");

mvwprintw(win,3,0,"Der Anpressdruck der Hand ist ebenfalls fest vorgegeben");

mvwprintw(win,5,0,"Die Steuerung erfolgt mittels Tasten. Welches Element des Arms");

mvwprintw(win,6,0,"zu welchen Tasten gehört, kann man im Hauptbildschirm sehen");

mvwprintw(win,8,0,"+ - : Schrittweite ändern");

mvwprintw(win,9,0,"Leertaste : Arm in Grundstellung");

mvwprintw(win,10,0,"R : Hardware-Reset des Arms");

mvwprintw(win,12,0,"--- Speichern der Positionen");

mvwprintw(win,13,0,"Pfeiltasten links-rechts: Speicherposition auswählen");

mvwprintw(win,14,0,", . : Speicherposition in 50er Schritten auswählen");

mvwprintw(win,15,0,"Pfeiltaste unten : Position speichern");

mvwprintw(win,16,0,"Pfeiltaste oben : Position laden");

mvwprintw(win,17,0,"SHIFT-D : aktuelle Position auf NULL setzen");

mvwprintw(win,18,0,"STRG-Pfeil-unten : alle Positionen in Datei sichern");

mvwprintw(win,19,0,"STRG-Pfeil-oben : alle Positionen aus Datei laden");

mvwprintw(win,21,0,"--- Roboterprogramm");

mvwprintw(win,22,0,"STRG-G : alle Positionen als Programm an Roboter senden");

mvwprintw(win,23,0,"SHIFT-G : Programm im Roboter starten");

wrefresh(win);

while (c!='q') { c=getch(); }

wclear(win);

wrefresh(win);

delwin(win);

redrawwin(mainwin);

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 */

}

}

int i;

mainwin=initscr(); noecho(); cbreak();immedok(curscr,TRUE);

keypad(stdscr, TRUE);

posdata=(robot_pos *)calloc(POS_SPACE+1,sizeof(robot_pos));

for(i=0;i<=POS_SPACE;i++) posdata[i].saved=0;

printw("Robotersteuerung RM 501 V1.0\n");

refresh();

reset();

main_loop();

echo();

endwin();