%% ECE 179D Segway Lab
%This file generates various filters and transfer functions for the segway
%simulink model

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  THIS FILE IS COMPLETE AS IS. YOU DO NOT NEED TO MODIFY THIS AT ALL
% (feel free to look / modify values AFTER the rest of your lab is done)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Low Pass Filter Coefficients for timer and battery control
a_b = 0.8;							% average battery value
a_d = 0.8;							% suppress velocity noise
a_r = 0.996;						% smooth reference signal
a_gc = 0.8;							% calibrate gyro offset
a_gd = 0.999;						% compensate gyro drift

T=0.001;    %sampling time of brick
Ts=0.002;   %sampling time of control task
	
%very rough 1st order sensor TF's from systemID
s=tf('s');
Ha = 15.75/(s+15.75);   %accelerometer frequency response
Hg = 28.24/(s+28.24);   %gyro frequency response
%Ha = 1;
%Hg = 1;

%% **Filtering**

%Complimentary filters
Ga = tf([1], [2,1]);    %generate accel filter
%Ga = tf([1], [.5,1]);    %generate accel filter
A=Ha*Ga;
figure
bode(A, 'r'), hold on

Gg = minreal( (1 - Ha*Ga)/(tf([1,0],[1])*Hg) ); %solve for gyro filter
B = tf([1,0],[1])*Gg;
bode(B)
legend(' Ha*Ga (accel)','Hg*Gg*s (gyro)')
title('Comp. Filters');

sum = minreal(A+B);
Gg_discr = c2d(Gg, Ts,'tustin');
Ga_discr = c2d(Ga, Ts, 'tustin');
Ga_discr;
Gg_discr;

H_a = tf(Ga_discr.num, Ga_discr.den,-1)    %AccFilt
H_g = tf(Gg_discr.num, Gg_discr.den,-1)    %GyroFilt


%Low pass filters for smoothing derivative and voltage input
%f=40;
f=10;
VF = tf([f],[1, f]);
VFd = c2d(VF, Ts, 'tustin');
VoltageFilter = tf(VFd.num, VFd.den,-1);
LowPass= VoltageFilter; %derivative filter