% File: pitch.m
% Date: June 11, 2006
% Author: Jason Moore
% Description: Runs various checks to verify the equations derived in
% Autolev for pitch of the frame/rider.
clear
close all
dr     = 0.6985;                          %rear wheel diameter [m]
df     = 0.6985;                          %front wheel diameter [m]
wb     = 1.0287;                          %wheelbase [m]
tr     = 0.0602;                          %trail [m]
lambda = 73*pi/180;                       %head tube angle [rad]
fo     = df/2*cos(lambda)-tr*sin(lambda); %fork offset [m]
RR     = dr/2;                            %rear wheel radius [m]
RF     = df/2;                            %front wheel radius [m]
L1     = wb-fo*sin(lambda)-((dr-df)/2+fo*cos(lambda))/tan(lambda);
L2     = ((dr-df)/2+fo*cos(lambda))/sin(lambda);
L3     = fo;
THETA  = pi/2-lambda;
%---------Check whether linearization works
Q4     = 0/180*pi; %hold roll constant
Q6     = 0/180*pi; %hold steering constant
Q8     = -90/180*pi:0.01:90/180*pi; %vary pitch
nonlinear = RR.*cos(Q4) + L1.*sin(Q8).*cos(Q4) + RF.*(-1+(sin(Q4).*cos...
            (Q6)+sin(Q6).*cos(Q4).*sin(THETA+Q8)).^2)./(1-(sin(Q4).*cos...
            (Q6)+sin(Q6).*cos(Q4).*sin(THETA+Q8)).^2).^0.5 - L2.*cos(Q4)...
            .*cos(THETA+Q8) - L3.*(sin(Q4).*sin(Q6)-cos(Q4).*cos(Q6).*...
            sin(THETA+Q8));
linear = RR.*cos(Q4) + RF.*(-1+(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*...
         cos(Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
         (Q4)).^2).^0.5 + Q8.*cos(Q4).*(L1+L2.*sin(THETA)+L3.*cos(THETA)...
         .*cos(Q6)+2.*RF.*cos(THETA).*sin(Q6).*(sin(Q4).*cos(Q6)+sin...
         (THETA).*sin(Q6).*cos(Q4))./(1-(sin(Q4).*cos(Q6)+sin(THETA).*...
         sin(Q6).*cos(Q4)).^2).^0.5+RF.*cos(THETA).*sin(Q6).*(sin(Q4).*...
         cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).*(-1+(sin(Q4).*cos(Q6)+...
         sin(THETA).*sin(Q6).*cos(Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin...
         (THETA).*sin(Q6).*cos(Q4)).^2).^1.5) - L2.*cos(THETA).*cos(Q4)...
         - L3.*(sin(Q4).*sin(Q6)-sin(THETA).*cos(Q4).*cos(Q6));
figure(1)
plot(Q8.*180./pi,nonlinear.*180./pi,'r',Q8.*180./pi,linear.*180./pi,'g')
title('Linearization Check')
xlabel('Pitch (Q8) [deg]')
ylabel('f(Q4,Q6,Q8)')
legend('nonlinear','linear',2);
%---------Check Q8 with Q6=0
Q4     = -89/180*pi:0.01:89/180*pi;
Q6     = 0/180*pi; 
autolevq8 = (L2.*cos(THETA).*cos(Q4)+L3.*(sin(Q4).*sin(Q6)-sin(THETA).*...
            cos(Q4).*cos(Q6))-RR.*cos(Q4)-RF.*(-1+(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2).^0.5)./(cos(Q4).*(L1+L2.*sin...
            (THETA)+L3.*cos(THETA).*cos(Q6)+2.*RF.*cos(THETA).*sin(Q6).*...
            (sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4))./(1-(sin(Q4)...
            .*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2).^0.5+RF.*cos...
            (THETA).*sin(Q6).*(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*...
            cos(Q4)).*(-1+(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2).^1.5));
figure(2)
subplot(221);plot(Q4.*180./pi,autolevq8.*180./pi)
axis([Q4(1).*180./pi Q4(length(Q4)).*180./pi -90 90])
title('Steering (Q6) = 0')
xlabel('Roll (Q4) [deg]')
ylabel('Pitch (Q8) [deg]')
%---------Check DOT(D1>,N3>)
Q4     = -89/180*pi:0.01:89/180*pi;
Q6     = 0/180*pi; 
d1dotn3 = cos(Q4).*sin((L2.*cos(THETA).*cos(Q4)+L3.*(sin(Q4).*sin(Q6)-...
          sin(THETA).*cos(Q4).*cos(Q6))-RR.*cos(Q4)-RF.*(-1+(sin(Q4).*...
          cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2)./(1-(sin(Q4).*cos...
          (Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2).^0.5)./(cos(Q4).*(L1+...
          L2.*sin(THETA)+L3.*cos(THETA).*cos(Q6)+2.*RF.*cos(THETA).*sin...
          (Q6).*(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4))./(1-...
          (sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2).^0.5+RF.*...
          cos(THETA).*sin(Q6).*(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*...
          cos(Q4)).*(-1+(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4))...
          .^2)./(1-(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2)...
          .^1.5)));
subplot(222);plot(Q4.*180./pi,d1dotn3.*180./pi)
axis([Q4(1).*180./pi Q4(length(Q4)).*180./pi -1 1])
title('DOT(D1>,N3>) with Steering (Q6) = 0')
xlabel('Roll (Q4) [deg]')
ylabel('DOT(D1>,N3>)')
%---------Check Q8 when Q4 = 0
Q4     = 0/180*pi;
Q6     = -89/180*pi:0.01:89/180*pi; 
autolevq8 = (L2.*cos(THETA).*cos(Q4)+L3.*(sin(Q4).*sin(Q6)-sin(THETA).*...
            cos(Q4).*cos(Q6))-RR.*cos(Q4)-RF.*(-1+(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2).^0.5)./(cos(Q4).*(L1+L2.*sin...
            (THETA)+L3.*cos(THETA).*cos(Q6)+2.*RF.*cos(THETA).*sin(Q6).*...
            (sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4))./(1-(sin(Q4)...
            .*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2).^0.5+RF.*cos...
            (THETA).*sin(Q6).*(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*...
            cos(Q4)).*(-1+(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2).^1.5));
subplot(223);plot(Q6.*180./pi,autolevq8.*180./pi)
title('Roll (Q4) = 0')
xlabel('Steering (Q6) [deg]')
ylabel('Pitch (Q8) [deg]')
%---------Set lambda = 90 deg and check Q8
lambda = 90/180*pi;
fo     = df/2*cos(lambda)-tr*sin(lambda);
RR     = dr/2;
RF     = df/2;
L1     = wb-fo*sin(lambda)-((dr-df)/2+fo*cos(lambda))/tan(lambda);
L2     = ((dr-df)/2+fo*cos(lambda))/sin(lambda);
L3     = fo;
THETA  = pi/2-lambda;
Q4     = 0/180*pi;
Q6     = -89/180*pi:0.01:89/180*pi; 
autolevq8 = (L2.*cos(THETA).*cos(Q4)+L3.*(sin(Q4).*sin(Q6)-sin(THETA).*...
            cos(Q4).*cos(Q6))-RR.*cos(Q4)-RF.*(-1+(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin...
            (THETA).*sin(Q6).*cos(Q4)).^2).^0.5)./(cos(Q4).*(L1+L2.*sin...
            (THETA)+L3.*cos(THETA).*cos(Q6)+2.*RF.*cos(THETA).*sin(Q6).*...
            (sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4))./(1-(sin(Q4)...
            .*cos(Q6)+sin(THETA).*sin(Q6).*cos(Q4)).^2).^0.5+RF.*cos...
            (THETA).*sin(Q6).*(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*...
            cos(Q4)).*(-1+(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2)./(1-(sin(Q4).*cos(Q6)+sin(THETA).*sin(Q6).*cos...
            (Q4)).^2).^1.5));
subplot(224);plot(Q6.*180./pi,autolevq8.*180./pi)
title('Roll (Q4) = 0 with THETA = 0')
xlabel('Steering (Q6) [deg]')
ylabel('Pitch (Q8) [deg]')