brainControl.c0000664000076600007660000000505710667333452013164 0ustar iobeidiobeid#include "global.h" void moveservo(int,int); void b(double x,double y){ double num,den; double theta; num = pow(x,2) + pow(y,2) - pow(l1Est,2) - pow(l2Est,2); den = 2*l1Est*l2Est; m2 = acos(num/den); num = l2Est * sin(m2); den = l1Est + l2Est*cos(m2); m1 = atan(y/x) - atan(num/den); if (x<0) m1 = m1 + M_PI; if (den < 0) m1 = m1 + M_PI; if (m1 > M_PI) m1 = m1 - 2*M_PI; } void defineRBF(){ FILE *fid; int nCenters; double sig,rf; if ((fid = fopen("initG5.bin","rb")) == NULL) printf("no such file!\n"); else{ fread(&nCenters,sizeof(int),1,fid); c.c1 = (double *)malloc(nCenters*sizeof(double)); c.c2 = (double *)malloc(nCenters*sizeof(double)); fread(&sig,sizeof(double),1,fid); fread(&rf,sizeof(double),1,fid); fread(c.c1,sizeof(double),nCenters,fid); fread(c.c2,sizeof(double),nCenters,fid); fclose(fid); c.nCenters = nCenters; c.sigma = sig; c.receptiveField = rf; } } double dist(int i){ double d; d = sqrt(pow((m1 -c.c1[i]),2) + pow((m2 - c.c2[i]),2)); return(d); } void g(){ int i; for (i=0;iSERVO_POS_MAX)?SERVO_POS_MAX:servPos; return(servPos); } void driveServo(){ servoPos1 = angle2servo(m1); servoPos2 = angle2servo(m2); //need to see what happens if we execute m2 //command before m1 command is done moveservo(SERVO_M1,servoPos1); moveservo(SERVO_M2,servoPos2); } void moveservo(int servonum, int position) { char buffer[6]; int num; buffer[0]=SERVO_BOARD%16 + 0xf0; buffer[1]=0x10; buffer[2]=servonum%16; buffer[3]=position%201; buffer[4]=SERVO_SPEED%101; //create packet buffer[5]='\0'; num=write(fServoPort,buffer,6); //send packet } global.h0000664000076600007660000000165510667335663012003 0ustar iobeidiobeid#include #include #include #include #include #include #include #include #define l1Est 24 #define l2Est 19 #define l1Actual 24.8 #define l2Actual 17.9 #define MAXCHAR 20 typedef struct { double *c1; double *c2; double sigma; signed int nCenters; double receptiveField; } rbfType; extern double xd,yd; extern double xc,yc; extern double xa,ya; extern double xerr,yerr; extern double m1,m2; extern double *w1,*w2; extern double *pf; extern double beta; extern rbfType c; extern int fServoPort; extern int servoPos1; extern int servoPos2; #define SERVO_POS_MIN 5 #define SERVO_POS_MAX 195 #define PORTNUM 1 #define SERVO_M1 0 #define SERVO_M2 1 #define SERVO_BOARD 0 #define SERVO_SPEED 50 #define BAUD 9600 // These next two should be defined empirically; these // are just placeholder values #define THETA_MIN_DEG 5 #define THETA_MAX_DEG 175 initG5.bin0000664000076600007660000000042410665337042012203 0ustar iobeidiobeidrzӛ@fv @UgP? g%v`ȿv`?UgP? g%v`ȿv`?%~F @P? g%v`ȿv`?%~F @@@@@@@@@@@???????????init.h0000664000076600007660000000073610666150140011465 0ustar iobeidiobeidextern void b(double,double); extern void defineRBF(); extern void g(); extern void p(); extern void updateW(); extern void computeErr(); extern double dist(int); extern int initServoPort(); extern void init_port(); extern int angle2servo(float); extern int open_port(); extern void driveServo(); double xd,yd; double xc,yc; double xa,ya; double xerr,yerr; double m1,m2; double *w1,*w2; double *pf; double beta = 0.05; rbfType c; int fServoPort; int servoPos1; int servoPos2; mainSim.c0000664000076600007660000000215710667271444012125 0ustar iobeidiobeid#include "global.h" #include "init.h" int main(){ char s[MAXCHAR]; float f1,f2; int kg = 1; FILE *fDesiredPositions,*fOut; defineRBF(); w1 = (double *)calloc(c.nCenters,sizeof(double)); w2 = (double *)calloc(c.nCenters,sizeof(double)); pf = (double *)calloc(c.nCenters,sizeof(double)); fOut = fopen("outData.bin","wb"); if (initServoPort()==0) return(0); else printf("Port opened properly\n"); //define start positions and motor values xa = 20; ya = 24; b(xa,ya); if ((fDesiredPositions=fopen("xd00.txt","r"))==NULL) printf("no such file\n"); else{ while (kg){ fscanf(fDesiredPositions,"%s",s); if (strcmp(s,"end")==0) kg = 0; else{ fwrite(&xa,sizeof(double),1,fOut); fwrite(&ya,sizeof(double),1,fOut); sscanf(s,"%f",&f1); fscanf(fDesiredPositions,"%f",&f2); xd = (double)f1; yd = (double)f2; g(); b(xd+xc,yd+yc); driveServo(); p(); computeErr(); updateW(); } } } fclose(fDesiredPositions); fclose(fOut); free(c.c1); free(c.c2); free(w1); free(w2); free(pf); printf("Simulation Complete\n"); return(0); } Makefile0000664000076600007660000000104510667333710012012 0ustar iobeidiobeidxd00: xdVecGen00.c gcc -o xdVecGen00 xdVecGen00.c ./xdVecGen00 bc: brainControl.c global.h gcc -c -lm -o brainControl.o brainControl.c servoSetup: servoSetup.c global.h gcc -c -lm -o servoSetup.o servoSetup.c mainSim: mainSim.c global.h gcc -c -lm -o mainSim.o mainSim.c main: mainSim.o servoSetup.o brainControl.o global.h gcc -lm -o mainSim servoSetup.o brainControl.o mainSim.o test: test.c brainControl.o servoSetup.o global.h gcc -lm -o test brainControl.o servoSetup.o test.c clean: rm *.exe rm *.o rm *~ rm outData.bin outData.bin0000664000076600007660000000310010667336527012450 0ustar iobeidiobeid4@8@k.oA@TN%yqA@9|r.?@2jnA@.6@,dA@ ]@6/EUA@@e 4BA@A+{@@)A@rC@5@[jv A@ Hs @Vvד@@cw"@6FZ@@׃ԙ$@cu@@%p7&@/Vo@@Ð(z(@ Gp>@@k?*@G· @@8,@2*z_?@'l.@z7H+?@AP9&0@\ d>@8h1@ח2>@11@N=@.Z2@Z~=@q3@IABUr<@9E4@O~-f;@9!Yq5@=% (;@wBRC6@Icn_y:@w4X7@9@M/7@( 9@0)8@dyM8@wߊV9@W7@Op0lY :@]X6@lpw:@i6@7s}M;@#4+5@w';@1gQO4@͇5,<@o3@$i#J:=@d۹52@=@1@+MZ>@m#0@>@ܓ:p/@pX\?@Ĕʳ-@K$?@R4ި+@Hf;: @@JHi)@S@@zHn'@q=@@, %@&ݹ$ @@#@Ce@@=!@O'@@YL@gdA@;y@/7A@^T'-}@:!PA@-Y@XXF3dA@4n!6b@X{uA@zв] @̺A@4`[?, A@Ү]l6?쨎A@oyOwؿ>F0A@Tī^'A@ʫd%QЩA@\TA@ [@ZI2 Br=@e,3Azg=@sɌ 4%'<@r4ȅ5<@hZB̈́5D;@d6Ţ.:@hX97I3>:@J >|8?Vh9@=j8D M8@(N9:䛕 8@o7C:4mkG7@#np:#{6@Ŋ,;EROo5@@64[B=6%u-3@,MC==ӪFG2@ Cm>W]1@>H7p0@ߡؿv?JCD.@'T\?6=a-@].@++@Sϝb@7)@Eua@,|o?'@'7@eqA%@h@A:?#@:D<A-19!@MV&A)Z]@ӾAA"A@plotOutData.asv0000664000076600007660000000026310665560660013332 0ustar iobeidiobeidclear; fid=fopen('outData.bin','rb'); xArchive = fread(fid,'double'); fclose(fid); xArchive = reshape(xArchive,2,[])'; fid = fopen('xd00.txt','r'); kg = 1; while kg plotOutData.m0000664000076600007660000000070210665562650012774 0ustar iobeidiobeidclc; fid=fopen('outData.bin','rb'); xArchive2 = fread(fid,'double'); fclose(fid); xArchive2 = reshape(xArchive2,2,[])'; fid = fopen('xd00.txt','r'); kg = 1; i = 1; while kg foo1 = fscanf(fid,'%s',1); if (strcmp(foo1,'end')) kg = 0; else xdVec2(i,1) = str2num(foo1); xdVec2(i,2) = fscanf(fid,'%f',1); i=i+1; end end figure(2);clf; PlotArmMotionColor(xArchive2,xdVec2); axis equal; servoSetup.c0000664000076600007660000000261110667334523012700 0ustar iobeidiobeid#include "global.h" int open_port(){ int fd; char portfile[100]={'\0'}; if(PORTNUM==1) sprintf(portfile,"/dev/ttyS0"); else if(PORTNUM==2) sprintf(portfile,"/dev/ttyS1"); else if(PORTNUM==3) sprintf(portfile,"/dev/ttyS2"); else if(PORTNUM==4) sprintf(portfile,"/dev/ttyS3"); else { printf("open_port: unrecognized port number\n"); return (-1); } if((fd=open(portfile, O_RDWR | O_NOCTTY | O_NDELAY))==-1) perror("open_port: unable to open /dev/ttyS0 - "); return (fd); } void init_port(){ struct termios options; //note: the termios structure does not support a baud rate of 14400 tcgetattr(fServoPort,&options); switch(BAUD) { case 9600: cfsetispeed(&options,B9600); cfsetospeed(&options,B9600); break; case 19200: cfsetispeed(&options,B19200); cfsetospeed(&options,B19200); break; case 38400: cfsetispeed(&options,B38400); cfsetospeed(&options,B38400); break; default: cfsetispeed(&options,B9600); cfsetospeed(&options,B9600); break; } options.c_cflag |= (CLOCAL | CREAD); options.c_cflag &= ~PARENB; options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSIZE; options.c_cflag |= CS8; tcsetattr(fServoPort,TCSANOW,&options); printf("got here\n"); } int initServoPort(){ int i = 1; if((fServoPort=open_port())==-1) //open serial port i = 0; init_port(); } test.c0000664000076600007660000000116410667335445011506 0ustar iobeidiobeid#include "global.h" #include "init.h" int main(){ if (initServoPort()==0){ printf("No can do, chief\n"); return(0); } else printf("Port opened properly\n"); while (1){ // printf("about to ask for servo posn\n"); // scanf("Enter Servo Position 1 [0-200]: %i",&servoPos1); // scanf("Enter Servo Position 2 [0-200]: %i",&servoPos2); printf("Enter Servo Position 1 [0-200]: "); scanf("%d",&servoPos1); printf("Enter Servo Position 2 [0-200]: "); scanf("%d",&servoPos2); moveservo(SERVO_M1,servoPos1); moveservo(SERVO_M2,servoPos2); printf("Done\n\n"); } } xd00.txt0000664000076600007660000000374210667271532011677 0ustar iobeidiobeid34.982730 1.099377 34.934378 2.142242 34.854946 3.183201 34.744502 4.221328 34.603145 5.255699 34.431001 6.285395 34.228223 7.309498 33.994992 8.327097 33.731514 9.337288 33.438025 10.339171 33.114786 11.331856 32.762084 12.314458 32.380232 13.286103 31.969572 14.245928 31.530467 15.193078 31.063310 16.126711 30.568514 17.045995 30.046522 17.950113 29.497796 18.838260 28.922825 19.709647 28.322122 20.563497 27.696219 21.399052 27.045675 22.215568 26.371068 23.012318 25.672998 23.788594 24.952086 24.543704 24.208974 25.276978 23.444322 25.987762 22.658812 26.675424 21.853142 27.339353 21.028028 27.978957 20.184206 28.593668 19.322425 29.182938 18.443453 29.746244 17.548071 30.283084 16.637077 30.792981 15.711280 31.275481 14.771504 31.730154 13.818586 32.156596 12.853373 32.554428 11.876725 32.923296 10.889509 33.262871 9.892605 33.572852 8.886899 33.852962 7.873287 34.102952 6.852669 34.322601 5.825955 34.511712 4.794057 34.670117 3.757893 34.797676 2.718386 34.894274 1.676461 34.959827 0.633044 34.994275 -0.410936 34.997588 -1.454551 34.969762 -2.496871 34.910824 -3.536970 34.820825 -4.573922 34.699845 -5.606805 34.547992 -6.634699 34.365401 -7.656690 34.152234 -8.671869 33.908682 -9.679332 33.634960 -10.678184 33.331313 -11.667534 32.998010 -12.646504 32.635348 -13.614222 32.243650 -14.569828 31.823264 -15.512470 31.374564 -16.441310 30.897950 -17.355523 30.393845 -18.254294 29.862699 -19.136823 29.304982 -20.002326 28.721193 -20.850033 28.111850 -21.679189 27.477495 -22.489057 26.818693 -23.278916 26.136030 -24.048063 25.430113 -24.795814 24.701571 -25.521504 23.951051 -26.224486 23.179221 -26.904137 22.386769 -27.559850 21.574398 -28.191043 20.742833 -28.797153 19.892812 -29.377643 19.025092 -29.931994 18.140445 -30.459714 17.239658 -30.960334 16.323532 -31.433408 15.392884 -31.878515 14.448540 -32.295259 13.491341 -32.683269 12.522138 -33.042200 11.541794 -33.371733 10.551181 -33.671575 9.551181 -33.941458 8.542682 -34.181143 7.526584 -34.390416 6.503788 -34.569092 5.475206 end xdVecGen00.c0000664000076600007660000000060310667271500012356 0ustar iobeidiobeid#include #include #include int main(){ double r = 35; int nSteps = 100; int i; FILE *fid,*fid2; double theta,dt; fid = fopen("xd00.txt","w"); dt = M_PI*(0.95 - 0.01)/(nSteps-1); theta = M_PI*0.01; for (i=0;i #include #include #include #include #include int open_port(int portnum) { int fd; char portfile[100]={'\0'}; if(portnum==1) sprintf(portfile,"/dev/ttyS0"); else if(portnum==2) sprintf(portfile,"/dev/ttyS1"); else if(portnum==3) sprintf(portfile,"/dev/ttyS2"); else if(portnum==4) sprintf(portfile,"/dev/ttyS3"); else { printf("open_port: unrecognized port number\n"); return (-1); } if((fd=open(portfile, O_RDWR | O_NOCTTY | O_NDELAY))==-1) perror("open_port: unable to open /dev/ttyS0 - "); return (fd); } void init_port(int *fd, unsigned int baud) { struct termios options; //note: the termios structure does not support a baud rate of 14400 tcgetattr(*fd,&options); switch(baud) { case 9600: cfsetispeed(&options,B9600); cfsetospeed(&options,B9600); break; case 19200: cfsetispeed(&options,B19200); cfsetospeed(&options,B19200); break; case 38400: cfsetispeed(&options,B38400); cfsetospeed(&options,B38400); break; default: cfsetispeed(&options,B9600); cfsetospeed(&options,B9600); break; } options.c_cflag |= (CLOCAL | CREAD); options.c_cflag &= ~PARENB; options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSIZE; options.c_cflag |= CS8; tcsetattr(*fd,TCSANOW,&options); } void moveservo(int *fd, int boardnum, int servonum, int position, int speed) { char buffer[6]; int num; buffer[0]=boardnum%16 + 0xf0; buffer[1]=0x10; buffer[2]=servonum%16; buffer[3]=position%201; buffer[4]=speed%101; //create packet buffer[5]='\0'; num=write(*fd,buffer,6); //send packet } int main() { int fd,board,servo,position,speed,portnum; printf(" \n"); printf(" ServoCenter 3.1 Demonstration Program \n"); printf(" (c)2000-2004 Yost Engineering, Inc. \n"); printf(" www.YostEngineering.com \n"); printf(" \n"); printf("Enter Port Number (1-4)\n"); scanf("%d",&portnum); if((fd=open_port(portnum))==-1) //open serial port return (1); init_port(&fd,9600); //set serial port to 9600,8,n,1 while(1) { printf("Enter Board Number (0-15)\n"); scanf("%d",&board); printf("Enter Servo Number (0-15)\n"); scanf("%d",&servo); printf("Enter Position (0-200)\n"); scanf("%d",&position); printf("Enter Speed (1-100)\n"); scanf("%d",&speed); printf("Sending Command..."); moveservo(&fd,board,servo,position,speed); printf("done!\n"); } return (0); }