Makefile0100644000175400010010000000113610667560457012103 0ustar Iyad ObeidNonexd00: xdVecGen00.c gcc -o xdVecGen00 xdVecGen00.c ./xdVecGen00 yost: ServoCenter_gcc.c gcc -o yost ServoCenter_gcc.c 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 Matlab/0040755000175400010010000000000010667536410011635 5ustar Iyad ObeidNoneMatlab/B.asv0100755000175400010010000000070410604171610012516 0ustar Iyad ObeidNonefunction [m] = B(x) global l1Est l2Est zeta x1 = x(:,1); x2 = x(:,2); l1 = l1Est; l2 = l2Est; num = x1.^2 + x2.^2 - l1^2 - l2^2; den = 2*l1*l2; theta = acos(num/den); m(:,2) = zeta * theta; num = l2 * sin(m(:,2)); den = l1 + l2*cos(m(:,2)); m(1) = atan(x2./x1) - atan(num./den); r = find(x1<0); m(r,1) = m(r,1) + pi; r = find(den<0); % if den < 0 m(1,r) = m(1,r) + pi; % end r = find(m(1,:)>pi); % if m(1) > pi m(1,r) = m(1,r) - 2*pi; % endMatlab/B.m0100755000175400010010000000070610604171610012163 0ustar Iyad ObeidNonefunction [m] = B(x) global l1Est l2Est zeta x1 = x(:,1); x2 = x(:,2); l1 = l1Est; l2 = l2Est; num = x1.^2 + x2.^2 - l1^2 - l2^2; den = 2*l1*l2; theta = acos(num/den); m(:,2) = zeta * theta; num = l2 * sin(m(:,2)); den = l1 + l2*cos(m(:,2)); m(:,1) = atan(x2./x1) - atan(num./den); r = find(x1<0); m(r,1) = m(r,1) + pi; r = find(den<0); % if den < 0 m(r,1) = m(r,1) + pi; % end r = find(m(:,1)>pi); % if m(1) > pi m(r,1) = m(r,1) - 2*pi; % endMatlab/calCoords.mat0100755000175400010010000000076710604171610014247 0ustar Iyad ObeidNoneMATLAB 5.0 MAT-file, Platform: PCWIN, Created on: Thu Mar 29 16:37:10 2007 IMjxc``(b6 13Ɋ N y$k68Ȯ & d48-v$ |A(/u<s0A dxc``(b6 13JN C: w87<6aSx8yy[v&Lhp]n<5Xo&93$Qxc``Hb6 3BHs>JCR@!<^/YNF0^Fέs>8xc``b6 313fdҜ@,/z{&Aj- lMatlab/calibrateMotorCenters.asv0100755000175400010010000000217710604171610016636 0ustar Iyad ObeidNone% calibrate motor coordinate grid from figure 6 % use ginput to capture points from the picture and then % work backwards to determine the motor centers mpic = imread('motorCoordPic.bmp'); clf; image(mpic); axis equal; % figure(3);xx = ginput(4); % save calCoords xx load calCoords one_x = (xx(2,1)-xx(1,1))/2; % one_x is in PIXELS offset_x = (xx(3,1)-xx(1,1))/one_x; dx = (xx(4,1)-xx(3,1))/one_x; % figure(3); yy = ginput(4); % save calCoords yy -append load calCoords one_y = (yy(2,2)-yy(1,2))/1; offset_y =(yy(3,2)-yy(1,2))/one_y; dy = (yy(4,2)-yy(3,2))/one_y; % hold on % for x=-3:2 % for y = 0:2 % plot((offset_x + x*dx)*one_x+xx(1,1), ... % (offset_y + y*dy)*one_y+yy(1,2),'rx'); % end % end x = offset_x + [-3:2]*dx; y = offset_y + [0:2]*dy; xt = x*one_x + xx(1,1); yt = y*one_y + yy(1,2); set(gca,'xtick',sort(xt)); set(gca,'ytick',sort(yt)); grid on; rf = sqrt(max(diff(x)*one_x)^2 + max(diff(y)*one_y)^2); th = linspace(0,2*pi,50); cx = x(3); cy = y(2); xc = rf*cos(th); yc = rf*sin(th); hold on; plot(xc,yc,'r'); disp(x) disp(y) % save calCoords x y -appendMatlab/calibrateMotorCenters.m0100755000175400010010000000230010604171610016265 0ustar Iyad ObeidNone% calibrate motor coordinate grid from figure 6 % use ginput to capture points from the picture and then % work backwards to determine the motor centers mpic = imread('motorCoordPic.bmp'); clf; image(mpic); axis equal; % figure(3);xx = ginput(4); % save calCoords xx load calCoords one_x = (xx(2,1)-xx(1,1))/2; % one_x is in PIXELS offset_x = (xx(3,1)-xx(1,1))/one_x; dx = (xx(4,1)-xx(3,1))/one_x; % figure(3); yy = ginput(4); % save calCoords yy -append load calCoords one_y = (yy(2,2)-yy(1,2))/1; offset_y =(yy(3,2)-yy(1,2))/one_y; dy = (yy(4,2)-yy(3,2))/one_y; % hold on % for x=-3:2 % for y = 0:2 % plot((offset_x + x*dx)*one_x+xx(1,1), ... % (offset_y + y*dy)*one_y+yy(1,2),'rx'); % end % end x = offset_x + [-3:2]*dx; y = offset_y + [0:2]*dy; xt = x*one_x + xx(1,1); yt = y*one_y + yy(1,2); set(gca,'xtick',sort(xt)); set(gca,'ytick',sort(yt)); grid on; %take a stab at plotting the receptive field rf = .5*sqrt((one_x*max(diff(x)))^2 + (one_y*max(diff(y)))^2); th = linspace(0,2*pi,50); cx = xt(3); cy = yt(2); xc = cx + rf*cos(th); yc = cy + rf*sin(th); hold on; plot(xc,yc,'r'); disp(x) disp(y) % save calCoords x y -appendMatlab/convert_c.m0100755000175400010010000000052410665336770014003 0ustar Iyad ObeidNonefunction convert_c(fcn,fname) % use this to covert rbf centers (defined in Matlab) into a c-friendly % format saved in a binary file c = feval(fcn); fid = fopen(fname,'wb'); fwrite(fid,c.nCenters,'int'); fwrite(fid,c.sig,'double'); fwrite(fid,c.receptiveField,'double'); fwrite(fid,c.c1,'double'); fwrite(fid,c.c2,'double'); fclose(fid); Matlab/convert_c.m~0100755000175400010010000000030510665332420014162 0ustar Iyad ObeidNonefunction convert_c(fcn,fname) % use this to covert rbf centers (defined in Matlab) into a c-friendly % format saved in a binary file c = feval(fcn); fid = fopen(fname,'wb'); fwrite( fclose(fid); Matlab/foo.m~0100755000175400010010000000013310665336504012771 0ustar Iyad ObeidNonefunction foo x = 12; fid = fopen('../C/test.bin','wb'); fwrite(fid,x,'char'); fclose(fid);Matlab/foo1.png0100755000175400010010000002752710643456070013221 0ustar Iyad ObeidNonePNG  IHDR4 pHYsgRtIMEϠz"tEXtCreation Time06-Jul-2007 11:14:08$tEXtSoftwareMATLAB, The Mathworks, Inc.R IDATxۖP_yp. ֜lBB? HzLK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rz~~>)lV62Y|_|)ц b=^? h\?F2atk @ %\ݝd  L=` eN@%%J2m,]hXJd{Pd [bB@SĒ() 5;݉ﷆv"9 d %*2a&ZƔ 6 \/Q 2J;qJd†X)JJ{q/VTR2ؑLJJ ۢPI`2aCTw*)3f-M@ %'%2wzPI`w rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\~|N?8LJ夷{JLdӎuٯUKB`)m]ѿ- v-."2T?mI{mƴh}2acpӽfyЖvΝ6TdB5yIwe͘ѽWS[+s''#LUoX,4cA2 dBjsY5_o&$pK;I]4Ʉv91<~MhKm8`raL&dσs!S^ٳuo_> idtuPPi,|V+;3Ldk`+,п}ݜd8߮ M5-y&I&XasTP-<7=Мd ¥%g|-_)nZ?9VYo9)6 8퀵W<n,4oZ1SrĩUoAY=8O,OZ9 .a0A'?U83\w>ܻլ4mXGt'8q=;uչ:'j|5vT[]if\;1YԘ! C={<'C >m=79rY 'BW O<3sP0qm):'@#{ݿ/n{ftbG:z3VH/ ]5[ё(/GH/4HW1N}C1Cդ]ϷlyrzBNAGod.^id=~q{v[mZ4L8ρD<.Æs?arNY'4V)/L]_og9\YJy\?2]bPfkANghg|T/;Tzpvd\Qnp9s۝`n&y2?d(5z2!Tɥ&0"2s'MܖLHy!- ]9̌ 6]k2q_Q!pM,W̠c%gB8! }8 ,.&f} dBX̗+T,*,9k70dLZ:W9` ³ȃ5ŃwsV a+A01ƁWlAocsǟ}dBU, #T&ɐL͚yTX_b`ߕf&߿+\( }Ci?nx6>jU@3gnp{ g踑 5$ az|ZN 8Aa9sj8BdBX&M1-7YvI&|8#T瘼hɗטl7b[]&LGwrK  ?!'Q\WZU6bg]+>Hh8 =&nl~H??:Nh:%瘛jP6E-O_vi70L  K~u#7 \`[#?yĹj3BhP/_#eq͐y&7@SF0iWG+:U}9*_<9w^bsցdB8[a*G-sj>+ N,QL~ўK&M|÷d3XGJ;%fBKӦw/(Hg7|BkŹ;13+<^4kp輸\!*sE aoqLϦ1]tGjd|QYQsRƱܟp13u70|摎oiMɸD%5vKp0lhǼq○+\;0_ң:^quS!BHy/:8 &…&<@Q{#}. [/h9$;2N73u*/jl8يCdBLnҗ@V Yy"8wt`*B4 ~ |ip%c_y>X`ᙓ+%zx33V(%d+#cVJ.r3aeyFqg>u'yFK䔌sV{.КL8h“$~/X-dWWrT5fql|Lؙ*gg,t&P2C %<@,LE޴OI&ɕo=L)G,!Zyot8B)1v͍# o+ )O׏>C~ަ,ms\b]:}uM~ѶFZrs A?Mv6UiJ8ayP-+-ɢJƺq='@.5fZ:N)_+i<\ad9_Ԁ* ^#H 3ᗆHAt,u,o?oGH;wo4P(^2 2!@b`i&.xqZcK6sHْKR?f yL !Ը[y 9WsӶ d'{uШXX S49xe^%KYm|S*o9;wdh[zʡbn0紒!'pP8vf!YMSYpعt\n'dwܑqV :ipCKFM*@Ʉ͙k3f0tiR[3Z2*.eoɤʮva!`iDOKxwpS ۢKM;&:dto;9ɄUT;2]^X?UL,$ЭKBS}q?D5RQou%H_bTlXz? t2Ax>GH' )c!&D:m(юOcB8'jXf2=z8ḀnUB<@gL8pG>{/evd@&|&CcEq\fUvmJF 5LUX 4H 5f'\ ?Ng&y'^ЊiS Y&@%h!EfB],DD̒\$@qSJ߉{v"MI+AB3ᢲ}B 6s pt<^Qݸ'g̽ Yݡ5J8NXJ?Fe~dn'i4"d$ffw8+w!lwBPtBȄhjv[&#'XBd$I-Iwܸe68np/ qC8篫73Ȅ%:=*Υ}iTb&Y2ĹRnB#Fү': G/0r)/>w4#f~#BS p.}](<[ѯ+Twh`ՙCGi4Kr3W[^Z6=DZm W>J+ %G ;:g􍃢iT`07Hm1dB !ĿydB  3Ʉ vo!p!ܒ:SI3Ʉ -Io,4rF&X$^Ȅkc?M%%TSE^h 6LGڽw( v09;N4ؗL˶ `LaSIEM`#`g s4!aÎCx5ˇp_2!%+򡙥p#2!ù X ʁp/2!+  'Æp/zLK&`m(ܗ J3pk ؓ1C LL%{ rɄdB\2!@. LK&% rɄdB\2!@. LK& `?|dLؖ2.mH/zdㄭTOSɔ vdE|uƔ 6 ?[SMd/-/%]ȄdB\m>4r&;& R2֌މudPI& KLx; q=aM1e]hHp/JҐ\|'?~/ .aE1%d8 @%%mˠvd R ^Pd# ; %Eu.d†TR2؋uGWIq/  r; 8!@. ן7}W3+ -GK }ۉfv40eJ)7z+}.^?fL^ol'|dq_;YFD3;OshiP2bm}U['DK{E_v'L%%u[s=L6 rlN7h>aΡP2ѻi~b`5--/B;̎ZZ%Id3&94jll'|œCKn!P2 5fni!=ܖQnl'|S2Bɸ/k@v!dBH1wqeQ\O^Wz+%8!d5Cɀ BIDAT-QJ2NOVY?Op STS2adB\2!@.9?W`+hiN49F %M֘'s@Bd@j HKv'L %s( 2w #и*:4jll'|P28wv+Xh*ZZEZ7h>aj(dܗx5s4jll'|P281>YelڢYwIv'L % r; K&% rɄdB\2!@. LK&% rɄdB\2!@. LK& hs8%8qB\ p9gvsRHsRhLWRd1w LmXb&?>;?&m(܎dU\OM#VϷ:%e[p[P2ʯU2`pWsrݓ {moUQ-sWXn< ?-Q_yi?#%c)qBhڠn-NS.?2h6=GQS2 Q2@&ZZ ϟnm3@S2`@&vrRscۥpny_/%dR鞿5N܂Q.:{h& ()p_Bsx +}/k9nAɀ(2!ƥȡd]; )%ypA,چgAɀ(2!<\ܛ `vJd !)u~/9nʿԊR2 <]A: eQ\ت/۷aHز _B%p/}6 Hc78yfΛ/؆kvy-PxnpNp;Jf(@. LK&% r@.dB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@. LK&% rɄdB\2!@胨I`IENDB`Matlab/G.asv0100755000175400010010000000105510665603410012530 0ustar Iyad ObeidNonefunction [p,xc] = G(m) global w c %granule cell function %this is the cerebellum model % % execute this code if there is no receptive field limitation % p = exp(-((m(1)-c.c1).^2 + (m(2)-c.c2).^2)/(2*c.sig^2)); % xc = p' * w; % execute this code if only cells within receptive field of m are active r = find(dist(m,c).001 || cnt==1) cnt = cnt + 1; % xArchive = [xArchive ; x]; % mArchive = [mArchive ; m]; % dxArchive = [dxArchive ; dx]; % dxTotArchive = [dxTotArchive ; dxTot]; % wArchive = [wArchive ; reshape(w,1,[])]; [p,xc] = G(m); m = B(xd + xc); x = P(m); [dx,dxTot] = xErr(x,xd); updateW(dx,p); end wArchiv xBlk(i,j) = x(1); yBlk(i,j) = x(2); i = i + 1; end j = j + 1; end xGray = xGray'; yGray = yGray'; xBlk = xBlk'; yBlk = yBlk'; for i = 1:length(thVec) for r = 1:length(rVec)-1 h1(r,i) = line([xGray(r,i) xGray(r+1,i)],[yGray(r,i) yGray(r+1,i)]); h2(r,i) = line([xBlk(r,i) xBlk(r+1,i)],[yBlk(r,i) yBlk(r+1,i)]); end end set(h1,'color',[.5 .5 .5],'linewidth',3); set(h2,'color',[ 0 0 0 ]); clear h1 h2 for r = 1:length(rVec) for i = 1:length(thVec)-1 h1(r,i) = line([xGray(r,i) xGray(r,i+1)],[yGray(r,i) yGray(r,i+1)]); h2(r,i) = line([xBlk(r,i) xBlk(r,i+1)],[yBlk(r,i) yBlk(r,i+1)]); end end set(h1,'color',[.5 .5 .5],'linewidth',3); set(h2,'color',[ 0 0 0 ]); axis([-3 3 -3 3]); axis equal Matlab/recreateFig6.m0100755000175400010010000000365010604171610014311 0ustar Iyad ObeidNone% attempt to recreate figure 2 clear;clc; global l1Est l2Est l1Actual l2Actual zeta c w beta l1Est = 1.1; l2Est = 1.8; l1Actual = 1; l2Actual = 2; zeta = 1; c = initG5; w = zeros(c.nCenters,2); %initialize weights to zero beta = 0.05; clf; hold on; thVec = pi*linspace(-160,160,26)/180; x = [1.5 0]; j = 1; rVec = linspace(1.3,2.5,7); wArchive = []; for r = rVec i = 1; for th = thVec xGray(i,j) = r*cos(th); yGray(i,j) = r*sin(th); xd = [xGray(i,j) yGray(i,j)]; m = B(x); % starting (and current) motor position [dx,dxTot] = xErr(x,xd); cnt = 1; while(dxTot>.001 || cnt==1) cnt = cnt + 1; % xArchive = [xArchive ; x]; % mArchive = [mArchive ; m]; % dxArchive = [dxArchive ; dx]; % dxTotArchive = [dxTotArchive ; dxTot]; % wArchive = [wArchive ; reshape(w,1,[])]; [p,xc] = G(m); m = B(xd + xc); x = P(m); [dx,dxTot] = xErr(x,xd); updateW(dx,p); end wArchive = [wArchive w(:)]; xBlk(i,j) = x(1); yBlk(i,j) = x(2); i = i + 1; end j = j + 1; end xGray = xGray'; yGray = yGray'; xBlk = xBlk'; yBlk = yBlk'; for i = 1:length(thVec) for r = 1:length(rVec)-1 h1(r,i) = line([xGray(r,i) xGray(r+1,i)],[yGray(r,i) yGray(r+1,i)]); h2(r,i) = line([xBlk(r,i) xBlk(r+1,i)],[yBlk(r,i) yBlk(r+1,i)]); end end set(h1,'color',[.5 .5 .5],'linewidth',3); set(h2,'color',[ 0 0 0 ]); clear h1 h2 for r = 1:length(rVec) for i = 1:length(thVec)-1 h1(r,i) = line([xGray(r,i) xGray(r,i+1)],[yGray(r,i) yGray(r,i+1)]); h2(r,i) = line([xBlk(r,i) xBlk(r,i+1)],[yBlk(r,i) yBlk(r,i+1)]); end end set(h1,'color',[.5 .5 .5],'linewidth',3); set(h2,'color',[ 0 0 0 ]); axis([-3 3 -3 3]); axis equal Matlab/rsquared.m0100755000175400010010000000051010645706054013634 0ustar Iyad ObeidNonefunction varargout = rsquared(x,y) p=polyfit(x,y,1); yhat = polyval(p,x); ssr = mean((yhat - mean(y)).^2); sst = mean((y - mean(y)).^2); r2 = ssr/sst; disp(['r-squared = ' sprintf('%.3f',r2)]); if nargout >= 1 varargout{1} = r2; end if nargout >= 2 vargout{2} = p; end if nargout >= 3 varagout{3} = yhat; endMatlab/simArm.asv0100755000175400010010000000533010665424710013576 0ustar Iyad ObeidNoneclear all; clc;clf; global l1Est l2Est l1Actual l2Actual zeta w c beta %define the cerebellum model initG = @initG5; %initialize constants l1Actual = 1.1; l1Est = 1; l2Actual = 1.8; l2Est = 2; % l1Est = 1.1; l1Actual = 1; % l2Est = 1.8; l2Actual = 2; for k = 1:1 zeta = 1; c = feval(initG); if k == 1 w = zeros(c.nCenters,2); %initialize weights to zero beta = 0.05; else % w = 0.5*rand(c.nCenters,2) - 0.25; load wbad; beta = 0; end nSteps = 100; nLaps = 1; % define continuously moving desired position, xd xdVec = xdVecGen00(nSteps,nLaps); nSteps = nSteps * nLaps; %define start point and make sure its actually a point that the arm reach x = [0 1.5]; % starting (and current) position m = B(x); % starting (and current) motor position if ~isreal(m), disp('ERROR'), return; end; [dx,dxTot] = xErr(x,xdVec(1,:)); % initial error % NOTE: xc is xCerebellum xArchive = []; mArchive = []; dxArchive = []; dxTotArchive = []; wArchive = []; for i = 1:nSteps cnt = 1; xd = xdVec(i,:); while (cnt==1) % while(dxTot>.001 || cnt==1) cnt = cnt + 1; xArchive = [xArchive ; x]; mArchive = [mArchive ; m]; dxArchive = [dxArchive ; dx]; dxTotArchive = [dxTotArchive ; dxTot]; wArchive = [wArchive ; reshape(w,1,[])]; [p,xc] = G(m); m = B(xd + xc); x = P(m); [dx,dxTot] = xErr(x,xd); updateW(dx,p); end end if k == 1 good = xArchive; target = xdVec; else bad = xArchive; end end %create the plot % figure(1); clf; % PlotArmMotionColor(xArchive,xdVec); % axis equal % % figure(2); clf; % PlotError(dxTotArchive); % % figure(3);clf; % subplot 211; plot(wArchive(:,1:end/2)); % subplot 212; plot(wArchive(:,(end/2)+1:end)); % figure(2); % clf % subplot 321 % plot(good(:,1),'linewidth',2,'color','k','linestyle',':'); % ylim([-2 2]); % hold on % plot(target(:,1),'linewidth',2,'color','k','linestyle','-'); % plot(bad(:,1),'linewidth',2,'color','k','linestyle','--'); % xlabel('timestep','fontsize',12); % ylabel('position','fontsize',12); % set(gca,'fontsize',12); % % subplot 322 % plot(good(:,2),'linewidth',2,'color','k','linestyle',':'); % ylim([-2 2]); % hold on % plot(target(:,2),'linewidth',2,'color','k','linestyle','-'); % plot(bad(:,2),'linewidth',2,'color','k','linestyle','--'); % xlabel('timestep','fontsize',12); % ylabel('position','fontsize',12); % set(gca,'fontsize',12); Matlab/simArm.m0100755000175400010010000000535110667536546013261 0ustar Iyad ObeidNoneclc global l1Est l2Est l1Actual l2Actual zeta w c beta %define the cerebellum model initG = @initG5; %initialize constants l1Actual = 24.8; l1Est = 24; l2Actual = 17.9; l2Est = 19; % l1Est = 1.1; l1Actual = 1; % l2Est = 1.8; l2Actual = 2; for k = 1:1 zeta = 1; c = feval(initG); if k == 1 w = zeros(c.nCenters,2); %initialize weights to zero beta = 0.05; else % w = 0.5*rand(c.nCenters,2) - 0.25; load wbad; beta = 0; end nSteps = 100; nLaps = 1; % define continuously moving desired position, xd xdVec = xdVecGen00(nSteps,nLaps); nSteps = nSteps * nLaps; %define start point and make sure its actually a point that the arm reach x = [20 24]; % starting (and current) position m = B(x); % starting (and current) motor position if ~isreal(m), disp('ERROR'), return; end; pf=zeros(c.nCenters,1); [dx,dxTot] = xErr(x,xdVec(1,:)); % initial error % NOTE: xc is xCerebellum xArchive = []; mArchive = []; dxArchive = []; dxTotArchive = []; wArchive = []; for i = 1:nSteps cnt = 1; xd = xdVec(i,:); while (cnt==1) % while(dxTot>.001 || cnt==1) cnt = cnt + 1; xArchive = [xArchive ; x]; mArchive = [mArchive ; m]; dxArchive = [dxArchive ; dx]; dxTotArchive = [dxTotArchive ; dxTot]; wArchive = [wArchive ; reshape(w,1,[])]; [pf,xc] = G(m); m = B(xd + xc); x = P(m); [dx,dxTot] = xErr(x,xd); updateW(dx,pf); end end if k == 1 good = xArchive; target = xdVec; else bad = xArchive; end end %create the plot figure(1); clf; PlotArmMotionColor(xArchive,xdVec); axis equal % % figure(2); clf; % PlotError(dxTotArchive); % % figure(3);clf; % subplot 211; plot(wArchive(:,1:end/2)); % subplot 212; plot(wArchive(:,(end/2)+1:end)); % figure(2); % clf % subplot 321 % plot(good(:,1),'linewidth',2,'color','k','linestyle',':'); % ylim([-2 2]); % hold on % plot(target(:,1),'linewidth',2,'color','k','linestyle','-'); % plot(bad(:,1),'linewidth',2,'color','k','linestyle','--'); % xlabel('timestep','fontsize',12); % ylabel('position','fontsize',12); % set(gca,'fontsize',12); % % subplot 322 % plot(good(:,2),'linewidth',2,'color','k','linestyle',':'); % ylim([-2 2]); % hold on % plot(target(:,2),'linewidth',2,'color','k','linestyle','-'); % plot(bad(:,2),'linewidth',2,'color','k','linestyle','--'); % xlabel('timestep','fontsize',12); % ylabel('position','fontsize',12); % set(gca,'fontsize',12); Matlab/simArm.m~0100755000175400010010000000521510643456544013447 0ustar Iyad ObeidNoneclear all; clc;clf; global l1Est l2Est l1Actual l2Actual zeta w c beta %define the cerebellum model initG = @initG5; %initialize constants l1Actual = 1.1; l1Est = 1; l2Actual = 1.8; l2Est = 2; % l1Est = 1.1; l1Actual = 1; % l2Est = 1.8; l2Actual = 2; for k = 1:2 zeta = 1; c = feval(initG); if k == 1 w = zeros(c.nCenters,2); %initialize weights to zero beta = 0.05; else w = 0.5*rand(c.nCenters,2) - 0.25; beta = 0; end nSteps = 100; nLaps = 1; % define continuously moving desired position, xd xdVec = xdVecGen00(nSteps,nLaps); nSteps = nSteps * nLaps; %define start point and make sure its actually a point that the arm reach x = [0 1.5]; % starting (and current) position m = B(x); % starting (and current) motor position if ~isreal(m), disp('ERROR'), return; end; [dx,dxTot] = xErr(x,xdVec(1,:)); % initial error % NOTE: xc is xCerebellum xArchive = []; mArchive = []; dxArchive = []; dxTotArchive = []; wArchive = []; for i = 1:nSteps cnt = 1; xd = xdVec(i,:); while (cnt==1) % while(dxTot>.001 || cnt==1) cnt = cnt + 1; xArchive = [xArchive ; x]; mArchive = [mArchive ; m]; dxArchive = [dxArchive ; dx]; dxTotArchive = [dxTotArchive ; dxTot]; wArchive = [wArchive ; reshape(w,1,[])]; [p,xc] = G(m); m = B(xd + xc); x = P(m); [dx,dxTot] = xErr(x,xd); updateW(dx,p); end end if k == 1 good = xArchive; target = xdVec; else bad = xArchive; end end %create the plot % figure(1); clf; % PlotArmMotionColor(xArchive,xdVec); % axis equal % % figure(2); clf; % PlotError(dxTotArchive); % % figure(3);clf; % subplot 211; plot(wArchive(:,1:end/2)); % subplot 212; plot(wArchive(:,(end/2)+1:end)); figure(2); clf subplot 221 plot(good(:,1),'linewidth',2,'color','k','linestyle',':'); ylim([-2 2]); hold on plot(target(:,1),'linewidth',2,'color','k','linestyle','-'); plot(bad(:,1),'linewidth',2,'color','k','linestyle','d'); xlabel('timestep','fontsize',16); ylabel('position','fontsize',16); set(gca,'fontsize',18); subplot 222 plot(good(:,2),'linewidth',2,'color','k','linestyle','--'); ylim([-2 2]); hold on plot(target(:,2),'linewidth',2,'color','k','linestyle','-'); plot(bad(:,2),'linewidth',2,'color','k','linestyle',':'); xlabel('timestep','fontsize',16); ylabel('position','fontsize',16); set(gca,'fontsize',18); Matlab/simStatic.m0100755000175400010010000000072110576340550013750 0ustar Iyad ObeidNoneclear all; clc;clf; global l1 l2 zeta l1 = 1; l2 = 2; zeta = 1; xd = [1 1.5]; % desidered position md = B(xd); % ideally the final value of m xc = [1.5 1.5]; % starting position mc = B(xc); % starting motor position dxc = xErr(xc,xd); % initial error xArchive = []; mArchive = []; dxArchive = []; while(dxc>.01) xArchive = [xArchive ; xc]; mArchive = [mArchive ; mc]; dxArchive = [dxArchive ; dxc]; p = G(mc); end Matlab/tagData.m0100755000175400010010000000005610604171610013345 0ustar Iyad ObeidNonefunction tagData(h) set(h,'tag','usrData'); Matlab/test.asv0100755000175400010010000000011110604171610013304 0ustar Iyad ObeidNoneclear global l1,l2,zeta; zeta = 1; l1 = 1; l2 = 2; r1 A = (sqrt(2)/2Matlab/test.m0100755000175400010010000000153210604171610012757 0ustar Iyad ObeidNoneclear; clc; global l1 l2 zeta zeta = 1; l1 = 1; l2 = 2; r1 = 1.5; r2 = 2.5; % %these are the extent of the grid space for the test % [GA(1),GA(2)] = pol2cart( pi/4,r1); % [GB(1),GB(2)] = pol2cart( pi/4,r2); % [GC(1),GC(2)] = pol2cart(3*pi/4,r1); % [GD(1),GD(2)] = pol2cart(3*pi/4,r2); % % MA = B(GA); % MB = B(GB); % MC = B(GC); % MD = B(GD); % % GA2 = P(MA); % GB2 = P(MB); % GC2 = P(MC); % GD2 = P(MD); clf; hold on % m2 = linspace(1.3,2.3,10); % m1 = linspace(-1,1.45,10); m2 = linspace(0,pi,25); m1 = linspace(-pi,pi,25); [m1,m2]=meshgrid(m1,m2); [r,c]=size(m1); for i = 1:r for j = 1:c x = P([m1(i,j) m2(i,j)]); plot(x(1),x(2),'.'); end end axis equal % axis([-3 3 -3 3]); grid on th = linspace(0,pi,100); plot(1.5*cos(th),1.5*sin(th),'r'); plot(2.5*cos(th),2.5*sin(th),'r'); Matlab/test2.asv0100755000175400010010000000137010604171610013376 0ustar Iyad ObeidNoneclear all; clc global zeta l1 l2 zeta = 1; l1 = 1; l2 = 2; r = sqrt(l1^2 + l2^2); m = l1 + l2; x = [1 1]*.71 ; mEst = B(x); xEst = P(mEst); disp('x'); disp(x); disp(xEst); disp(mEst); disp(['xDist = ' num2str(sqrt(sum(x.^2)))]) disp(['lDist = ' num2str(sqrt(sum([l1 l2].^2)))]) disp(isreal(mEst)); clf; hold on plot([0 l1*cos(mEst(1))],[0 l1*sin(mEst(1))],'marker','o'); plot([l1*cos(mEst(1)) xEst(1)],[l1*sin(mEst(1)) xEst(2)],'marker','o'); plot(x(1),x(2),'marker','x','markersize',14,'markeredgecolor','m','linewidth',3); axis equal; axis([-1 1 -1 1]*m*1.05); grid on theta = linspace(0,2*pi,1000); plot(r*cos(theta),r*sin(theta),'r'); plot(m*cos(theta),m*sin(theta),'g'); % disp('m'); % disp(m); % disp(mEst); Matlab/test2.m0100755000175400010010000000175510604171610013050 0ustar Iyad ObeidNoneclear all; clc global zeta l1Est l2Est l1Actual l2Actual zeta = 1; l1Est = 1; l2Est = 2; l1Actual = l1Est; l2Actual = l2Est; l1 = l1Est; l2 = l2Est; r = sqrt(l1^2 + l2^2); mx = l1 + l2; mn = abs(l1 - l2); x = [1 1]*1; mEst = B(x); xEst = P(mEst); disp('x'); disp(x); disp(xEst); disp(mEst); disp(['xDist = ' num2str(sqrt(sum(x.^2)))]) disp(['lDist = ' num2str(sqrt(sum([l1 l2].^2)))]) disp(isreal(mEst)); clf; hold on plot([0 l1*cos(mEst(1))],[0 l1*sin(mEst(1))],'marker','o'); plot([l1*cos(mEst(1)) xEst(1)],[l1*sin(mEst(1)) xEst(2)],'marker','o'); plot(x(1),x(2),'marker','x','markersize',14,'markeredgecolor','m','linewidth',3); axis equal; axis([-1 1 -1 1]*mx*1.05); grid on theta = linspace(0,2*pi,1000); plot(r*cos(theta),r*sin(theta),'r'); plot(mx*cos(theta),mx*sin(theta),'g'); plot(mn*cos(theta),mn*sin(theta),'g'); if l1 < l2 r2 = sqrt(l2^2 - l1^2); plot(r2*cos(theta),r2*sin(theta),'r--'); end % disp('m'); % disp(m); % disp(mEst); Matlab/updateW.m0100755000175400010010000000007110604171610013406 0ustar Iyad ObeidNonefunction updateW(dx,p) global w beta w = w-beta*p*dx;Matlab/wBad.mat0100755000175400010010000000070410643460160013206 0ustar Iyad ObeidNoneMATLAB 5.0 MAT-file, Platform: MACI, Created on: Fri Jul 6 11:32:31 2007 IM<xc``0`d``@P31#iN0ၮG2 k|g;ao3_I! ?zbyGͪi7￱+q͓U4oٺ^ Σ;~2fvl*T;oGDxim;Oo815վF_3==bHNp=_`s2kBk-cAU=v_ܱ',V KδԽPu=2[d/a#g&8‰Matlab/xdVecGen00.m0100755000175400010010000000052510667536524013666 0ustar Iyad ObeidNonefunction xdVec = xdVecGen00(nSteps,nLaps) r = 35; theta = []; for i = 1:nLaps if ~iseven(i) theta = [theta linspace(.01*pi,.95*pi,nSteps)]; else theta = [theta linspace(.95*pi,.01*pi,nSteps)]; end end xdVec(:,1) = r*cos(theta); xdVec(:,2) = r*sin(theta); function y = iseven(x) y = (mod(x,2)==0); Matlab/xdVecGen01.m0100755000175400010010000000047410604171610013650 0ustar Iyad ObeidNonefunction xdVec = xdVecGen01(nSteps,nLaps) theta = pi/4; r = []; for i = 1:nLaps if iseven(i) r = [r linspace(2.9,1.1,nSteps)]; else r = [r linspace(1.1,2.9,nSteps)]; end end xdVec(:,1) = r*cos(theta); xdVec(:,2) = r*sin(theta); function y = iseven(x) y = (mod(x,2)==0); Matlab/xdVecGen02.asv0100755000175400010010000000032510604171610014201 0ustar Iyad ObeidNonefunction xdVec = xdVecGen02(nSteps,nLaps) r = 1.5; theta = [linspace(.25*pi,.75*pi,nSteps) ones(1,100)*.75; xdVec(:,1) = r*cos(theta); xdVec(:,2) = r*sin(theta); function y = iseven(x) y = (mod(x,2)==0); Matlab/xdVecGen02.m0100755000175400010010000000022610604171610013644 0ustar Iyad ObeidNonefunction xdVec = xdVecGen02(nSteps,nLaps) r = 1.5; theta = linspace(.25*pi,.75*pi,nSteps); xdVec(:,1) = r*cos(theta); xdVec(:,2) = r*sin(theta); Matlab/xErr.m0100755000175400010010000000017510604171610012722 0ustar Iyad ObeidNonefunction [e,eTot] = xErr(x,xd) % e = sqrt(sum(x-xd).^2); % i now think this is wrong e = x - xd; eTot = sqrt(sum(e.^2)); ServoCenter_gcc.c0100755000175400010010000000664410064575760013670 0ustar Iyad ObeidNone /***********************************************************\ * This demo program illustrates how to move servo motors * * using raw serial communication access to the * * Yost Engineering, Inc. ServoCenter 3.1 controller board. * * in Linux. This code was compiled using gcc. * * (c) 2001-2004 Yost Engineering, Inc. * * www.YostEngineering.com * * * \***********************************************************/ #include #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; tcgetattr(*fd,&options); switch(baud) //note: the termios structure does not support a baud rate of 14400 { 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) { unsigned char buffer[6]; int num; buffer[0]=(unsigned char)boardnum%16 + 0xf0; buffer[1]=0x10; buffer[2]=(unsigned char)servonum%16; buffer[3]=(unsigned char)position%200; buffer[4]=(unsigned char)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-199)\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); } brainControl.c0100644000175400010010000000662510667625010013236 0ustar Iyad ObeidNone#include "global.h" void moveservo(int,int); void b(double x,double y,int flag){ double num,den; double theta; double l1,l2; if (flag==0){ l1 = l1Est; l2 = l2Est; } else{ l1 = l1Actual; l2 = l2Actual; } num = pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2); den = 2*l1*l2; m2 = acos(num/den); num = l2 * sin(m2); den = l1 + l2*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.servoMax){ *servoPos = servo.servoMax; flag = 2; } return(flag); } void driveServo(int errFlag){ int servoPos1, servoPos2; // 0 - 199 servoCenter position int extremaFlag[2]; extremaFlag[0] = angle2servo(m1,servo1,&servoPos1); extremaFlag[1] = angle2servo(m2,servo2,&servoPos2); if (errFlag) dispServoErr(extremaFlag); //need to see what happens if we execute m2 //command before m1 command is done moveservo(SERVO_M1,servoPos1); moveservo(SERVO_M2,servoPos2); } void dispServoErr(int *ef){ int i; for (i=0;i<2;i++){ if (ef[i]==1) printf("Servo %i - min value reached\n",i+1); if (ef[i]==2) printf("Servo %i - max value reached\n",i+1); } } 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 } void calibrate_servos(){ // initialize the calibration constants for servo 1 servo1.servoMin = 5; servo1.servoMax = 199; servo1.degreesMin = -2.5; servo1.degreesMax = 176; // initialize the calibrationc constants for servo 2 servo2.servoMin = 5; servo2.servoMax = 199; servo2.degreesMin = -44; servo2.degreesMax = 141; } brainControl.o0100644000175400010010000001267510667625571013270 0ustar Iyad ObeidNoneLt7.text` `.data@.bss.rdata @@UhE]E]}u]]]] \$E$] \$E$E] \$E$E] \$E$m]EM]Eu$$E]$ME]Eu$]Eu$mEw(Ew((w0U(D$8$;E}u$FED$ D$D$E$E$E$ED$ D$D$E$ED$ D$D$E$ED$ ED$D$$ED$ ED$D$$E$EEEU(X\$E$$]X\$E$$E$]EUS4EE;|E$gw`\$E$$]`\$E$$E]؋E؋U܁EU`\$$}$EEE EE;|mE E E E E놃4[]U%%]UWVSEE;|E4=E E   7E4=E E   7EZ[^_]U$h]$pE$h]$pEUS$EEx]EeE$e]EEe E ]],E$}Ef fEmmE,E wE,E mmEE,EwE,EmmEE$[]UHED$$D$D$D$ D$D$D$D$D$ $EED$$D$D$D$ D$D$D$D$D$ ${E}t E$(ED$$ED$$nUE}~UEE<uE@D$$EE<uE@D$$EUS4EEEEUU܃}yEEU)‰ЈE] y)ȍ)É؈EE(ED$ED$$E4[]U]Ð8@3@8@fffff1@@-DT! @-DT!@rbinitG5.binno such file! @@8@fffff1@4C-DT! @Servo %i - min value reached Servo %i - max value reached @h@f@Fa@#.7@O6Xg6u665432217?SYagm~10/.. ,/N/iq//-6 6#,BZl666+ 4<L`o{ !"#$,;A%J!br%$334 4!,"EM*& &&&&&/&8&A&Kb'k't'}'''''&+0MR0() &  &  & $ &* 0 '6 < 'B H 'N T '.filegbrainControl.c_b  _dist _g/  _updateW _p 2 ( 4 F_ Q .textZ .data.bss.rdata_m2_m1_c_pf_xc_yc_w1_w2_xa_xd_xerr_ya_yd_yerr_beta_servo1_servo2__write _floor _exp _sqrt _fclose _malloc _fread _printf _fopen _atan _cos _sin _acos _pow k_defineRBF_computeErr_angle2servo_driveServo_calibrate_servos_moveservo_dispServoErr_fServoPortglobal.h0100644000175400010010000000160310667625532012047 0ustar Iyad ObeidNone#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; typedef struct { double servoMin; double servoMax; double degreesMin; double degreesMax; } servoType; 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 servoType servo1,servo2; extern int fServoPort; #define SERVO_POS_MIN 5 #define SERVO_POS_MAX 195 #define PORTNUM 2 #define SERVO_M1 0 #define SERVO_M2 1 #define SERVO_BOARD 0 #define SERVO_SPEED 40 #define BAUD 9600 init.h0100644000175400010010000000105710667624266011560 0ustar Iyad ObeidNoneextern void b(double,double,int); 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,servoType,int *); extern int open_port(); extern void driveServo(); extern void calibrate_servos(); extern void dispServoErr(int *); 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; servoType servo1, servo2; int fServoPort; initG5.bin0100644000175400010010000000042410665337042012261 0ustar Iyad ObeidNonerzӛ@fv @UgP? g%v`ȿv`?UgP? g%v`ȿv`?%~F @P? g%v`ȿv`?%~F @@@@@@@@@@@???????????mainSim.c0100644000175400010010000000222410667612223012170 0ustar Iyad ObeidNone#include "global.h" #include "init.h" #include 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,0); driveServo(); p(); usleep(33333); computeErr(); updateW(); } } } fclose(fDesiredPositions); fclose(fOut); free(c.c1); free(c.c2); free(w1); free(w2); free(pf); printf("Simulation Complete\n"); return(0); } outData.bin0100644000175400010010000000310010667566477012537 0ustar Iyad ObeidNone4@8@k.oA@N%yqA@9|r.?@2jnA@.6@,dA@ ]@5/EUA@@e 4BA@C+{@@)A@uC@5@[jv A@ Hs @Vvד@@cw"@7FZ@@׃ԙ$@cu@@$p7&@/Vo@@(z(@ Gp>@@n?*@F· @@8,@2*z_?@'l.@z7H+?@BP9&0@\ d>@8h1@ח2>@11@P=@,Z2@Z~=@q3@IABUr<@9E4@N~-f;@9!Yq5@<% (;@wBRC6@Icn_y:@w4X7@9@M/7@( 9@0)8@dyM8@wߊV9@W7@Pp0lY :@]X6@lpw:@i6@7s}M;@#4+5@w';@1gQO4@ˇ5,<@o3@#i#J:=@d۹52@=@~1@+MZ>@m#0@>@ܓ:p/@pX\?@ǔʳ-@L$?@N4ި+@Gf;: @@JHi)@S@@tHn'@q=@@, %@&ݹ$ @@#@Ce@@=!@~O'@@VL@gdA@%;y@/7A@_T'-}@:!PA@-Y@WXF3dA@F0A@sTī^'A@d%QЩA@hTA@y [@ZI2 Br=@e,3Azg=@rɌ 4''<@r4ȅ5<@hZB̈́5D;@d6Ţ.:@hX97K3>:@J >|8?Vh9@=j8D M8@(N9:䛕 8@p7C:4mkG7@"np:#{6@Ċ,;EROo5@>64[B<m4@^ !v <֪3@Ha߁>=4%u-3@,MC==ӪFG2@ Cm>W]1@>H7p0@ޡؿv?JCD.@'T\?6=a-@\.@++@Sϝb@7)@Eua@*|o?'@'7@mqA%@h@A:?#@:D<A-19!@MV&A)Z]@ҾAA,A@servoSetup.c0100644000175400010010000000261110667334523012756 0ustar Iyad ObeidNone#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(); } sleeptest.c0100755000175400010010000000071210667546055012617 0ustar Iyad ObeidNone#include #include #include int main(){ int i; unsigned long int TDELAY; float foo; int nSecs = 10; int fps = 30; int nFrames = nSecs * fps; foo = (float)1000000/fps; printf("float = %f\n",foo); TDELAY = 33333; printf("starting, TDELAY = %d\n",TDELAY); for (i=0;i LX4e___main@ __alloca_xau@ _freadp h_fopen _calloc` __fmode$H_acos $_reallocp 7EZf_exp0 r0__end__l_malloc` ,_fcloseP , _perror _sqrt@ $_abort _xc_m2+GT_cos Tk_pow _sin x_w2_write _c(_xd `p _beta#?_yerr0W_pf@_servo2P_printf du8__open64_floor  _mainCRTStartup___do_sjlj_init_defineRBF_computeErr_angle2servo_driveServo_calibrate_servos_moveservo_dispServoErr_open_port_init_port_initServoPort_get_mode_w32_atom_suffix___w32_sharedptr_default_unexpected_dw2_object_mutex.0_sjl_fc_key.1_sjl_once.2___w32_eh_shared_initialize___w32_sharedptr_initialize___w32_sharedptr_set___w32_sharedptr_get___w32_sharedptr_fixup_after_fork_cygwin_crt0__cygwin_crt0_common@8_do_pseudo_reloc__pei386_runtime_relocator_cygwin_premain3_cygwin_premain2_cygwin_premain1_cygwin_premain0___sjlj_init_ctor__imp__floor___RUNTIME_PSEUDO_RELOC_LIST____imp__tcgetattr___cygwin_crt0_bp__data_start_____DTOR_LIST___tcgetattr__nm___impure_ptr__imp__cos___w32_sharedptr_terminate___tls_start___dll_crt0__FP11per_process__imp__FindAtomA@4__imp__abort__imp__sin__size_of_stack_commit____size_of_stack_reserve____major_subsystem_version_____crt_xl_start___AddAtomA@4___crt_xi_start_____chkstk___crt_xi_end____imp__open_GetModuleHandleA@4_tcsetattr__bss_start_____RUNTIME_PSEUDO_RELOC_LIST_END____imp__write__size_of_heap_commit____imp__exp___crt_xp_start_____crt_xp_end____minor_os_version____image_base____section_alignment____imp___impure_ptr_cygwin_internal__RUNTIME_PSEUDO_RELOC_LIST____data_end_____w32_sharedptr__CTOR_LIST____imp__sprintf__imp__perror__bss_end____imp__scanf___crt_xc_end_____crt_xc_start____imp__sqrt__fopen64___CTOR_LIST____imp__GetAtomNameA@12__imp__tcsetattr__imp__fread__imp___open64_cygwin1_dll_iname__head_cygwin1_dll__imp____main__file_alignment____imp__malloc__major_os_version____imp__atan__imp__acos__imp__realloc__imp__GetModuleHandleA@4__DTOR_LIST____imp__pthread_atfork__imp__fclose__size_of_heap_reserve_____crt_xt_start____subsystem____imp__calloc__imp__pow_fServoPort___w32_sharedptr_unexpected__imp__fopen__imp__cygwin_internal___tls_end____imp__dll_crt0__FP11per_process__imp___fopen64__imp__free__major_image_version____loader_flags____imp__printf__imp__AddAtomA@4_pthread_atfork__head_libkernel32_a__minor_subsystem_version____minor_image_version___FindAtomA@4_GetAtomNameA@12__RUNTIME_PSEUDO_RELOC_LIST_END____libkernel32_a_iname___crt_xt_end__xd00.txt0100644000175400010010000000374210667271532011755 0ustar Iyad ObeidNone34.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.c0100644000175400010010000000060310667271500012434 0ustar Iyad ObeidNone#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