function ODEsimPyriformisPhaseLag
% Simulates control using F-threshold method, a feedback controller for n
% cells.
%
% This simulation takes n robots, leanrs their models by rottating the
% magnetic field at a constant rate, and then uses a control-Lyapunov
% method to drive the cells to orbit their goal positions.
SHOW_MOVIE = true; %animation showing the magnetic field
close all
clc
global freq vel a running goal calibCenter FrameCount MOVIE_NAME MAKE_MOVIE
MAKE_MOVIE = 1;
FrameCount = 0;
MOVIE_NAME = 'sim4CellsToGoal';
running = 1; %if you close the figure with the movie, this cleanly shuts down the program
set(0,'defaultaxesfontsize',14);
set(0,'defaulttextfontsize',14);
format compact
N = 4; %number of robots
a = linspace(5,8,N)'; % a values, should be between 3 and 10
freq = 15; % as the frequency increases, the lines become straighter, but the differentiation decreases)
vel = 0.7; %mm/s (Estimate by Yan Ou) 700 \mum/s
tCal = 15;
tF = 200; %final time
%initial conditions for cells (x,t,theta)
x = 10*ones(N,1);
y = 5+ones(N,1).*5/N.*(0:N-1)'; %spaces cells vertically
th = 0*ones(N,1); % performance is very dependent on initial orientation for large amplitude
phase = 0; % phase of magnetic frequency
xc = zeros(N,1); %x value, center of rotation
yc = zeros(N,1); %y value, center of rotation
options = odeset('RelTol',1e-4,'AbsTol',1e-4 );
%first find the center of rotation
calibCenter = true;
Dt = 1/30;
[T0,Y0] = ode45(@simPyriformis,[0:Dt:0.1],[x;y;th;phase;xc;yc],options); %start rotating
[T1,Y1] = ode45(@simPyriformis,[0.1:Dt:tCal],Y0(end,:)',options); %calibrate
%first find the center of rotation
calibCenter = false;
x1 = Y1(3:end,1+0*N:1*N);
y1 = Y1(3:end,1+1*N:2*N);
th1 = Y1(3:end,1+2*N:3*N);
phase1 = Y1(3:end,3*N+1);
xcenter = (max(x1)+min(x1))/2;
ycenter = (max(y1)+min(y1))/2;
%set the center locations for initial data so Lyapunov looks right
Y0(:,3*N+1 + (1:N)) = repmat(xcenter,size(Y0(:,3*N+1 + (1:N)),1),1);
Y0(:,4*N+1 + (1:N)) = repmat(ycenter,size(Y0(:,3*N+1 + (1:N)),1),1);
Y1(:,3*N+1 + (1:N)) = repmat(xcenter,size(Y1(:,3*N+1 + (1:N)),1),1);
Y1(:,4*N+1 + (1:N)) = repmat(ycenter,size(Y1(:,3*N+1 + (1:N)),1),1);
[T2,Y2] = ode45(@simPyriformis,[tCal:Dt:tF],[x1(end,:)';y1(end,:)';th1(end,:)';phase1(end,:);xcenter';ycenter'],options);
T = [T0;T1;T2];
Y = [Y0;Y1;Y2];
%%%%%%%%%%%%%%%%%%%%%%% PLOTTING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%draw path, cells
figure(1)
clf
%c = ['r','g','m','y','k','c','b'];
c = distinguishable_colors(N+1);
for i = 1:N
hold on
plot(Y(:,i),Y(:,i+N),'-','color',c(i,:))
text(Y(1,i),Y(1,i+N)+1,['\it a \rm = ',num2str(a(i))])
end
axis equal
title({[num2str(N),' cells simulated for ',num2str(tF),' s'];});%['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t)']});
xlabel('mm')
ylabel('mm')
for i = 1:N
drawCell(Y(end,i),Y(end,i+N),Y(end,i+2*N),a(i),0.05);
end
ax = axis;
hold off
figure(3) %plot sum squared error
r = vel/(freq); % radius of circle the robot is turning around
%(xc,yc) is the center of the circle the robot is following
th = Y(:,1+2*N:3*N);
%xc = Y(:,1+0*N:1*N) -r*cos(th-pi/2);
%yc = Y(:,1+1*N:2*N) -r*sin(th-pi/2);
%sum((Y(:,1:nRob).^2 + Y(:,nRob+1:2*nRob).^2).^.5,2)./nRob;
xc = Y(:,3*N+1 + (1:N));
yc = Y(:,4*N+1 + (1:N));
for i = 1:N
plot( T, ((xc(:,i)-goal(1)).^2+(yc(:,i)-goal(1)).^2).^1,'linewidth',1,'color',c(i,:));
hold on
end
plot( T, sum(((xc-goal(1)).^2+(yc-goal(1)).^2).^1,2),'linewidth',2,'color',c(N+1,:))
%plot( T, sum(((xc).^2+(yc).^2).^.5,2))
xlabel('time')
ylabel('sum squared error')
title('Lyapunov Function')
axLyp = axis;
crec = rectangle('Position',[0,0,tCal,2000],'facecolor',[.8,.8,.8]);
uistack(crec,'bottom')
axis(axLyp);
text(tCal/2,axLyp(4)/2,'Calibration','Rotation',90,'VerticalAlignment','middle','HorizontalAlignment','center')
% figure(4)
% plot( T, Y(:,end),'.')
% xlabel('time')
% ylabel('phase integrator')
% figure(5)
%
% %F = -sum( vel.*(xerr.*cos(th)+yerr.*sin(th))); %gradient of the distance
% % if F < -1/2*sum(vel.*(xerr.^2+yerr.^2).^.5,2) --> M=0; %sum of the distances F>0 is bad!
%
% plot( T, -sum(vel.*((xc-goal(1)).*cos(th)+(yc-goal(1)).*sin(th)),2),'linewidth',2,'color',c(N+1,:))
% hold on
% plot( T, -1/2*sum(vel.*(((xc-goal(1)).^2+(yc-goal(1)).^2).^.5),2),'linewidth',1,'color','b')
% xlabel('time')
% legend('F','thresh')
% ylabel('Lyapunov Derivative')
% title('Lyapunov Derivative')
% hold off
if ~SHOW_MOVIE
return
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% MAKE A MOVIE! %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Setup figures %%%%%%%%%
figure(2);
set(gcf,'CloseRequestFcn',@F2Close);
clf
hpath = zeros(N,1);
hpathOld = zeros(N,1);
hCell = cell(N,1);
for i = 1:N
%start pos
plot(Y(1,i),Y(1,i+N),'x','color', c(i,:))
hold on
hpath(i) = plot(Y(1,i),Y(1,i+N),'-','color', c(i,:),'linewidth',1);
hpathOld(i) = plot(Y(1,i),Y(1,i+N),':','color', c(i,:),'linewidth',1);
ht =text(Y(1,i),Y(1,i+N)+1,['\it a \rm = ',num2str(a(i))]);
uistack(ht,'top')
plot(goal(i,1),goal(i,2),'+','color','w','linewidth',3);
plot(goal(i,1),goal(i,2),'+','color', c(i,:),'linewidth',2);
end
for i = 1:N
%draw cells
hCell{i} = drawCell(Y(1,i),Y(1,i+N),Y(1,i+2*N),a(i),0.05);
end
hTitle = title({[num2str(N),' cells simulated for ',num2str(tF),' s']});%;['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t), T = ',num2str(T(1)),' s']});
Magsc = 1;
MagRad = max(ax(2)-ax(1),ax(4)-ax(3))/2;
MagOrigin = [ (ax(2)+ax(1))/2, (ax(4)+ax(3))/2];
%thetaM = Mag*sin(freq*T(1));
%hMag = drawMagnet(thetaM,MagRad,Magsc,MagOrigin);
axis equal
ax(1) = ax(1)-2*Magsc;
ax(2) = ax(2)+2*Magsc;
ax(3) = ax(3)-2*Magsc;
ax(4) = ax(4)+2*Magsc;
axis(ax);
%hMagLine = zeros(6*N,1);
%hMagLineAr = zeros(6*N,1);
%magcolor = [.5,.5,1];
% for i = 1:numel(hMagLine)
% %draw lines
% hMagLine(i) = plot( MagOrigin(1)+[-3*MagRad,3*MagRad],...
% MagOrigin(2)+(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)*[1,1],'Color',magcolor);
% uistack(hMagLine(i),'bottom')
% hMagLineAr(i) = patch( MagOrigin(1)+[0,0,Magsc],...
% MagOrigin(2)+(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)+[-Magsc/4,Magsc/4,0],magcolor);
% set(hMagLineAr(i),'edgecolor',magcolor);
% uistack(hMagLineAr(i),'bottom')
% end
MagredX = [0,5,0];
MagredY = [-1,0,1]/4;
MagwhtX = [0,-5,0];
MagwhtY = [-1,0,1]/4;
magO = [-5,10];
hTime = text(-5,-4,{'\it t \rm=',num2str(T(1),'%3.0f'),' s'});
text(magO(1)+2,magO(2)+2,{'Magnetic';'Orientation'})
hMagR = patch(magO(1)+MagredX,magO(2)+MagredY,'r','linewidth',2);
hMagW = patch(magO(1)+MagwhtX,magO(2)+MagwhtY,'w','linewidth',2);
text(magO(1)+2,magO(2)+2,{'Magnetic';'Orientation'})
%%%% show movie frames %%%%%%%%%%%%%%%%%%%
for j = 2:numel(T)
if ~running
break
end
set(hTitle,'string',{[num2str(N),' cells, \it t \rm=',num2str(T(j),'%3.0f'),' s']});%['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t), T = ',num2str(T(j), '% 5.0f'),' s']});
for i = 1:N
set(hpath(i),'Xdata',Y(max(1,j-50):j,i),'Ydata',Y(max(1,j-50):j,i+N));
set(hpathOld(i),'Xdata',Y(1:j,i),'Ydata',Y(1:j,i+N));
updateCell(hCell{i},Y(j,i),Y(j,i+N),Y(j,i+2*N));
end
%thetaM = Mag*sin(freq*T(j));
thetaM = Y(j,3*N+1);
set(hTime,'String',['\it t \rm=',num2str(T(j),'%3.0f'),' s']);
set(hMagR,'Xdata',magO(1)+MagredX*cos(thetaM)-MagredY*sin(thetaM),...
'Ydata',magO(2)+MagredX*sin(thetaM)+MagredY*cos(thetaM));
set(hMagW,'Xdata',magO(1)+MagwhtX*cos(thetaM)-MagwhtY*sin(thetaM),...
'Ydata',magO(2)+MagwhtX*sin(thetaM)+MagwhtY*cos(thetaM));
% %updateMagnet(hMag, thetaM);
% for i = 1:numel(hMagLine)
% if( Y(j,3*N+1) == Y(j-1,3*N+1))
% mcol = [1,1,1];
% else
% mcol = magcolor;
% end
% %draw lines
% xmag = [-3*MagRad,3*MagRad];
% ymag = (i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)*[1,1];
% set(hMagLine(i),'Color',mcol,'Xdata',MagOrigin(1)+xmag*cos(thetaM)-ymag*sin(thetaM), 'Ydata',MagOrigin(2)+xmag*sin(thetaM)+ymag*cos(thetaM));
%
% xar=1+[0,0,Magsc];
% yar=(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)+[-Magsc/4,Magsc/4,0];
% set(hMagLineAr(i),'faceColor',mcol,'edgecolor',mcol,'Xdata',MagOrigin(1)+xar*cos(thetaM)-yar*sin(thetaM), 'Ydata',MagOrigin(2)+xar*sin(thetaM)+yar*cos(thetaM));
%
% end
axis(ax);
updateDrawing;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% END MOVIE! %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
function F2Close(~,~)
global running
running = 0;
delete(gcf);
end
function dq = simPyriformis(~,q)
% simulates a differential equation model for the cells
% q = [x_coordinates; y_coordinates; theta_coordinates,
% magnetic_field_orientation]
global freq vel a goal calibCenter
%thetaM = Mag*sin(freq*t);
M = 1; %magnetic field, 1= on, 0 = off
goal = [0,0;
0,5;
5,0;
5,5]; %desired point we want cells to circle around
dq = zeros(size(q)); % a column vector for the change in state
N = (numel(q)-1)/5; %number of robots
%r = vel/freq; % radius of circle the robot is turning around
th = q(2*N+1:3*N); %current orientations of the cells
%sum((Y(:,1:nRob).^2 + Y(:,nRob+1:2*nRob).^2).^.5,2)./nRob;
% trans = -sum(eps*(x*cos(theta) + y*sin(theta)))/unicycles, where x and
% y are the distance to each robot's goal
xc = q(3*N+1 + (1:N));%-r*cos(th-pi/2); %(xc,yc) is the center of the circle the robot is following
yc = q(4*N+1 + (1:N));%-r*sin(th-pi/2);
%goal = [mean(xc),mean(yc)]; %to collect all robots together (clumping/rendezvous)
xerr = goal(:,1)-xc; %error between goal position and current circle center
yerr = goal(:,2)-yc;
F = -sum( vel.*(xerr.*cos(th)+yerr.*sin(th))); %gradient of the distance
% TODO: what is the best threshold? do a test and iterate to find the best
% value
if (calibCenter == false) && (F < -1/4*sum(vel.*((xerr.^2+yerr.^2).^.5),1)) %sum of the distances F>0 is bad!
M=0; %go straight if the gradient is large (and negative)
end
thetaM = q(3*N+1); % magnetic field direction
dq(1:N) = vel*cos(th);
dq(N+1:2*N) = vel*sin(th);
angDif = thetaM-th;
dq(2*N+1:3*N) = M*a.*sin(angDif);
dq(3*N+1) = freq*M; %only update the magnetic field orientation if the magnetic field is on.
if M==0
dq(3*N+1 + (1:N)) = vel*cos(th);
dq(4*N+1 + (1:N)) = vel*sin(th);
end
end
function updateDrawing
global FrameCount MOVIE_NAME MAKE_MOVIE
set(gcf, 'position',[-98 57 1774 965]);
drawnow
if(MAKE_MOVIE)
FrameCount=FrameCount+1;
F = getframe(gca);
fname = sprintf('img/%s%06d.png',MOVIE_NAME,FrameCount);
imwrite(F.cdata, fname,'png');
while(FrameCount < 10)
updateDrawing
end
end
end