clear all
% close all
clc
format long g
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Rb = 0.040; %radius of basic circle [m]
omega = 1500*2*pi/60; %rotating speed [rad/s]
h = 0.004; %total rise [m]
beta = 75*pi/180; % rise angle [rad]
N = 101; %steps 
theta = linspace(0,2*pi,N);
dth = 2*pi/(N-1);


% harmonic profile
for i = 1:N
    if 0 <= theta(i) && theta(i) <= beta
        s(i) = (h/beta)*theta(i)-(h/(2*pi))*sin(2*pi*theta(i)/beta);
        q(i) = (h/beta)-(2*h*pi*cos(2*pi*theta(i)/beta))/(2*pi*beta);
        sdot(i) = (h*omega/beta)*(1-cos(2*pi*theta(i)/beta));
        sddot(i) = (2*h*pi*omega^2/(beta^2))*sin(2*pi*(theta(i))/beta);
    end
    if beta < theta(i) && theta(i) < (2*beta)
        s(i) = -(h/beta)*(theta(i)-2*beta)+(h/(2*pi))*sin(2*pi*theta(i)/beta);
        q(i) = -(h/beta)+(2*h*pi*cos(2*pi*(theta(i)-2*beta)/beta))/(2*pi*beta);
        sdot(i) = -(h*omega/beta)*(1-cos(2*pi*(theta(i)-2*beta)/beta));
        sddot(i) = -(2*h*pi*omega^2/(beta^2))*sin(2*pi*(theta(i)-2*beta)/beta);
    end
    if (2*beta) <= theta(i) && theta(i)<=(2*pi)
        s(i) = 0;
        q(i) = 0;
        sdot(i) = 0;
        sddot(i) = 0;
    end
end

figure(1)
subplot(4,1,1)
plot(theta*180/pi,1000*s,'k-')
xlabel('Angle \theta [deg]')
ylabel('s [mm]')
grid on
xlim([0 360])

figure(1)
subplot(4,1,2)
plot(theta*180/pi,1000*q,'k-')
xlabel('Angle \theta [deg]')
ylabel('q [mm/rad]')
grid on
xlim([0 360])

figure(1)
subplot(4,1,3)
plot(theta*180/pi,1000*sdot,'k-')
xlabel('Angle \theta [deg]')
ylabel('sdot [mm/s]')
grid on
xlim([0 360])

figure(1)
subplot(4,1,4)
plot(theta*180/pi,1000*sddot,'k-')
xlabel('Angle \theta [deg]')
ylabel('sddot [mm/s^{2}]')
grid on
xlim([0 360])

rc = sqrt((Rb+s).^2 + q.^2);
hta = atan(q./(Rb+s));

psi_c = theta + hta; % hta  has positive (during rise) and negative (during return) values

% cam drawing
figure(2)
plot(psi_c*180/pi,1000*rc,'k-')
xlabel('Angle \psi [deg]')
ylabel('r_{c} [mm]')
grid on
xlim([0 360])

figure(3)
polar(psi_c,1000*rc,'k-')

m = 0.100; %follower mass [kg]
k = 5e3; %spring stiffness [N/m], selected by the designer
Fint = -m*sddot;
x0 = 20/k; %initial displacement of the spring to produce 20N
Fs = -k*(s+x0);
Fc = -Fs - Fint;

figure(4)
plot(theta*180/pi,Fint,'k:')
hold on
plot(theta*180/pi,Fs,'k--')
hold on
plot(theta*180/pi,Fc,'k-')
legend('F_{int}','F_{s}','F_{c}')
xlabel('Angle \theta [deg]')
ylabel('Forces [N]')
grid on
xlim([0 360])

M = Fc.*q;
figure(5)
plot(theta*180/pi,M,'k-')
xlabel('Angle \theta [deg]')
ylabel('Moment M(\theta) [Nm]')
grid on
xlim([0 360])

W_cycle = 0;
for i=1:length(M)
    W_cycle = W_cycle + M(i)*dth;
end

DE = 0;
for i=1:length(M)
    if M(i)>0
        DE = DE + M(i)*dth;
    end
end

%calculation of flywheel
Cs = 0.01;
I_f = DE/((omega^2)*Cs); %mass moment of inertia [kgm^2]
ro = 7860; %density of the steel flywheel [kg/m^3]
w = 0.01; %width of the flywheel [m]
R_f = sqrt(sqrt(2*I_f/(pi*w*ro))); %radius of the flywheel [m]
m_f = pi*(R_f^2)*w*ro; %mass of the flywheel [kg]

%calculation of the motor
Pc = Fc.*sdot; %Power transmitted from the cam to the follower
figure(6)
plot(theta*180/pi,Pc,'k-')
xlabel('Angle \theta [deg]')
ylabel('Power P_{c} [W]')
grid on
xlim([0 360])