% Modelling the IMU system
r = 0.5*.0254; %in

freq = 100; %Hz
t_step = 1/freq;

max_rpm = 5; %RPM
max_omega = max_rpm*pi/30;

alpha_rpm = 5/60;
alpha = alpha_rpm*pi/30;

t = 0:t_step:360; %Seconds
t = t(1:length(t)-2);

% Computing omega
omega(1:60/t_step) = alpha*t(1:60/t_step);
omega(60/t_step:120/t_step) = max_omega;
omega(120/t_step:240/t_step-1) = max_omega - alpha*t(1:120/t_step);
omega(240/t_step:300/t_step) = -max_omega;
omega(300/t_step:360/t_step-1) = -max_omega + alpha*t(1:60/t_step);
figure;
plot(t,omega);
title('Omega vs. Time');
xlabel('Time (s)');
ylabel('Omega (rad/s)');

% Computing centripetal acceleration
acc = (omega.^2)*r;
%acc(180/t_step:length(acc)) = -acc(180/t_step:length(acc));
figure;
plot(t,acc);
title('Centripetal acceleration vs. time');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');

% Computing tangential velocity
% vel_tan = omega*r;
% figure;
% plot(t,vel_tan);
% title('Tangential velocity vs. time');
% xlabel('Time (s)');
% ylabel('Velocity (m/s)');

% Numerically integrating centripetal accel
integrated_acc = cumtrapz(t, acc)*100;
figure;
plot(t, integrated_acc);
title('Integrated centripetal acceleration');
xlabel('Time (min)');
ylabel('Velocity? (cm/s)');

