% 1-DOF, beam helicopter simulation
%
% Thrust is proportional to RPM^2
% Measuring angle from the horizontal: 
%    lifting-thrust is proportional to cos(angle)
%
% Gravitational torque is (moment( x (angular acceleration)
% Gravitational torque, T, moment is proportional to cos(angle)
%
% Natural period of the free swinging beam/motor is about 2 Hz
% and natural w=sqrt(g/L) for a beam with a point mass
% and moment of intertial of m x (L^2) = Io
%
% ADC input 0-1023 for 340 degrees of rotation
% PWM output (1 KHZ) 0-40000 for full motor control
clear all
figure(1); clf;

Fs = 1000;
dt = 1/Fs ;
tmax = 20 ; %seconds
t = 0:dt:tmax; %seconds

% initial conditions
angle = zeros(1,length(t));
w  = zeros(1,length(t));
RPM = zeros(1,length(t));
V_control = zeros(1,length(t)); 
angle(1) = 350 ; % hanging down
% angular speed
w(1) = 0 ; % starts at rest

% command angle
desired_angle = 520 ; % hroizontal
% and intial error
error_ang = desired_angle - angle(1);

% machine constants
% voltage to thrust scale Kt
Kt = 5e-10 ;
% length of the beam
L = 0.3 ; % meter
% mass of the motor (guess)
m = 0.01 ; % kilogram
g = 9.8 ; % m/s^2 -- one g

% control loop gain
Kp = 200. ; % 
% control loop derivitive gain
Kd = 4000 ; % 
% control loop integral gain
Ki = 1/16 ; % 
integral_cntl = 0;
% motor low pass
motor_lp = 0.95 ;
% damping
DD = 5e-3 ;

for i=2:length(t)
    % === control algorithm ===
    last_error = error_ang;
    error_ang = (desired_angle - angle(i-1));
    
    proportional_cntl = Kp * error_ang ;
    differential_cntl = Kd * (error_ang - last_error); 
    integral_cntl = integral_cntl + error_ang ;
    if (sign(error_ang) ~= sign(last_error))
        integral_cntl = 0;
    end
    V_control(i) =  proportional_cntl + differential_cntl + Ki * integral_cntl ;
    %uncomment this next statement to get a passive pendulum
    %V_control(i) = 0;  
    
   % physical bound of the PWM 
   if V_control(i)<0
        V_control(i) = 0 ;
   end
   % physical bound of the PWM 
   if V_control(i)>39999
        V_control(i) = 39999 ;
   end
   
    % === physics ===
    % motor has low pass response to voltage step
    RPM(i) = (1-motor_lp)*V_control(i) + motor_lp*RPM(i-1);
    % thrust is roughly proportional to rotation speed squared
    thrust = Kt * (RPM(i)^2) ;
    % motor torque
    % convert angle in ADC units to radians
    lift_torque = thrust * L ;
    % beam gravitational torque
    beam_torque = m * g *  L * cos((angle(i-1)-520)/265) ;
    %angular acceleration
    % 265 converts radians to ADC units
    w_dot = (lift_torque - beam_torque)*265 / (m * L^2) ;
    % Euler numerical integration
    % DD is linear speed damping term
    w(i) = w(i-1)*(1-DD) + w_dot*dt ;
    % angular position
    angle(i) = angle(i-1) + w(i)*dt ;
    
    %add a weight at 10 seconds
%     if t(i) == 10
%         m = 0.015 ;
%     end
end
subplot(2,1,1)
plot(t,angle);
hold on
line([0 tmax], [desired_angle desired_angle], 'color', 'r')
ylabel('angle')

subplot(2,1,2)
plot(t, V_control)
ylabel('Control V')

