% Name:             Beta4.m
% Date Created:     06/06/02
% Author:           A. McClung
% Description:      Rough values for determining the rotation caused by tripod actuation
%Based on measurements from Sprawlettes and simulation values
%Revised to basic F , and latest s01.spr values [TM] 6/6/2
repeat=0;

% Defined values
pres=482;                       % [kPa]	Air pressure //1psi=6.89kPa
l=[35,45,35];                   % [mm] 	Horizontal Leg Force Moment Arm 
thetadeg=[-10,0;15,25;30,40];	
                                % [deg]	Leg Angles [TM]
m=0.355;                        % [kg]	Mass
a=47;						    % [mm]	Effective width
b=19;                           % [mm]	Effective height
bodyL=14;                       % [cm]	Body length
C=[0,0];                        % [rad/s,rad] 	Experimental Integration constants		
strideP=135;                    % [ms]	Stride Period [TM]
DC=[35;35];                     % [%]   Duty Cycle
time=3;                         % [s]   Time for simulation [TM]
freq=1000/strideP;              % [Hz]  Freq of stride period
tether=1;                       % [%]   Percentage reduction of velocity and heading due to tether

while(repeat~=2)
    % Preliminary Calculations
    theta=pi*thetadeg/90;	% [rad]	Leg angle conversion
    F=pres/79.58-3.06;      % [N] Theoretical piston force
                            % F=PA, Di=4mm, offset=3.06 to get F~3N [TM]
    FpistR=(pres/79.58)*(DC(2)*strideP/10000);
    FpistL=(pres/79.58)*(DC(1)*strideP/10000);
                            % New throretical piston force (for each side based on same side DC)
    tau=((2/3*max(DC)+1/3*min(DC))/100)*(strideP/100);
                            % [s]   Effective Force Application Length using effective DC for tripod [TM]
    % Calculation for beta (Angle about axis thru COM and normal to ground plane)
    Rbeta=[12*(l(2)*FpistL*sin(theta(2,1))-l(1)*FpistR*(sin(theta(1,2))+sin(theta(3,2))))*tau^2]/[2*m*(a^2+b^2)]+C(1)*tau+C(2);
    Lbeta=[12*(l(1)*FpistL*(sin(theta(1,1))+sin(theta(3,1)))-l(2)*FpistR*sin(theta(2,2)))*tau^2]/[2*m*(a^2+b^2)]+C(1)*tau+C(2);
    
    Rbetadeg=180*Rbeta/pi;
    Lbetadeg=180*Lbeta/pi;

    % Calculations for angular speed and acceleration
    Rbetadot=[12*(l(2)*FpistL*sin(theta(2,1))-l(1)*FpistR*(sin(theta(1,2))+sin(theta(3,2))))*tau]/[m*(a^2+b^2)]+C(1);
    Lbetadot=[12*(l(1)*FpistL*(sin(theta(1,1))+sin(theta(3,1)))-l(2)*FpistR*sin(theta(2,2)))*tau]/[m*(a^2+b^2)]+C(1);
    Rbetadotdot=[12*(l(2)*FpistL*sin(theta(2,1))-l(1)*FpistR*(sin(theta(1,2))+sin(theta(3,2))))]/[m*(a^2+b^2)];
    Lbetadotdot=[12*(l(1)*FpistL*(sin(theta(1,1))+sin(theta(3,1)))-l(2)*FpistR*sin(theta(2,2)))]/[m*(a^2+b^2)];

    %Calculations for longitudinal acceleration, velocity and position
    xR=(FpistL*sin(theta(2,1))+FpistR*sin(theta(1,2))+FpistR*sin(theta(3,2)))*tau^2/2*m+C(1)*tau+C(2);
    xL=(FpistL*sin(theta(1,1))+FpistR*sin(theta(2,2))+FpistL*sin(theta(3,1)))*tau^2/2*m+C(1)*tau+C(2);
    xRdot=(FpistL*sin(theta(2,1))+FpistR*sin(theta(1,2))+FpistR*sin(theta(3,2)))*tau/m+C(1);
    xLdot=(FpistL*sin(theta(1,1))+FpistR*sin(theta(2,2))+FpistL*sin(theta(3,1)))*tau/m+C(1);
    xRdotdot=(FpistL*sin(theta(2,1))+FpistR*sin(theta(1,2))+FpistR*sin(theta(3,2)))/m;
    xLdotdot=(FpistL*sin(theta(1,1))+FpistR*sin(theta(2,2))+FpistL*sin(theta(3,1)))/m;
    
    % Plotting the heading  (Right step then left step in each period)
    pos=zeros(floor(freq*time)+1,4);
    clear t anghead
    for i=1:freq*time
        t(i)=i/freq;
        heading=i*(Rbeta+Lbeta)*tether;
        if i~=1
            pos(i,3)=pos(i-1,1)+xLdot*tether*sin(heading+Lbeta);     % X position for L tripod
            pos(i,4)=pos(i-1,2)+xLdot*tether*cos(heading+Lbeta);     % Y position for L tripod
        end
        pos(i,1)=pos(i,3)+xRdot*tether*sin(heading+Rbeta);   % X position for R tripod
        pos(i,2)=pos(i,4)+xRdot*tether*cos(heading+Rbeta);   % Y position for R tripod
        anghead(i)=180*heading/pi;
    end
    figure; 
    subplot(2,1,1); hold on; grid on; 
    plot(pos(1:length(pos)-1,1),pos(1:length(pos)-1,2),'ro')    %Plot the result of Right tripod activation
    plot(pos(1:length(pos)-1,3),pos(1:length(pos)-1,4),'bx')    %Plot the result of Left tripod activation
    legend('Right Tripod','Left Tripod')
    xlabel('X - axis')
    ylabel('Y - axis')
    title(['Motion : LA=[' num2str(thetadeg(1,1)) ',' num2str(thetadeg(1,2)) ';' num2str(thetadeg(2,1)) ',' num2str(thetadeg(2,2)) ';' num2str(thetadeg(3,1)) ',' num2str(thetadeg(3,2)) '] DC=[' num2str(DC(1)) ',' num2str(DC(2)) '] Time=' num2str(time) 's Tether=' num2str(tether) ' (' num2str(anghead(i)/time) 'deg/s)'])
    curaxis=axis;
    maxx=max(abs(curaxis(1)), abs(curaxis(2)));
    axis([-maxx maxx curaxis(3) curaxis(4)])
    if maxx<=10
        axis([-100*xRdot*tether 100*xRdot*tether curaxis(3) curaxis(4)])
    end
    subplot(2,1,2); hold on; grid on; 
    plot(t,anghead)
    xlabel('Time(s)')
    ylabel('Heading (deg)')
    title(['Heading : Stride Period=' num2str(strideP) 'ms  Pressure=' num2str(pres/6.89) 'psi  Final Angle=' num2str(anghead(i))])
    curaxis=axis;
    maxang=max(abs(curaxis(3)), abs(curaxis(4)));
    if maxang<=90
        axis([curaxis(1) curaxis(2) -90 90])    
    end
    repeat=menu('Again?','Yes','No');
    if repeat==1
        change=0;
        while(change~=7)
        change=menu('Change parameters','Leg Angles','DC','Period','Pressure','Time','Tether','None');
        switch(change)
            case 1,
                legside=menu('Select Side','Right','Left');
                if legside==1
                    thetadeg(:,2)=input(['Current Right Side Angles (deg) : [' num2str(thetadeg(1,2)) ';' num2str(thetadeg(2,2)) ';' num2str(thetadeg(3,2)) ']\nNew Right Side Angles (deg)     : ']);
                else
                    thetadeg(:,1)=input(['Current Left Side Angles (deg) : [' num2str(thetadeg(1,1)) ';' num2str(thetadeg(2,1)) ';' num2str(thetadeg(3,1)) ']\nNew Left Side Angles (deg)     : ']);
                end
            case 2,
%               DC=input(['Current Duty Cycle (%) : ' num2str(DC) '\nNew Duty Cycle (%)     : ']);
               DCside=menu('Select Side','Right','Left');
               if DCside==1
                   DC(2)=input(['Current Right Side Duty Cycle (%) : ' num2str(DC(2)) '\nNew Right Side Duty Cycle (%)     : ']);
               else
                   DC(1)=input(['Current Left Side Duty Cycle (%) : ' num2str(DC(1)) '\nNew Left Side Duty Cycle (%)     : ']);
               end
            case 3,
               strideP=input(['Current Stride Period (ms) : ' num2str(strideP) '\nNew Stride Period (ms)     : ']);
               freq=1000/strideP;
            case 4,
                pres=input(['Current Pressure (psi) : ' num2str(pres/6.89) '\nNew Pressuse (psi)      : '])*6.89;
            case 5,
                time=input(['Current time (s) : ' num2str(time) '\nNew time (s)     : ']);
            case 6,
                tether=input(['Current Tether (%) : ' num2str(tether) '\nNew Tether     (%) : ']);
            end
        end
    end
end
closeall=menu('Close All Windows?','Yes','No');
if closeall==1
    close all
end

% Constants
clear l m a b bodyL C freq                      
% Gait variables
clear pres thetadeg strideP DC time tether            
% Intermediate variables
clear F theta FpistR FpistL tau                 
% Body performance variables
%clear Rbeta Lbeta Rbetadeg Lbetadeg Rbetadot Lbetadot Rbetadotdot Lbetadotdot 
clear xR xL xRdot xLdot xRdotdot xLdotdot
% Results variables
clear pos t heading h i anghead                 
%Plotting and configuration variables
clear curaxis maxang maxx repeat change legside DCside closeall
