How calculate period for a discontinuous system using fix point iteration?

Question

I have dynamic system that performs a periodic motion. It is a woodpecker hammering down a rod and its motion is described by two states depending if some constraints are activated or not. These states are

State I :

$$(I_2+m_2b^2)\ddot{\theta}=-c\theta+m_2bg_{gr}\tag{1}$$

State II :

$$(I_2+m_2b^2(1-\frac{m_2}{m_1+m_2}))\ddot{\theta}=-c\theta\tag{2}$$ $$(m_1+m_2)\ddot{z}+m_2b\ddot{\theta}=(m_1+m_2)g_{gr}\tag{3}$$

Now, when I simulate this in Matlab I get (after the transient part dies out) a periodic solution (this is indicated by the phaseplot below).

But how do I calculate its period? I dont want to measure the period graphically, instead I want to use fixpoint iteration. But the system is discontinuous and all that, so it seems really complicated. However, my professor gave me this advice:

The system is decribed by an explicit ODE

$$\dot{y}=f(y)$$ where $$y=(\theta, \dot{\theta},z,\dot{z})^T$$ is the statevector.

We wish to calculate the period $T$:

$$y(t;y_0),\quad y(0;y_0)=y_0,\quad y(T;y_0)=y_0$$

We define the function $F$ as:

$$F(y_0)\equiv y(T;y_0)-y_0 \iff F(y_0)=0$$

which is a nonlinear system of equations. Now, since we don't know T, which is what we wish to find out, we cannot solve it. His approach was to make the following transformation

$$t\in[0,T]\mapsto s\in[0,1],\quad s=\frac{t}{T}$$

Then we can present nonlinear system of equations in a different form

$$\frac{dy}{ds}=\frac{dy}{dt}\frac{dt}{ds}=Tf(y),\quad s\in [0,1]$$

$$\bar{y}\equiv (y,T)^T \Rightarrow \frac{d\bar{y}}{ds}=(Tf(y),0)^T=g(\bar{y})$$

Finally we arrive at a nonlinear system of equations independent of $T$

$$G(y_0)=\bar{y}(1;\bar{y}_0)-\bar{y}_0 \iff G(y_0)=0$$

I am not quite sure why we need to differentiate $\bar{y}$. My quesetion is how I implement this method numerically? I dont'quite understand how fix point iteration works in this case. Do I guess a initial value $\bar{y_0}$ and then $\bar{y}(1;\bar{y}_0)-\bar{y}_0$ becomes the new guess that I insert which brings me closer to the fixed point. Then I do a backtransformation to obtain the period $T$ using the expression $T=t/s$. In that case, what is $t$?

I hope I am being coherent in my questions. Best regards

Edit: Regarding the last question what $t$ is, this is ofcourse the period since $s$ is 1.

Edit 2:

So, for my system to complete one cycle/period it goes through different stages, and every stage yields a new set of equations to be solved. So if I treat this as a boundary value problem, the trouble becomes that the integration process must be terminated and started several timed during the procedure. Can a BVP solver handle this?

The cycle goes like this:

Stage 1: Solve State I for arbitrary intialvalue $y_0$.

Stage 2: Solve Sate II for initialvalue $y_1$.

Stage 3: Solve State I for initialvalue $y_2$.

Stage 4: Solve State I for initial value $y_3$.

Stage 5: Solve State II for initial value $y_4$.

When Stage 5 terminates, then the cycle is complete.

So how do I implement the idea of this being a boundaary value problem when it is discontinous like this?

Edit 3:

CODE

%General constants
I2 = 7*10^(-7); % [kgm^2]
m1 = 0.003; % [kg]
m2 = 0.0045; % [kg]
b = 0.015; % [m]
c = 0.0056; % [Nm]
d1 = 0.18335;
d3 = 0.04766;
g = 9.81; % [m/s^2]
K1 = 0.174532925; % [rads]
K2 = 0.20943951; % [rads]
p = [I2 m1 m2 b c d1 d3 g K1 K2]; % parametervector

% Constants to the function firstODE
A1 =  I2 + m2*b^2;
B1 = m2*b*g;

% Constants to the function secondODE
A2 = I2 + m2*b^2*(1-m2/(m1+m2));
B2 = m1 + m2;
C2 = m2*b;
D2 = (m1 + m2)*g;

IC = [K1+0.01 15.2161 0 0]; % Initial Condition

tstart = 0;
tfinal = 10;

y0 = IC;
options = odeset('Events',@myEventsFcn);
tout = tstart;

% Lists for different statevariables output
thout = y0(1:1);
thvout = y0(2:2);
zout = y0(3:3);
zvout = y0(4:4);

ieout = [];
yeout = [];
if isempty(ieout)
    if y0(1)>K1
        ieout = [ieout 5];
    elseif  K1>y0(1) && y0(1)>-K1 && y0(2)<0
        ieout = [ieout 1];
    elseif -K1>y0(1) && y0(1)>-K2 && y0(2)<0
        ieout = [ieout 2];
    elseif -K1>y0(1) && y0(1)>-K2 && y0(2)>0
        ieout = [ieout 3];
    elseif K1>y0(1) && y0(1)>-K1 && y0(2)>0
        ieout = [ieout 4];
    end
end

% Main program
for i = 1:25 % Change this to 5 for one cycle (sometimes 4 will do, as long as it is one period)
    event = ieout(end);
    switch event
        case 1
            [t,y,te,ye,ie] = ode45(@(t,y) secondODE(t,y,A2,B2,C2,D2,c),[0 tfinal],y0,options);
            [thout,thvout,zout,zvout,tstart,tout]=Update(y,t,thout,thvout,zout,zvout,tstart,tout,2);
            ie = ie(end);
            ieout = [ieout ie];
            ye=ye(end,:);
            y0 = ICgenerate(ye,ie,p);
        case 2
            y00 = [y0(1) y0(2)];
            [t,y,te,ye,ie] = ode45(@(t,y) firstODE(t,y,A1,B1,c),[0 tfinal],y00,options);
            [thout,thvout,zout,zvout,tstart,tout]=Update(y,t,thout,thvout,zout,zvout,tstart,tout,1);
            ie = ie(end);
            ieout = [ieout ie];
            ye = [ye(end,:) zout(end) zvout(end)];
            y0 = ICgenerate(ye,ie,p);
        case 3
            y00 = [y0(1) y0(2)];
            [t,y,te,ye,ie] = ode45(@(t,y) firstODE(t,y,A1,B1,c),[0 tfinal],y00,options);
            [thout,thvout,zout,zvout,tstart,tout]=Update(y,t,thout,thvout,zout,zvout,tstart,tout,1);
            ie = ie(end);
            ieout = [ieout ie];
            ye = [ye(end,:) zout(end) zvout(end)];
            y0 = ICgenerate(ye,ie,p);
        case 4
            [t,y,te,ye,ie] = ode45(@(t,y) secondODE(t,y,A2,B2,C2,D2,c),[0 tfinal],y0,options);
            [thout,thvout,zout,zvout,tstart,tout]=Update(y,t,thout,thvout,zout,zvout,tstart,tout,2);
            ie = ie(end);
            ieout = [ieout ie];
            ye = ye(end,:);
            y0 = ICgenerate(ye,ie,p);
        case 5
            y00 = [y0(1) y0(2)];
            [t,y,te,ye,ie] = ode45(@(t,y) firstODE(t,y,A1,B1,c),[0 tfinal],y00,options);
            [thout,thvout,zout,zvout,tstart,tout]=Update(y,t,thout,thvout,zout,zvout,tstart,tout,1);
            ie = ie(end);
            ieout = [ieout ie];
            ye = [ye(end,:) zout(end) zvout(end)];
            y0 = ICgenerate(ye,ie,p);
    end
end

k1 = repmat(K1,1,length(tout));
k2 = repmat(-K2,1,length(tout));

% This section gives different plots. Remember to comment out what you
% don't want to plot!

plot(tout,thout)
hold on
plot(tout,k1)
hold on
plot(tout,-k1)
hold on
plot(tout,k2)

legend('\theta(t)','K1','-K1','-K2')
title('Vinkelutslag kontra tid')
xlabel('tid')
ylabel('vinkel')

% plot(tout,thvout)
% legend('d\theta/dt')
% title('Vinkelhastighet kontra tid')
% xlabel('tid')
% ylabel('vinkelhastighet')

% plot(thout,thvout)
% legend('Faskurva')
% title('Fasporträtt med B.V. (0.175532925, 15.2161, 0, 0)')
% xlabel('\theta')
% ylabel('d\theta/dt')

% plot(tout,zout)
% legend('z(t)','Location','northwest')
% title('Position i z-led kontra tid')
% xlabel('tid')
% ylabel('z-koordinat')

% plot(tout,zvout)
% legend('dz/dt(t)','Location','northwest')
% title('Fallhastighet kontra tid')
% xlabel('tid')
% ylabel('hastighet i z-led')

% plot(thout,thvelout) % lägg till y0 då i=5 för fullständig plott
% thout = [thout y0(1)]; % y0(1)
% thvelout = [thvelout y0(2)]; % y0(2)
% plot(thout, thvelout)

function [thout,thvout,zout,zvout,tstart,tout] =     Update(y,t,thout,thvout,zout,zvout,tstart,tout,i)

    % This function updates the variables in a correct way since the
    % degrees of freedom change, thus some variables are constant or zero.
      nt = length(t);
      t_new = t(2:nt)' + tstart;
      tout = [tout t_new];
      tstart = tstart + t(end);

      thout = [thout y(2:nt,1)']; 
      thvout = [thvout y(2:nt,2)'];

      if i == 1 %first ODE
          zstat = repmat(zout(end),1,nt-1);
          zout = [zout zstat];
          zvstat = zeros(nt-1,1);
          zvout = [zvout zvstat'];

      elseif i == 2 %second ODE
          zout = [zout y(2:nt,3)'];
          zvout = [zvout y(2:nt,4)'];

      end
end

function dy = firstODE(t,y,A1,B1,c)
dy = zeros(2,1);
dy(1) = y(2);
dy(2) = 1/(A1)*(B1-c*y(1));
end


function dy = secondODE(t,y,A2,B2,C2,D2,c)
dy = zeros(4,1); 
dy(1) = y(2);
dy(2) = -c/A2*y(1);
dy(3) = y(4);
dy(4) = D2/B2 + c*C2/(A2*B2)*y(1);
end

function [value,isterminal,direction] = myEventsFcn(t,y)
K1 = 0.174532925; % 0.174532925 rads
K2 = 0.20943951; % 0.20943951 rads
value = [y(1)-K1, y(1)+K1, y(1)+K2, y(1)+K1, y(1)-K1];
isterminal = [1, 1, 1, 1, 1];
direction = [-1, -1, -1, 1, 1];
end

function y0 = ICgenerate(ye,ie,p)
% Generates new Initial Conditions based on which event happend

thetapos = ye(1);
thetavel  = ye(2);
zetapos = ye(3);
zetavel = ye(4);
if ie == 1
    tpos = thetapos;
    tvel = thetavel;
    zpos = zetapos;
    zvel = 0;
    y0 = [tpos tvel zpos zvel];
end
if ie == 2
    tpos = thetapos;
    tvel = (1-p(7))*(thetavel+p(3)*p(4)/(p(1)+p(3)*p(4)^2)*zetavel);
    zpos = zetapos;
    zvel = 0;
    y0 = [tpos tvel zpos zvel];
end
if ie == 3
    tpos = thetapos;
    tvel = -thetavel;
    zpos = zetapos;
    zvel = 0;
    y0 = [tpos tvel zpos zvel];
end
if ie == 4
    tpos = thetapos;
    tvel = thetavel;
    zpos = zetapos;
    zvel = 0;
    y0 = [tpos tvel zpos zvel];
end
if ie == 5
    tpos = thetapos;
    tvel = (1-p(6))*(thetavel+p(3)*p(4)/(p(1)+p(3)*p(4)^2)*zetavel);
    zpos = zetapos;
    zvel = 0;
    y0 = [tpos tvel zpos zvel];
end   
end